Diagnostics: motors jogging and homing, disoensers jogging and homing

4 files changed, 414 insertions, 24 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/Diagnostics.h b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/Diagnostics.h
deleted file mode 100644
index 97820f8a8..000000000
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/Diagnostics.h
+++ /dev/null
@@ -1,23 +0,0 @@
-/*
- * Diagnostics.h
- *
- *  Created on: 28 June 2018
- *      Author: shlomo
- */
-
-#ifndef MODULES_ALARMHANDLING_DIAGNOSTICS_H_
-#define MODULES_ALARMHANDLING_DIAGNOSTICS_H_
-
-uint32_t StartDiagnosticsRequestFunc(MessageContainer* requestContainer);
-uint32_t StopDiagnosticsRequestFunc(MessageContainer* requestContainer);
-
-uint32_t DiagnosticsStart(void);
-uint32_t DiagnosticsStop(void);
-
-void DiagnosticsReset(void);
-void DiagnosticCollection(void);
-
-
-
-
-#endif /* MODULES_ALARMHANDLING_DIAGNOSTICS_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index 63624b596..c67334625 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -4,16 +4,19 @@
  *  Created on: 26 june 2018
  *      Author: shlomo
  */
+#include <DataDef.h>
 #include "include.h"
 #include "Modules/General/GeneralHardware.h"
 
-#include "AlarmHandling.h"
+#include "Modules/AlarmHandling/AlarmHandling.h"
 #include <driverlib/timer.h>
 #include <inc/hw_ints.h>
 
 #include "drivers/adc_sampling/adc.h"
 #include "Control/control.h"
 
+#include <Container.h>
+
 #include "drivers/Motors/Motor.h"
 #include "drivers/Danser_SSI/SSI_Comm.h"
 #include "drivers/Heater/TemperatureSensor.h"
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.h b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.h
new file mode 100644
index 000000000..a83d80df9
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.h
@@ -0,0 +1,34 @@
+/*
+ * Diagnostics.h
+ *
+ *  Created on: 28 June 2018
+ *      Author: shlomo
+ */
+
+#ifndef MODULES_DIAGNOSTICS_DIAGNOSTICS_H_
+#define MODULES_DIAGNOSTICS_DIAGNOSTICS_H_
+#include "PMR/common/MessageContainer.pb-c.h"
+
+uint32_t StartDiagnosticsRequestFunc(MessageContainer* requestContainer);
+uint32_t StopDiagnosticsRequestFunc(MessageContainer* requestContainer);
+
+uint32_t DiagnosticsStart(void);
+uint32_t DiagnosticsStop(void);
+
+void DiagnosticsReset(void);
+void DiagnosticCollection(void);
+
+uint32_t MotorHomingRequestFunc(MessageContainer* requestContainer);
+uint32_t DispenserHomingRequestFunc(MessageContainer* requestContainer);
+uint32_t MotorJoggingRequestFunc(MessageContainer* requestContainer);
+uint32_t DispenserJoggingRequestFunc(MessageContainer* requestContainer);
+
+uint32_t MotorAbortHomingRequestFunc(MessageContainer* requestContainer);
+uint32_t DispenserAbortHomingRequestFunc(MessageContainer* requestContainer);
+uint32_t MotorAbortJoggingRequestFunc(MessageContainer* requestContainer);
+uint32_t DispenserAbortJoggingRequestFunc(MessageContainer* requestContainer);
+
+
+
+
+#endif /* MODULES_DIAGNOSTICS_DIAGNOSTICS_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
new file mode 100644
index 000000000..f0a960f3c
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
@@ -0,0 +1,376 @@
+/*
+ * DiagnosticsHoming.c
+
+ *
+ *  Created on: July 29 2018
+ *      Author: shlomo
+ */
+#include <DataDef.h>
+#include "include.h"
+
+#include <inc/hw_ints.h>
+#include <Container.h>
+
+#include "drivers/Motors/Motor.h"
+#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
+#include "drivers/FPGA/FPGA.h"
+
+#include  <PMR/Diagnostics/DispenserHomingRequest.pb-c.h>
+#include  <PMR/Diagnostics/DispenserHomingResponse.pb-c.h>
+#include  <PMR/Diagnostics/DispenserAbortHomingRequest.pb-c.h>
+#include  <PMR/Diagnostics/DispenserAbortHomingResponse.pb-c.h>
+
+#include  <PMR/Diagnostics/DispenserJoggingRequest.pb-c.h>
+#include  <PMR/Diagnostics/DispenserJoggingResponse.pb-c.h>
+#include  <PMR/Diagnostics/DispenserAbortJoggingRequest.pb-c.h>
+#include  <PMR/Diagnostics/DispenserAbortJoggingResponse.pb-c.h>
+
+#include  <PMR/Diagnostics/MotorHomingRequest.pb-c.h>
+#include  <PMR/Diagnostics/MotorHomingResponse.pb-c.h>
+#include  <PMR/Diagnostics/MotorAbortHomingRequest.pb-c.h>
+#include  <PMR/Diagnostics/MotorAbortHomingResponse.pb-c.h>
+
+#include  <PMR/Diagnostics/MotorJoggingRequest.pb-c.h>
+#include  <PMR/Diagnostics/MotorJoggingResponse.pb-c.h>
+#include  <PMR/Diagnostics/MotorAbortJoggingRequest.pb-c.h>
+#include  <PMR/Diagnostics/MotorAbortJoggingResponse.pb-c.h>
+
+#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
+#include "drivers/Motors/Motor.h"
+#include "drivers/Valves/Valve.h"
+
+#include "StateMachines/Printing/PrintingSTM.h"
+
+FPGA_GPI_ENUM     Motor_Id_to_LS_Id[NUM_OF_MOTORS] = {
+      MAX_GPI, //MOTO_DH_CLEANHEAD = 0,
+      MAX_GPI, //MOTO_DH_CLEANMECH = 1,
+      MAX_GPI, //MOTO_DH_LID = 2,
+      MAX_GPI, //MOTO_DRYER_DRIVING = 3,
+      MAX_GPI, //MOTO_DRYER_LID = 4,
+      MAX_GPI, //MOTO_DRYER_LOADARM = 5,
+      GPI_LS_DISPENSER_DOWN_1, //MOTO_DISPENSER_1 = 6,
+      GPI_LS_DISPENSER_DOWN_2, //MOTO_DISPENSER_2 = 7,
+      GPI_LS_DISPENSER_DOWN_3, //MOTO_DISPENSER_3 = 8,
+      GPI_LS_DISPENSER_DOWN_4, //MOTO_DISPENSER_4 = 9,
+      GPI_LS_DISPENSER_DOWN_5, //MOTO_DISPENSER_5 = 10,
+      GPI_LS_DISPENSER_DOWN_6, //MOTO_DISPENSER_6 = 11,
+      GPI_LS_DISPENSER_DOWN_7, //MOTO_DISPENSER_7 = 12,
+      GPI_LS_DISPENSER_DOWN_8, //MOTO_DISPENSER_8 = 13,
+      GPI_LS_SCREW_RIGHT, //MOTO_SCREW = 14,
+      MAX_GPI, //MOTO_WINDER = 15,
+      GPI_LS_LDANCER1_DOWN, //MOTO_LDANCER1 = 16,
+      GPI_LS_LDANCER2_DOWN, //MOTO_LDANCER2 = 17,
+      MAX_GPI, //MOTO_LDRIVING = 18,
+      GPI_LS_LLOADMOTOR_UP, //MOTO_LLOADING = 19,
+      GPI_LS_LPIVOT_UP, //MOTO_LPIVOT1 = 20,
+      GPI_LS_RDANCER_DOWN, //MOTO_RDANCER = 21,
+      MAX_GPI, //MOTO_RDRIVING = 22,
+      GPI_LS_RLOADRAM_UP, //MOTO_RLOADARM = 23,
+      GPI_LS_RLOADMOTOR_DOWN, //MOTO_RLOADING = 24,
+      MAX_GPI, //MOTO_SPARE1_1 = 25,
+      MAX_GPI, //MOTO_SPARE1_2 = 26,
+      MAX_GPI, //MOTO_SPARE2_1 = 27,
+      MAX_GPI, //MOTO_SPARE2_2 = 28
+
+};
+char HomingToken[NUM_OF_MOTORS][36+1]={0};
+
+
+uint32_t MotorHomingRequestCallback(uint32_t deviceID, uint32_t ReadValue)
+{
+    MessageContainer responseContainer;
+    MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT;
+
+    responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[deviceID], false, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
+    responseContainer.continuous = false;
+    HomingToken[deviceID][0] = 0;
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    SendChars(container_buffer, container_size);
+
+return OK;
+
+}
+uint32_t DispenserHomingRequestCallback(uint32_t deviceID, uint32_t ReadValue)
+{
+    MessageContainer responseContainer;
+    MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT;
+
+    //open dry air valve in the dispenser
+    Valve_Set((Valves_t) (deviceID+VALVE_2W_MID_AIR_LF), Atm_MidTank_OFF);
+
+    responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[deviceID], false, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
+    responseContainer.continuous = false;
+    HomingToken[deviceID][0] = 0;
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    SendChars(container_buffer, container_size);
+
+return OK;
+
+}
+
+uint32_t MotorHomingRequestFunc(MessageContainer* requestContainer)
+{
+
+    MessageContainer responseContainer;
+
+    MotorHomingRequest* request = motor_homing_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT;
+
+    TimerMotors_t MotorId = (TimerMotors_t)request->code;
+
+    if ((MotorId)&&(MotorId <= NUM_OF_MOTORS))
+    {
+        if (JobIsActive() == false)
+        {
+            if ( Motor_Id_to_LS_Id[MotorId] != MAX_GPI)
+            {
+                MotorMovetoLimitSwitch (MotorId,1-MotorsCfg[MotorId].directionthreadwize, request->speed,  Motor_Id_to_LS_Id[MotorId], MotorHomingRequestCallback);
+                ustrncpy (HomingToken[MotorId], requestContainer->token,36);
+                return OK;
+            }
+        }
+    }
+    responseContainer.error = ERROR_CODE__INVALID_PROCESS_ID;
+    responseContainer.errormessage = "JOb Active or incorrect motor ID";
+    responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse,  requestContainer->token, false, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
+    responseContainer.continuous = false;
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    SendChars(container_buffer, container_size);
+
+return OK;
+}
+uint32_t DispenserHomingRequestFunc(MessageContainer* requestContainer)
+{
+
+    MessageContainer responseContainer;
+
+    DispenserHomingRequest* request = dispenser_homing_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    DispenserHomingResponse response = DISPENSER_HOMING_RESPONSE__INIT;
+
+    TimerMotors_t MotorId = (request->code)+MOTO_DISPENSER_1;
+
+    if ((MotorId)&&(MotorId <= NUM_OF_MOTORS))
+    {
+        if (JobIsActive() == false)
+        {
+            if ( Motor_Id_to_LS_Id[MotorId] != MAX_GPI)
+            {
+                //open dispenser valve dispenser to midtank direction
+                Valve_Set((Valves_t) request->code, MidTank_Dispenser);
+                //open dry air valve in the dispenser
+                Valve_Set((Valves_t) (request->code+VALVE_2W_MID_AIR_LF), Atm_MidTank_ON);
+
+
+                MotorMovetoLimitSwitch (MotorId,1-MotorsCfg[MotorId].directionthreadwize, request->speed,  Motor_Id_to_LS_Id[MotorId], DispenserHomingRequestCallback);
+                ustrncpy (HomingToken[MotorId], requestContainer->token,36);
+                return OK;
+            }
+        }
+    }
+
+    responseContainer = createContainer(MESSAGE_TYPE__DispenserHomingResponse,  requestContainer->token, false, &response, &dispenser_homing_response__pack, &dispenser_homing_response__get_packed_size);
+    responseContainer.continuous = false;
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    //USBCDCD_sendData(container_buffer, container_size,10);
+    SendChars(container_buffer, container_size);
+    //free (container_buffer);
+
+return OK;
+}
+uint32_t MotorJoggingRequestFunc(MessageContainer* requestContainer)
+{
+    uint32_t status = OK;
+
+    MessageContainer responseContainer;
+
+    MotorJoggingRequest* request = motor_jogging_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    MotorJoggingResponse response = MOTOR_JOGGING_RESPONSE__INIT;
+
+    TimerMotors_t MotorId = (request->code);
+
+    if ((MotorId)&&(MotorId <= NUM_OF_MOTORS))
+    {
+        if (JobIsActive() == false)
+        {
+            if ((request->has_direction)&&(request->has_speed))
+            {
+                MotorSetDirection(MotorId,request->direction);
+                MotorSetSpeed(MotorId, request->speed);
+            }
+            else
+            {
+                status = ERROR;
+            }
+        }
+        else
+        {
+            status = ERROR;
+        }
+    }
+    else
+    {
+        status = ERROR;
+    }
+
+    responseContainer = createContainer(MESSAGE_TYPE__MotorJoggingResponse,  requestContainer->token, false, &response, &motor_jogging_response__pack, &motor_jogging_response__get_packed_size);
+    if (status!= OK)
+    {
+        responseContainer.error = ERROR_CODE__INVALID_PROCESS_ID;
+        responseContainer.errormessage = "JOb Active or incorrect parameters";
+    }
+    responseContainer.continuous = false;
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    SendChars(container_buffer, container_size);
+
+    return OK;
+}
+uint32_t DispenserJoggingRequestFunc(MessageContainer* requestContainer)
+{
+    uint32_t status = OK;
+
+    MessageContainer responseContainer;
+
+    DispenserJoggingRequest* request = dispenser_jogging_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    DispenserJoggingResponse response = DISPENSER_JOGGING_RESPONSE__INIT;
+
+    TimerMotors_t MotorId = (request->code)+MOTO_DISPENSER_1;
+
+    if ((MotorId)&&(MotorId <= NUM_OF_MOTORS))
+    {
+        if (JobIsActive() == false)
+        {
+            if ((request->has_direction)&&(request->has_speed))
+            {
+                MotorSetDirection(MotorId,request->direction);
+                MotorSetSpeed(MotorId, request->speed);
+            }
+            else
+            {
+                status = ERROR;
+            }
+        }
+        else
+        {
+            status = ERROR;
+        }
+    }
+    else
+    {
+        status = ERROR;
+    }
+
+    responseContainer = createContainer(MESSAGE_TYPE__DispenserJoggingResponse,  requestContainer->token, false, &response, &dispenser_jogging_response__pack, &dispenser_jogging_response__get_packed_size);
+    if (status!= OK)
+    {
+        responseContainer.error = ERROR_CODE__INVALID_PROCESS_ID;
+        responseContainer.errormessage = "JOb Active or incorrect parameters";
+    }
+    responseContainer.continuous = false;
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    SendChars(container_buffer, container_size);
+
+    return OK;
+}
+
+uint32_t MotorAbortHomingRequestFunc(MessageContainer* requestContainer)
+{
+
+    MessageContainer responseContainer;
+
+    MotorAbortHomingRequest* request = motor_abort_homing_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    MotorAbortHomingResponse response = MOTOR_ABORT_HOMING_RESPONSE__INIT;
+
+    TimerMotors_t MotorId = (request->code);
+    MotorAbortMovetoLimitSwitch(MotorId);
+
+    responseContainer = createContainer(MESSAGE_TYPE__MotorAbortHomingResponse,  requestContainer->token, false, &response, &motor_abort_homing_response__pack, &motor_abort_homing_response__get_packed_size);
+    responseContainer.continuous = false;
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    SendChars(container_buffer, container_size);
+
+return OK;
+}
+uint32_t DispenserAbortHomingRequestFunc(MessageContainer* requestContainer)
+{
+
+    MessageContainer responseContainer;
+
+    DispenserAbortHomingRequest* request = dispenser_abort_homing_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    DispenserAbortHomingResponse response = DISPENSER_ABORT_HOMING_RESPONSE__INIT;
+
+    TimerMotors_t MotorId = (request->code)+MOTO_DISPENSER_1;
+    MotorAbortMovetoLimitSwitch(MotorId);
+
+    responseContainer = createContainer(MESSAGE_TYPE__DispenserAbortHomingResponse,  requestContainer->token, false, &response, &dispenser_abort_homing_response__pack, &dispenser_abort_homing_response__get_packed_size);
+    responseContainer.continuous = false;
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    //USBCDCD_sendData(container_buffer, container_size,10);
+    SendChars(container_buffer, container_size);
+    //free (container_buffer);
+
+return OK;
+}
+uint32_t MotorAbortJoggingRequestFunc(MessageContainer* requestContainer)
+{
+
+    MessageContainer responseContainer;
+
+    MotorAbortJoggingRequest* request = motor_abort_jogging_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    MotorAbortJoggingResponse response = MOTOR_ABORT_JOGGING_RESPONSE__INIT;
+
+    TimerMotors_t MotorId = (request->code);
+    MotorStop(MotorId,Hard_Hiz);
+
+    responseContainer = createContainer(MESSAGE_TYPE__MotorAbortJoggingResponse,  requestContainer->token, false, &response, &motor_abort_jogging_response__pack, &motor_abort_jogging_response__get_packed_size);
+    responseContainer.continuous = false;
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    SendChars(container_buffer, container_size);
+
+    return OK;
+}
+uint32_t DispenserAbortJoggingRequestFunc(MessageContainer* requestContainer)
+{
+
+    MessageContainer responseContainer;
+
+    DispenserAbortJoggingRequest* request = dispenser_abort_jogging_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
+
+    DispenserAbortJoggingResponse response = DISPENSER_ABORT_JOGGING_RESPONSE__INIT;
+
+    TimerMotors_t MotorId = (request->code)+MOTO_DISPENSER_1;
+    MotorStop(MotorId,Hard_Hiz);
+
+    responseContainer = createContainer(MESSAGE_TYPE__DispenserAbortJoggingResponse,  requestContainer->token, false, &response, &dispenser_abort_jogging_response__pack, &dispenser_abort_jogging_response__get_packed_size);
+    responseContainer.continuous = false;
+    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
+    size_t container_size = message_container__pack(&responseContainer, container_buffer);
+    free(responseContainer.data.data);
+    SendChars(container_buffer, container_size);
+
+    return OK;
+}