aboutsummaryrefslogtreecommitdiffstats
path: root/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
diff options
context:
space:
mode:
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c')
-rw-r--r--Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c53
1 files changed, 36 insertions, 17 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 92bfa9c92..e9cc3ddde 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -22,6 +22,7 @@
#include "drivers/Heater/TemperatureSensor.h"
#include "drivers/Heater/Heater.h"
#include "drivers/Motors/Motor.h"
+#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
#include "modules/heaters/heaters.h"
////////////////////////////////State machine operation////////////////////////////////////
@@ -36,8 +37,9 @@ HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANC
uint32_t ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF};
uint32_t SpeedControlId=0xFF;
+double DancerError[NUM_OF_DANCERS] = {0.0};
int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
-
+uint32_t JobCounter = 0;
typedef struct
{
bool m_isEnabled;
@@ -66,6 +68,8 @@ ProcessedLengthFunc ProcessedLengthFuncPtr = NULL;
void ThreadSegmentEnded(void);
void ThreadInterSegmentEnded(void);
void ThreadDistanceToSpoolEnded(void);
+
+double KeepNormalizedError = 0;
////////////////////////Slow Motor State////////////////////////////////////
//uint32_t ThreadPreSegmentState(void *JobDetails);
@@ -118,7 +122,6 @@ void ThreadUpdateProcessLength (double length, void *Funcptr)
}
double MotorSentData[1000] = {0};
uint32_t PosDif[1000] = {0};
-uint32_t tick[1000] = {0};
int MotorDataIndex = 0;
@@ -163,13 +166,12 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
totalLength+=length;
}
-#warning control disabled
+//#warning control disabled
CurrentProcessedLength+=length;
-#warning control disabled
+
PosDif[MotorDataIndex] = CurrentPosition;
//PosDif[MotorDataIndex] = positionDiff;
MotorSentData[MotorDataIndex] = length;
- tick[MotorDataIndex] = UsersysTickGet();
MotorDataIndex+=1;
if (MotorDataIndex == 999) MotorDataIndex = 0;
static int pooler_counter = 0;
@@ -177,6 +179,8 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
if (pooler_counter%10 == 0)
{
SendJobProgress(CurrentProcessedLength/CurrentRequestedLength,CurrentSegmentId,false);
+ //SendJobProgress(/*KeepNormalizedError*/MotorControlConfig[index].m_calculatedError,CurrentSegmentId,false);
+
}
if (pooler_counter>=100)
{
@@ -187,8 +191,6 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
}
if (CurrentProcessedLength>=CurrentRequestedLength )
{
- SendJobProgress(100,0,true);
- #warning handle job wit several segments!!!
// segment/intersegment/distance to spool finished
if (ProcessedLengthFuncPtr)
ProcessedLengthFuncPtr();
@@ -280,10 +282,14 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
{
DancerId = ThreadMotorIdToDancerId[index];
if (ReadValue < 10)
- return;
+ return OK;
TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
- if (index == POOLER_MOTOR) //pooler dancer is right sided: data is opposite
+ if (index == POOLER_MOTOR)
+ {
+ //pooler dancer is right sided: data is opposite
TranslatedReadValue = (-1*TranslatedReadValue);
+ JobCounter++;
+ }
//TranslatedReadValue = 0;//test
MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
MotorSamplePointer[index]++;
@@ -291,12 +297,21 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
avreageSampleValue += MotorSamples[index][i];
avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;
+ //Stop Execution if the dancer moves too much
+
+ if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond))
+ {
+ EndState(CurrentJob);
+ }
NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
MotorControlConfig[index].m_mesuredParam = NormalizedError;
+ DancerError[DancerId] = NormalizedError;
MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
&MotorControlConfig[index].m_params, &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
- if (index == FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
+ if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
+ else
+ KeepNormalizedError = NormalizedError;
calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
if (abs(calculated_speed-CurrentControlledSpeed[index])>5)
{
@@ -309,7 +324,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
pooler_counter++;
if (pooler_counter>=1000)
{
- float error_integered = MotorControlConfig[index].m_calculatedError*1000;
+ //float error_integered = MotorControlConfig[index].m_calculatedError*1000;
/*{
"HeaterGroupId": 0,
"Zone1Temp": 80,
@@ -324,7 +339,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
}*/
//HeatingTestSendResonse(0, false,true,true, MotorDriverRequest[22].Speed,MotorDriverRequest[18].Speed,MotorDriverRequest[15].Speed,MotorDriverRequest[3].Speed, "MotorSpeed");
- HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/_speed,OriginalMotorSpd_2PPS[index]/*(int)error_integered*/,calculated_speed,ReadValue, "FeederSpeed");
+ HeatingTestSendResonse(0, false,true,true, /*OriginalMotorSpd_2PPS[index]*/_speed,OriginalMotorSpd_2PPS[index]/*(int)error_integered*/,MotorControlConfig[index].m_calculatedError,ReadValue, "FeederSpeed");
pooler_counter = 0;
}
}
@@ -350,6 +365,8 @@ bool InitialProcess = false;
{
int Motor_i, HW_Motor_Id, Pid_Id;
CurrentSegmentId = 0;
+
+ JobCounter = 0;
//start thread control for all motors
for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
{
@@ -469,12 +486,13 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
//#warning rocker disabled
if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].has_directionthreadwize)
MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize);
- MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 10);
+ MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 5);
if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].has_directionthreadwize)
MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize);
- MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 10);
+ MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 5);
//#warning rocker disabled
+// MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 0, GPI_LS_RLOADMOTOR_UP, EndState); //TODO
// activate control fr all motors
//set speed for both rocker motors
@@ -486,6 +504,7 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
}
else
{
+ ThreadUpdateProcessLength (0,(void *)NULL);
PreSegmentReady(Module_Thread,ModuleDone);
InitialProcess = false;
}
@@ -527,10 +546,10 @@ uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
{
RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
}
- StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
+ MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
}
- StopMotor(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
- StopMotor(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);
+ MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
+ MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);
return OK;
}
generated by cgit v1.3.1 (git 2.54.0) at 2026-07-05 09:46:24 +0000