init default whs PID

2 files changed, 39 insertions, 0 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.c
index 4a02d2127..04410cc6f 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.c
@@ -197,6 +197,24 @@ bool WHS_init_Blower()
     status |= WHS_Set_Volt_Blower_Control(0); // init with 0 volt
     ReportWithPackageFilter(WasteFilter,"------------ WHS_init_Blower (status) 0V-----------------", __FILE__,__LINE__, status, RpMessage, 0, 0);
 
+    WHS_ControlData.m_params.MAX = 200;
+    WHS_ControlData.m_params.MIN = -200;
+    WHS_ControlData.m_params.Kd = 0;
+    WHS_ControlData.m_params.Kp = 300;
+    WHS_ControlData.m_params.Ki = 600;
+    WHS_ControlData.m_params.IntegralErrorMultiplier = 100;
+    WHS_ControlData.m_params.ProportionalErrorMultiplier = 100;
+    WHS_ControlData.m_params.epsilon = 0;
+    WHS_ControlData.m_params.dt = 2;
+    //WHS_ControlData.m_ingnoreValue =  PID_Request->sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
+    WHS_ControlData.m_calculatedError = 0;
+    WHS_ControlData.m_integral = 0;
+    WHS_ControlData.m_isEnabled = true;
+    WHS_ControlData.m_isReady = true;
+    WHS_ControlData.m_mesuredParam = 0;
+    WHS_ControlData.m_preError = 0;
+    WHS_ControlData.m_SetParam = 5.0;//need to update SetParams on presegment stage
+
     return status;
 }
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
index 3b010247d..2d14f972e 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
@@ -420,12 +420,22 @@ return OK;
 }
 uint32_t Diagnostics_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag)
 {
+    MessageContainer responseContainer;
+    MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT;
 
     Report("Diagnostics_Set_Load_Arm_To_Stopper time",__FILE__,__LINE__,msec_millisecondCounter,RpMessage,0,0);
 
     //NumberOfDrierLoaderCycles=0;
     //storeLoadArmParameters();
     SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);  //set this point as the spool home
+    responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[deviceID], true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
+    responseContainer.has_continuous = true;
+    responseContainer.continuous = true;
+    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    my_free(responseContainer.data.data);
+    SendChars(container_buffer, container_size);
+    HomingToken[deviceID][0] = 0;
 
     return OK;
 }
@@ -489,8 +499,19 @@ uint32_t Diagnostics_Dryer_UnLoading(void)
 }
 uint32_t Diagnostics_Dryer_MovetoEncoderPosition_Callback(uint32_t MotorId, uint32_t ReadValue)
 {
+    MessageContainer responseContainer;
+    MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT;
+
     //Report("Diagnostics_Dryer_MovetoEncoderPosition_Callback",__FILE__,__LINE__,0,RpMessage,CallbackCounter,0);
     MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200);
+    responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[MotorId], true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
+    responseContainer.has_continuous = true;
+    responseContainer.continuous = true;
+    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    my_free(responseContainer.data.data);
+    SendChars(container_buffer, container_size);
+    HomingToken[MotorId][0] = 0;
 
     return OK;
 }