diff options
Diffstat (limited to 'Software/Embedded_SW/Embedded/Modules/Control')
3 files changed, 82 insertions, 39 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c index 10286853e..4f78d2134 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c @@ -83,6 +83,8 @@ bool watchdogCriticalAlarm = false; uint32_t msec_millisecondCounter = 0; +extern bool Machine_Idle_Mode; + MillisecMotorDataStruc ScrewSetMaxSpeedPending = {0}; MillisecMotorDataStruc ScrewMovePending = {0}; MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0}; @@ -446,7 +448,8 @@ uint32_t MillisecLowLoop(uint32_t tick) //Speed_Data = Calculate_Speed_Sensor_Velocity(); //MillisecReadFromTempSensor(Sensor_Read, NULL); //if (Sensor_Read++ >= MAX_TEMPERATURE_SENSOR_ID) Sensor_Read = 0; - //Control_LED1_PWM(); + if(Machine_Idle_Mode == true) + Machine_Idle_Breathing_Led(); } if (Hundred_msTick) { diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c index f9aee7929..a443b4785 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c @@ -71,3 +71,51 @@ float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_ return output; } +float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral) +{ + float error; + float derivative; + float output; + + //double error = *mySetpoint - input; + error = _setPoint - _mesuredParam; + //ITerm+= (ki * error); + *_integral = *_integral + (error*params->Ki); + /* if(ITerm > outMax) + { + ITerm= outMax; + } + else if(ITerm < outMin) + { + ITerm= outMin; + } */ + if(*_integral > params->MAX) + { + *_integral = params->MAX; + } + else if(*_integral < params->MIN) + { + *_integral = params->MIN; + } + // double dInput = (input - lastInput); + derivative = error - *_pre_error; + + /*Compute PID Output*/ + // double output = kp * error + ITerm- kd * dInput; + output = params->Kp*error/params->ProportionalErrorMultiplier + *_integral/params->IntegralErrorMultiplier + params->Kd*derivative; + + //Saturation Filter + if(output > params->MAX) + { + output = params->MAX; + } + else if(output < params->MIN) + { + output = params->MIN; + } + + //Update error + *_pre_error = error; + + return output; +} diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c index 538e251ef..69820e306 100644 --- a/Software/Embedded_SW/Embedded/Modules/Control/control.c +++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c @@ -51,6 +51,7 @@ #include <inc/hw_ints.h> #include "drivers/adc_sampling/adc.h" +#include "Modules/General/buttons.h" #include "control.h" #include "MillisecTask.h" @@ -98,7 +99,7 @@ uint32_t ControlDatalog[MAX_TANGO_CONTROL_DEVICES]; uint16_t ControlBacklog[MAX_BACKLOG_SIZE]={0}; uint16_t backlogindex = 0; uint32_t Control_timerBase = TIMER0_BASE; //Timer handle -uint32_t MaxHighDevices = 0xFF; +uint32_t MaxHighDevices = 0; /******************** Functions ********************************************/ void OneMilliSecondFunction(UArg arg0); @@ -124,7 +125,7 @@ void ControlInit(void) ControlRestart = false; memset(ControlDatalog,0,sizeof(uint32_t)*MAX_TANGO_CONTROL_DEVICES); - MaxHighDevices = 0xFF; + MaxHighDevices = 0; for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES; Device_i++) { ControlArray[Device_i].ControlActive = false; @@ -157,10 +158,20 @@ void ControlStop(void) uint32_t ControlActivityLed( uint32_t Parameter1) { static bool flag = false; + static uint8_t counter; + const uint8_t Blink_Freq = 15;//odd number + if (flag==true) { COMM_RED_LED_ON; ACTIVITY_RED_LED_OFF; // Heaters indication - all the Heaters OFF + if(power.color == fastBILNK) + Pannel_Leds(POWER_ON_OFF,MODE_OFF); + + if((power.color == BLINK) && (counter % Blink_Freq == 0) ) + { + Pannel_Leds(POWER_ON_OFF,MODE_OFF); + } flag = false; } else @@ -168,8 +179,22 @@ uint32_t ControlActivityLed( uint32_t Parameter1) COMM_RED_LED_OFF; if (HeaterActive > 0)// Blink the led on heating ACTIVITY_RED_LED_ON;// Heaters indication - at least one of the Heaters is ON + + if(power.color == fastBILNK) + Pannel_Leds(POWER_ON_OFF,MODE_ON); + + if((power.color == BLINK) && (counter % Blink_Freq == 0) ) + { + Pannel_Leds(POWER_ON_OFF,MODE_ON); + } + flag = true; } + + if (counter < 0xFF) + counter++; + else + counter = Blink_Freq + 1; return OK; } uint32_t ControlEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue) @@ -218,13 +243,8 @@ uint32_t AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlF break; } } -/* if (MaxHighDevices == 0xFF) - MaxHighDevices = deviceId; - else - { - if ((deviceId!=0xFF )&&(deviceId> MaxHighDevices)) - MaxHighDevices = deviceId; - } + /* if ((deviceId!=0xFF )&&(deviceId> MaxHighDevices)) + // MaxHighDevices = deviceId; } else { @@ -339,23 +359,7 @@ uint32_t GetControlLowDevice_i(void) } uint32_t ControlLoop(uint32_t tick) { - //call all modules control functions - //test dancers and speed encoders - //check all callback units (state machine waiting for completion of a change) - //uint32_t ControlDevice_i; - /*bool Ten_msTick, Hundred_msTick, Onesecond_Tick,Tick98,Tick998; - Ten_msTick = (tick%eTenMillisecond == 0) ?true:false; - Hundred_msTick = (tick%eHundredMillisecond == 0) ?true:false; -// Hundred_msTick = (tick%200 == 0) ?true:false; - Onesecond_Tick = (tick%eOneSecond == 0) ?true:false; - Tick98 = (tick%eHundredMillisecond == 98) ?true:false; -// Tick98 = (tick%200 == 199) ?true:false; - Tick998 = (tick%eOneSecond == 996) ?true:false; -*/ - //ROM_IntMasterDisable(); - //if (MaxHighDevices == 0xFF) - // return OK; - //for (ControlDevice_i = 0; ControlDevice_i <= MaxHighDevices;ControlDevice_i++) + //for (ControlDevice_i = 0; ControlDevice_i < MaxHighDevices;ControlDevice_i++) for (ControlDevice_i = 0; ControlDevice_i < MAX_TANGO_CONTROL_DEVICES;ControlDevice_i++) { if (ControlArray[ControlDevice_i].ControlActive) @@ -386,18 +390,6 @@ uint32_t ControlLoop(uint32_t tick) } uint32_t ControlLowLoop(uint32_t tick) { - //call all modules control functions - //test dancers and speed encoders - //check all callback units (state machine waiting for completion of a change) - //uint32_t Device_i; - //bool Ten_msTick, Hundred_msTick, Onesecond_Tick,Tick98,Tick998; - //Ten_msTick = (tick%eTenMillisecond == 0) ?true:false; - //Hundred_msTick = (tick%eHundredMillisecond == 0) ?true:false; - //Onesecond_Tick = (tick%eOneSecond == 0) ?true:false; - //Tick98 = (tick%eHundredMillisecond == 98) ?true:false; - //Tick998 = (tick%eOneSecond == 996) ?true:false; - - //ROM_IntMasterDisable(); for (ControlLowDevice_i = 0; ControlLowDevice_i < MAX_TANGO_CONTROL_DEVICES;ControlLowDevice_i++) { if (ControlArray[ControlLowDevice_i].ControlActive) |