Merge branch 'master' of https://twinetfs.visualstudio.com/_git/Tango

3 files changed, 94 insertions, 1 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticActions.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticActions.c
index 6d575e17b..f8f467d63 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticActions.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticActions.c
@@ -16,6 +16,10 @@
 
 #include  <PMR/Diagnostics/SetDigitalOutRequest.pb-c.h>
 #include  <PMR/Diagnostics/SetDigitalOutResponse.pb-c.h>
+#include  <PMR/Diagnostics/SetValveStateRequest.pb-c.h>
+#include  <PMR/Diagnostics/SetValveStateResponse.pb-c.h>
+#include  <PMR/Diagnostics/ValveType.pb-c.h>
+#include  <PMR/Diagnostics/ValveStateCode.pb-c.h>
 
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 #include "drivers/I2C_Communication/DAC/blower.h"
@@ -190,6 +194,94 @@ uint32_t SetDigitalOutRequestRequestFunc(MessageContainer* requestContainer)
 
     return OK;
 }
+/********************************************************************************
+ *Valve Setting
+ ********************************************************************************/
+uint32_t SetValveStateRequestFunc(MessageContainer* requestContainer)
+{
+    MessageContainer responseContainer;
+    uint8_t* container_buffer;
+    uint32_t status = OK;
+    uint32_t valve;
+    SetValveStateRequest* request = set_valve_state_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+
+
+    SetValveStateResponse response = SET_VALVE_STATE_RESPONSE__INIT;
+
+    responseContainer = createContainer(MESSAGE_TYPE__SetValveStateResponse, requestContainer->token, true, &response, &set_valve_state_response__pack, &set_valve_state_response__get_packed_size);
+    container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
+
+
+    if (request->valvetype <= VALVE_TYPE__DispenserValve8 )  //dispenser 3 way valves
+    {
+        valve = request->valvetype;
+        if (request->state == VALVE_STATE_CODE__MidTank)
+        {
+            Control3WayValvesWithCallback(request->valvetype,MidTank_Dispenser,NULL);
+        }
+        else if (request->state == VALVE_STATE_CODE__VGroove)
+        {
+            Control3WayValvesWithCallback(request->valvetype,Dispenser_Mixer,NULL);
+        }
+        else
+            status = ERROR;
+    }
+    else if (request->valvetype <= VALVE_TYPE__MidTankInkValve8 ) //midtank toward dispenser
+    {
+        valve = IDS_Id_to_CartrideValve[request->valvetype - VALVE_TYPE__MidTankInkValve1];
+        if (request->state == VALVE_STATE_CODE__Open)
+        {
+            Valve_Set(valve,Cartridge_MidTank_ON);
+        }
+        else if (request->state == VALVE_STATE_CODE__Close)
+        {
+            Valve_Set(valve,Cartridge_MidTank_OFF);
+        }
+        else
+            status = ERROR;
+
+    }
+    else if (request->valvetype <= VALVE_TYPE__MidTankAirValve8 )//midtank toward air
+    {
+        valve = IDS_Id_to_AirValve[request->valvetype - VALVE_TYPE__MidTankAirValve1];
+        if (request->state == VALVE_STATE_CODE__Open)
+        {
+            Valve_Set(valve,Atm_MidTank_ON);
+        }
+        else if (request->state == VALVE_STATE_CODE__Close)
+        {
+            Valve_Set(valve,Atm_MidTank_OFF);
+        }
+        else
+            status = ERROR;
+    }
+    else if (request->valvetype <= VALVE_TYPE__CartridgeWasteValve ) //not supported
+    {
+        status = FAILED;
+    }
+    else if (request->valvetype <= VALVE_TYPE__CartridgeInkValve8 ) //cartridge - not supported yet
+    {
+        status = FAILED;
+    }
+    else
+    {
+        status = FAILED;
+    }
+
+    if (status == FAILED)
+    {
+        status = ERROR;
+        responseContainer.has_error = true;
+        responseContainer.error = ERROR_CODE__GENERAL_ERROR;
+    }
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    my_free(responseContainer.data.data);
+    SendChars(container_buffer, container_size);
+    set_valve_state_request__free_unpacked(request,NULL);
+
+    return status;
+}
 
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.h b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.h
index 91d531972..711faa7d5 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.h
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.h
@@ -36,6 +36,7 @@ uint32_t DispenserAbortJoggingRequestFunc(MessageContainer* requestContainer);
 
 uint32_t SetComponentValueRequestRequestFunc(MessageContainer* requestContainer);
 uint32_t SetDigitalOutRequestRequestFunc(MessageContainer* requestContainer);
+uint32_t SetValveStateRequestFunc(MessageContainer* requestContainer);
 
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index dc9cfcd6d..2fac46a3c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -459,7 +459,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
         MotorControlConfig[index].m_mesuredParam = NormalizedError;
         DancerError[DancerId] = NormalizedError;
-        MotorControlConfig[index].m_calculatedError = /*Advanced*/PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
+        MotorControlConfig[index].m_calculatedError = AdvancedPIDAlgorithmCalculation((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
         {