From 2f4183fed8cd530a517135f477b07d9ace49dcaf Mon Sep 17 00:00:00 2001 From: Shlomo Hecht Date: Sun, 29 Jul 2018 16:16:17 +0300 Subject: Diagnostics: motors jogging and homing, disoensers jogging and homing --- .../Embedded/Modules/AlarmHandling/Diagnostics.c | 374 -------------------- .../Embedded/Modules/AlarmHandling/Diagnostics.h | 23 -- .../Embedded/Modules/Diagnostics/Diagnostics.c | 377 +++++++++++++++++++++ .../Embedded/Modules/Diagnostics/Diagnostics.h | 34 ++ .../Modules/Diagnostics/DiagnosticsHoming.c | 376 ++++++++++++++++++++ 5 files changed, 787 insertions(+), 397 deletions(-) delete mode 100644 Software/Embedded_SW/Embedded/Modules/AlarmHandling/Diagnostics.c delete mode 100644 Software/Embedded_SW/Embedded/Modules/AlarmHandling/Diagnostics.h create mode 100644 Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c create mode 100644 Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.h create mode 100644 Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c (limited to 'Software/Embedded_SW/Embedded/Modules') diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/Diagnostics.c deleted file mode 100644 index 63624b596..000000000 --- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/Diagnostics.c +++ /dev/null @@ -1,374 +0,0 @@ -/* - * AlarmHandlingDiagnostics.c - * - * Created on: 26 june 2018 - * Author: shlomo - */ -#include "include.h" -#include "Modules/General/GeneralHardware.h" - -#include "AlarmHandling.h" -#include -#include - -#include "drivers/adc_sampling/adc.h" -#include "Control/control.h" - -#include "drivers/Motors/Motor.h" -#include "drivers/Danser_SSI/SSI_Comm.h" -#include "drivers/Heater/TemperatureSensor.h" -#include "drivers/FPGA/FPGA_SPI_Comm.h" -#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h" -#include "drivers/FPGA/FPGA.h" -#include "PMR/Hardware/HardwareDancerType.pb-c.h" -#include "modules/ids/ids_ex.h" - -#include "modules/thread/thread_ex.h" -#include -#include -#include -#include -#include -#include -#include "PMR/debugging/DebugLogCategory.pb-c.h" - - -char DiagnosticsToken[36+1] = {0}; -bool DiagnosticsActive = false; - -void SendDiagnostics(void); -uint32_t DiagnosticsStart(void); -uint32_t DiagnosticsStop(void); - -uint32_t Diagnostics_ControlTrigger(uint32_t IfIndex, uint32_t ReadValue); - - -#define DIAGNOSTICS_LIMIT 10 -int DiagnosticsIndex = 0; -int DiagnosticCollectionLimit = 3; //number of data samples to collect before sending to the host -int DiagnosticLimit = eHundredMillisecond; //frequency of data collection -//DigitalPin DigitalPinArray[1][DIAGNOSTICS_LIMIT]; -double dancer1angle[DIAGNOSTICS_LIMIT]; -double dancer2angle[DIAGNOSTICS_LIMIT]; -double dancer3angle[DIAGNOSTICS_LIMIT]; - -double feedermotorfrequency[DIAGNOSTICS_LIMIT]; -double dryermotor[DIAGNOSTICS_LIMIT]; -double pollermotor[DIAGNOSTICS_LIMIT]; -double windermotor[DIAGNOSTICS_LIMIT]; -double screwmotor[DIAGNOSTICS_LIMIT]; -double mixertemperature[DIAGNOSTICS_LIMIT]; -double headzone1temperature[DIAGNOSTICS_LIMIT]; -double headzone2temperature[DIAGNOSTICS_LIMIT]; -double headzone3temperature[DIAGNOSTICS_LIMIT]; -double headzone4temperature[DIAGNOSTICS_LIMIT]; -double headzone5temperature[DIAGNOSTICS_LIMIT]; -double headzone6temperature[DIAGNOSTICS_LIMIT]; -double dryerzone1temperature[DIAGNOSTICS_LIMIT]; -double dryerzone2temperature[DIAGNOSTICS_LIMIT]; -double dryerzone3temperature[DIAGNOSTICS_LIMIT]; - - -double dispensermotorfrequency[DIAGNOSTICS_LIMIT][MAX_SYSTEM_DISPENSERS]; -double dispenser1motorfrequency[DIAGNOSTICS_LIMIT]; -double dispenser2motorfrequency[DIAGNOSTICS_LIMIT]; -double dispenser3motorfrequency[DIAGNOSTICS_LIMIT]; -double dispenser4motorfrequency[DIAGNOSTICS_LIMIT]; -double dispenser5motorfrequency[DIAGNOSTICS_LIMIT]; -double dispenser6motorfrequency[DIAGNOSTICS_LIMIT]; -double dispenser7motorfrequency[DIAGNOSTICS_LIMIT]; -double dispenser8motorfrequency[DIAGNOSTICS_LIMIT]; - -Task_Handle Diagnostics_Task_Handle; -Mailbox_Handle DiagnosticsMsgQ = NULL; - -/******************** Functions ********************************************/ -//uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read); -//********************************************************************** -typedef enum -{ - DiagnosticsTrigger, -}DiagnosticsMessages; - -typedef struct DiagnosticsMessage{ - uint16_t messageId; - uint16_t msglen; - uint32_t tick; - uint8_t messageData[20]; -}DiagnosticsMessageStruc; - -/******************** CODE ********************************************/ -//********************************************************************** - -void DiagnosticsInit(void) -{ - Error_Block eb; - - Error_init(&eb); - - DiagnosticsMsgQ = Mailbox_create(sizeof(DiagnosticsMessageStruc), 20, NULL,&eb); - - return; -} - - -void DiagnosticCollection(void) -{ - int i; - if (DiagnosticsActive == false) - return; - dancer1angle[DiagnosticsIndex] = Control_Read_Dancer_Position(WINDER_DANCER, 0,0); - dancer2angle[DiagnosticsIndex] = Control_Read_Dancer_Position(POOLER_DANCER, 0,0); - dancer3angle[DiagnosticsIndex] = Control_Read_Dancer_Position(FEEDER_DANCER, 0,0); - - feedermotorfrequency[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING); - dryermotor[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING); - pollermotor[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDRIVING); - windermotor[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_WINDER); - screwmotor[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW); - mixertemperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_MIXCHIP_TEMP)/100; - headzone1temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP1)/100; - headzone2temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP2)/100; - headzone3temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP3)/100; - headzone4temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP4)/100; - headzone5temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP5)/100; - headzone6temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_AN_ENCLOSURETEMP3)/100; - dryerzone1temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP1)/100; - dryerzone2temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP2)/100; - dryerzone3temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP3)/100; - - for (i=0;i= DIAGNOSTICS_LIMIT) DiagnosticsIndex = DIAGNOSTICS_LIMIT; - if (DiagnosticsIndex >= DiagnosticCollectionLimit) - SendDiagnostics(); - -} -void DiagnosticsReset(void) -{ - DiagnosticsIndex = 0; - -} -DiagnosticsMonitors DiagnosticsMonitor = DIAGNOSTICS_MONITORS__INIT; -void SendDiagnostics(void) -{ - MessageContainer responseContainer; - StartDiagnosticsResponse response = START_DIAGNOSTICS_RESPONSE__INIT; -// DoubleArray DispensersMotorsFreq = DOUBLE_ARRAY__INIT; - if (DiagnosticsActive == false) - return; - - - DiagnosticsMonitor.n_dancer1angle = DiagnosticsIndex; - DiagnosticsMonitor.n_dancer2angle = DiagnosticsIndex; - DiagnosticsMonitor.n_dancer3angle = DiagnosticsIndex; - DiagnosticsMonitor.n_feedermotorfrequency = DiagnosticsIndex; - DiagnosticsMonitor.n_dryermotor = DiagnosticsIndex; - DiagnosticsMonitor.n_pollermotor = DiagnosticsIndex; - DiagnosticsMonitor.n_windermotor = DiagnosticsIndex; - DiagnosticsMonitor.n_screwmotor = DiagnosticsIndex; - DiagnosticsMonitor.n_mixertemperature = DiagnosticsIndex; - DiagnosticsMonitor.n_headzone1temperature = DiagnosticsIndex; - DiagnosticsMonitor.n_headzone2temperature = DiagnosticsIndex; - DiagnosticsMonitor.n_headzone3temperature = DiagnosticsIndex; - DiagnosticsMonitor.n_headzone4temperature = DiagnosticsIndex; - DiagnosticsMonitor.n_headzone5temperature = DiagnosticsIndex; - DiagnosticsMonitor.n_headzone6temperature = DiagnosticsIndex; - DiagnosticsMonitor.n_dryerzone1temperature = DiagnosticsIndex; - DiagnosticsMonitor.n_dryerzone2temperature = DiagnosticsIndex; - DiagnosticsMonitor.n_dryerzone3temperature = DiagnosticsIndex; - DiagnosticsMonitor.n_dispenser1motorfrequency = DiagnosticsIndex; - DiagnosticsMonitor.n_dispenser2motorfrequency = DiagnosticsIndex; - DiagnosticsMonitor.n_dispenser3motorfrequency = DiagnosticsIndex; - DiagnosticsMonitor.n_dispenser4motorfrequency = DiagnosticsIndex; - DiagnosticsMonitor.n_dispenser5motorfrequency = DiagnosticsIndex; - DiagnosticsMonitor.n_dispenser6motorfrequency = DiagnosticsIndex; - DiagnosticsMonitor.n_dispenser7motorfrequency = DiagnosticsIndex; - DiagnosticsMonitor.n_dispenser8motorfrequency = DiagnosticsIndex; - - DiagnosticsMonitor.dancer1angle = dancer1angle; - DiagnosticsMonitor.dancer2angle = dancer2angle; - DiagnosticsMonitor.dancer3angle = dancer3angle; - - DiagnosticsMonitor.feedermotorfrequency = feedermotorfrequency; - DiagnosticsMonitor.dryermotor = dryermotor; - DiagnosticsMonitor.pollermotor = pollermotor; - DiagnosticsMonitor.windermotor = windermotor; - DiagnosticsMonitor.screwmotor = screwmotor; - DiagnosticsMonitor.mixertemperature = mixertemperature; - DiagnosticsMonitor.headzone1temperature = headzone1temperature; - DiagnosticsMonitor.headzone2temperature = headzone2temperature; - DiagnosticsMonitor.headzone3temperature = headzone3temperature; - DiagnosticsMonitor.headzone4temperature = headzone4temperature; - DiagnosticsMonitor.headzone5temperature = headzone5temperature; - DiagnosticsMonitor.headzone6temperature = headzone6temperature; - DiagnosticsMonitor.dryerzone1temperature = dryerzone1temperature; - DiagnosticsMonitor.dryerzone2temperature = dryerzone2temperature; - DiagnosticsMonitor.dryerzone3temperature = dryerzone3temperature; - - //DiagnosticsMonitor.dispensersmotorsfrequency = &DispensersMotorsFreq; - //DispensersMotorsFreq.data = (double *)dispensermotorfrequency; - - DiagnosticsMonitor.dispenser1motorfrequency = dispenser1motorfrequency; - DiagnosticsMonitor.dispenser2motorfrequency = dispenser2motorfrequency; - DiagnosticsMonitor.dispenser3motorfrequency = dispenser3motorfrequency; - DiagnosticsMonitor.dispenser4motorfrequency = dispenser4motorfrequency; - DiagnosticsMonitor.dispenser5motorfrequency = dispenser5motorfrequency; - DiagnosticsMonitor.dispenser6motorfrequency = dispenser6motorfrequency; - DiagnosticsMonitor.dispenser7motorfrequency = dispenser7motorfrequency; - DiagnosticsMonitor.dispenser8motorfrequency = dispenser8motorfrequency; - - - response.monitors = &DiagnosticsMonitor; - response.n_digitalpins = 0/**MAX_GPI*/; - //response.digitalpins = DigitalPinArray; - response.n_events = 0; - response.n_componentsstates = 0; - - responseContainer = createContainer(MESSAGE_TYPE__StartDiagnosticsResponse, DiagnosticsToken, false, &response, &start_diagnostics_response__pack, &start_diagnostics_response__get_packed_size); - responseContainer.continuous = true; - - 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); - - DiagnosticsReset(); - - if (SendChars(container_buffer, container_size) == false) //comm tx mailbox full - { - DiagnosticsStop(); - } -} -uint32_t DiagnosticsControlId = 0xff; -bool DiagnosticRequestAccepted = false; -uint32_t DiagnosticsEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue) -{ - return OK; -} -uint32_t DiagnosticsStart(void) -{ - - if ( DiagnosticsActive == false) - { - if (DiagnosticRequestAccepted == true) - { - DiagnosticsActive = true; - DiagnosticsControlId = AddControlCallback(Diagnostics_ControlTrigger,DiagnosticLimit,DiagnosticsEmptyCBFunction,0,0,0); - } - } - return OK; -} -uint32_t DiagnosticsStop(void) -{ - if ( DiagnosticsActive == true) - { - DiagnosticsActive = false; - if (DiagnosticsControlId != 0xFF) - RemoveControlCallback(DiagnosticsControlId,Diagnostics_ControlTrigger); - DiagnosticsControlId = 0xFF; - } - return OK; -} -uint32_t StartDiagnosticsRequestFunc(MessageContainer* requestContainer) -{ - uint32_t status = NOT_SUPPORTED; - //MessageContainer responseContainer; - -// ReportInitParams InitParams; - //ControlStart(); - DiagnosticRequestAccepted = true; - DiagnosticsStart(); - - StartDiagnosticsRequest* request = start_diagnostics_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); - ustrncpy (DiagnosticsToken, requestContainer->token,36); - start_diagnostics_request__free_unpacked(request,NULL); - return status; -} -uint32_t StopDiagnosticsRequestFunc(MessageContainer* requestContainer) -{ - - MessageContainer responseContainer; - - StopDiagnosticsRequest* request = stop_diagnostics_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); - - StopDiagnosticsResponse response = STOP_DIAGNOSTICS_RESPONSE__INIT; - - DiagnosticsStop(); - - DiagnosticRequestAccepted = false; - - responseContainer = createContainer(MESSAGE_TYPE__StopDiagnosticsResponse, requestContainer->token, false, &response, &stop_diagnostics_response__pack, &stop_diagnostics_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 Diagnostics_ControlTrigger(uint32_t IfIndex, uint32_t ReadValue) -{ - DiagnosticsMessageStruc Message; - - //send message to the Millisec task - Message.messageId = DiagnosticsTrigger; -// Message.tick = UsersysTickGet(); - Message.msglen = sizeof(DiagnosticsMessageStruc); - if (DiagnosticsMsgQ != NULL) - Mailbox_post(DiagnosticsMsgQ , &Message, BIOS_NO_WAIT); - - - return OK; -} - - -/****************************************************************************** - * ======== messageTsk ======== - * Task for this function is created statically. See the project's .cfg file. - * this message task is created statically in system initialization, - ******************************************************************************/ -void DiagnosticsTask(UArg arg0, UArg arg1) -{ - DiagnosticsMessageStruc Message; - //char str[60]; - //uint16_t length; - //Clock_setTimeout(HostKAClock, 1000); - //Clock_start(HostKAClock); - DiagnosticsInit(); - Diagnostics_Task_Handle = Task_self(); - while(1) - { - Mailbox_pend(DiagnosticsMsgQ , &Message, BIOS_WAIT_FOREVER); - switch (Message.messageId) - { - case DiagnosticsTrigger: - DiagnosticCollection(); - break; - default: - break; - } - } -} 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/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c new file mode 100644 index 000000000..c67334625 --- /dev/null +++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c @@ -0,0 +1,377 @@ +/* + * AlarmHandlingDiagnostics.c + * + * Created on: 26 june 2018 + * Author: shlomo + */ +#include +#include "include.h" +#include "Modules/General/GeneralHardware.h" + +#include "Modules/AlarmHandling/AlarmHandling.h" +#include +#include + +#include "drivers/adc_sampling/adc.h" +#include "Control/control.h" + +#include + +#include "drivers/Motors/Motor.h" +#include "drivers/Danser_SSI/SSI_Comm.h" +#include "drivers/Heater/TemperatureSensor.h" +#include "drivers/FPGA/FPGA_SPI_Comm.h" +#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h" +#include "drivers/FPGA/FPGA.h" +#include "PMR/Hardware/HardwareDancerType.pb-c.h" +#include "modules/ids/ids_ex.h" + +#include "modules/thread/thread_ex.h" +#include +#include +#include +#include +#include +#include +#include "PMR/debugging/DebugLogCategory.pb-c.h" + + +char DiagnosticsToken[36+1] = {0}; +bool DiagnosticsActive = false; + +void SendDiagnostics(void); +uint32_t DiagnosticsStart(void); +uint32_t DiagnosticsStop(void); + +uint32_t Diagnostics_ControlTrigger(uint32_t IfIndex, uint32_t ReadValue); + + +#define DIAGNOSTICS_LIMIT 10 +int DiagnosticsIndex = 0; +int DiagnosticCollectionLimit = 3; //number of data samples to collect before sending to the host +int DiagnosticLimit = eHundredMillisecond; //frequency of data collection +//DigitalPin DigitalPinArray[1][DIAGNOSTICS_LIMIT]; +double dancer1angle[DIAGNOSTICS_LIMIT]; +double dancer2angle[DIAGNOSTICS_LIMIT]; +double dancer3angle[DIAGNOSTICS_LIMIT]; + +double feedermotorfrequency[DIAGNOSTICS_LIMIT]; +double dryermotor[DIAGNOSTICS_LIMIT]; +double pollermotor[DIAGNOSTICS_LIMIT]; +double windermotor[DIAGNOSTICS_LIMIT]; +double screwmotor[DIAGNOSTICS_LIMIT]; +double mixertemperature[DIAGNOSTICS_LIMIT]; +double headzone1temperature[DIAGNOSTICS_LIMIT]; +double headzone2temperature[DIAGNOSTICS_LIMIT]; +double headzone3temperature[DIAGNOSTICS_LIMIT]; +double headzone4temperature[DIAGNOSTICS_LIMIT]; +double headzone5temperature[DIAGNOSTICS_LIMIT]; +double headzone6temperature[DIAGNOSTICS_LIMIT]; +double dryerzone1temperature[DIAGNOSTICS_LIMIT]; +double dryerzone2temperature[DIAGNOSTICS_LIMIT]; +double dryerzone3temperature[DIAGNOSTICS_LIMIT]; + + +double dispensermotorfrequency[DIAGNOSTICS_LIMIT][MAX_SYSTEM_DISPENSERS]; +double dispenser1motorfrequency[DIAGNOSTICS_LIMIT]; +double dispenser2motorfrequency[DIAGNOSTICS_LIMIT]; +double dispenser3motorfrequency[DIAGNOSTICS_LIMIT]; +double dispenser4motorfrequency[DIAGNOSTICS_LIMIT]; +double dispenser5motorfrequency[DIAGNOSTICS_LIMIT]; +double dispenser6motorfrequency[DIAGNOSTICS_LIMIT]; +double dispenser7motorfrequency[DIAGNOSTICS_LIMIT]; +double dispenser8motorfrequency[DIAGNOSTICS_LIMIT]; + +Task_Handle Diagnostics_Task_Handle; +Mailbox_Handle DiagnosticsMsgQ = NULL; + +/******************** Functions ********************************************/ +//uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read); +//********************************************************************** +typedef enum +{ + DiagnosticsTrigger, +}DiagnosticsMessages; + +typedef struct DiagnosticsMessage{ + uint16_t messageId; + uint16_t msglen; + uint32_t tick; + uint8_t messageData[20]; +}DiagnosticsMessageStruc; + +/******************** CODE ********************************************/ +//********************************************************************** + +void DiagnosticsInit(void) +{ + Error_Block eb; + + Error_init(&eb); + + DiagnosticsMsgQ = Mailbox_create(sizeof(DiagnosticsMessageStruc), 20, NULL,&eb); + + return; +} + + +void DiagnosticCollection(void) +{ + int i; + if (DiagnosticsActive == false) + return; + dancer1angle[DiagnosticsIndex] = Control_Read_Dancer_Position(WINDER_DANCER, 0,0); + dancer2angle[DiagnosticsIndex] = Control_Read_Dancer_Position(POOLER_DANCER, 0,0); + dancer3angle[DiagnosticsIndex] = Control_Read_Dancer_Position(FEEDER_DANCER, 0,0); + + feedermotorfrequency[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING); + dryermotor[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING); + pollermotor[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDRIVING); + windermotor[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_WINDER); + screwmotor[DiagnosticsIndex] = MotorGetSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW); + mixertemperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_MIXCHIP_TEMP)/100; + headzone1temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP1)/100; + headzone2temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP2)/100; + headzone3temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP3)/100; + headzone4temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP4)/100; + headzone5temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DYEINGH_TEMP5)/100; + headzone6temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_AN_ENCLOSURETEMP3)/100; + dryerzone1temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP1)/100; + dryerzone2temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP2)/100; + dryerzone3temperature[DiagnosticsIndex] = TemperatureSensorRead(TEMP_SENSE_ANALOG_DRYER_TEMP3)/100; + + for (i=0;i= DIAGNOSTICS_LIMIT) DiagnosticsIndex = DIAGNOSTICS_LIMIT; + if (DiagnosticsIndex >= DiagnosticCollectionLimit) + SendDiagnostics(); + +} +void DiagnosticsReset(void) +{ + DiagnosticsIndex = 0; + +} +DiagnosticsMonitors DiagnosticsMonitor = DIAGNOSTICS_MONITORS__INIT; +void SendDiagnostics(void) +{ + MessageContainer responseContainer; + StartDiagnosticsResponse response = START_DIAGNOSTICS_RESPONSE__INIT; +// DoubleArray DispensersMotorsFreq = DOUBLE_ARRAY__INIT; + if (DiagnosticsActive == false) + return; + + + DiagnosticsMonitor.n_dancer1angle = DiagnosticsIndex; + DiagnosticsMonitor.n_dancer2angle = DiagnosticsIndex; + DiagnosticsMonitor.n_dancer3angle = DiagnosticsIndex; + DiagnosticsMonitor.n_feedermotorfrequency = DiagnosticsIndex; + DiagnosticsMonitor.n_dryermotor = DiagnosticsIndex; + DiagnosticsMonitor.n_pollermotor = DiagnosticsIndex; + DiagnosticsMonitor.n_windermotor = DiagnosticsIndex; + DiagnosticsMonitor.n_screwmotor = DiagnosticsIndex; + DiagnosticsMonitor.n_mixertemperature = DiagnosticsIndex; + DiagnosticsMonitor.n_headzone1temperature = DiagnosticsIndex; + DiagnosticsMonitor.n_headzone2temperature = DiagnosticsIndex; + DiagnosticsMonitor.n_headzone3temperature = DiagnosticsIndex; + DiagnosticsMonitor.n_headzone4temperature = DiagnosticsIndex; + DiagnosticsMonitor.n_headzone5temperature = DiagnosticsIndex; + DiagnosticsMonitor.n_headzone6temperature = DiagnosticsIndex; + DiagnosticsMonitor.n_dryerzone1temperature = DiagnosticsIndex; + DiagnosticsMonitor.n_dryerzone2temperature = DiagnosticsIndex; + DiagnosticsMonitor.n_dryerzone3temperature = DiagnosticsIndex; + DiagnosticsMonitor.n_dispenser1motorfrequency = DiagnosticsIndex; + DiagnosticsMonitor.n_dispenser2motorfrequency = DiagnosticsIndex; + DiagnosticsMonitor.n_dispenser3motorfrequency = DiagnosticsIndex; + DiagnosticsMonitor.n_dispenser4motorfrequency = DiagnosticsIndex; + DiagnosticsMonitor.n_dispenser5motorfrequency = DiagnosticsIndex; + DiagnosticsMonitor.n_dispenser6motorfrequency = DiagnosticsIndex; + DiagnosticsMonitor.n_dispenser7motorfrequency = DiagnosticsIndex; + DiagnosticsMonitor.n_dispenser8motorfrequency = DiagnosticsIndex; + + DiagnosticsMonitor.dancer1angle = dancer1angle; + DiagnosticsMonitor.dancer2angle = dancer2angle; + DiagnosticsMonitor.dancer3angle = dancer3angle; + + DiagnosticsMonitor.feedermotorfrequency = feedermotorfrequency; + DiagnosticsMonitor.dryermotor = dryermotor; + DiagnosticsMonitor.pollermotor = pollermotor; + DiagnosticsMonitor.windermotor = windermotor; + DiagnosticsMonitor.screwmotor = screwmotor; + DiagnosticsMonitor.mixertemperature = mixertemperature; + DiagnosticsMonitor.headzone1temperature = headzone1temperature; + DiagnosticsMonitor.headzone2temperature = headzone2temperature; + DiagnosticsMonitor.headzone3temperature = headzone3temperature; + DiagnosticsMonitor.headzone4temperature = headzone4temperature; + DiagnosticsMonitor.headzone5temperature = headzone5temperature; + DiagnosticsMonitor.headzone6temperature = headzone6temperature; + DiagnosticsMonitor.dryerzone1temperature = dryerzone1temperature; + DiagnosticsMonitor.dryerzone2temperature = dryerzone2temperature; + DiagnosticsMonitor.dryerzone3temperature = dryerzone3temperature; + + //DiagnosticsMonitor.dispensersmotorsfrequency = &DispensersMotorsFreq; + //DispensersMotorsFreq.data = (double *)dispensermotorfrequency; + + DiagnosticsMonitor.dispenser1motorfrequency = dispenser1motorfrequency; + DiagnosticsMonitor.dispenser2motorfrequency = dispenser2motorfrequency; + DiagnosticsMonitor.dispenser3motorfrequency = dispenser3motorfrequency; + DiagnosticsMonitor.dispenser4motorfrequency = dispenser4motorfrequency; + DiagnosticsMonitor.dispenser5motorfrequency = dispenser5motorfrequency; + DiagnosticsMonitor.dispenser6motorfrequency = dispenser6motorfrequency; + DiagnosticsMonitor.dispenser7motorfrequency = dispenser7motorfrequency; + DiagnosticsMonitor.dispenser8motorfrequency = dispenser8motorfrequency; + + + response.monitors = &DiagnosticsMonitor; + response.n_digitalpins = 0/**MAX_GPI*/; + //response.digitalpins = DigitalPinArray; + response.n_events = 0; + response.n_componentsstates = 0; + + responseContainer = createContainer(MESSAGE_TYPE__StartDiagnosticsResponse, DiagnosticsToken, false, &response, &start_diagnostics_response__pack, &start_diagnostics_response__get_packed_size); + responseContainer.continuous = true; + + 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); + + DiagnosticsReset(); + + if (SendChars(container_buffer, container_size) == false) //comm tx mailbox full + { + DiagnosticsStop(); + } +} +uint32_t DiagnosticsControlId = 0xff; +bool DiagnosticRequestAccepted = false; +uint32_t DiagnosticsEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue) +{ + return OK; +} +uint32_t DiagnosticsStart(void) +{ + + if ( DiagnosticsActive == false) + { + if (DiagnosticRequestAccepted == true) + { + DiagnosticsActive = true; + DiagnosticsControlId = AddControlCallback(Diagnostics_ControlTrigger,DiagnosticLimit,DiagnosticsEmptyCBFunction,0,0,0); + } + } + return OK; +} +uint32_t DiagnosticsStop(void) +{ + if ( DiagnosticsActive == true) + { + DiagnosticsActive = false; + if (DiagnosticsControlId != 0xFF) + RemoveControlCallback(DiagnosticsControlId,Diagnostics_ControlTrigger); + DiagnosticsControlId = 0xFF; + } + return OK; +} +uint32_t StartDiagnosticsRequestFunc(MessageContainer* requestContainer) +{ + uint32_t status = NOT_SUPPORTED; + //MessageContainer responseContainer; + +// ReportInitParams InitParams; + //ControlStart(); + DiagnosticRequestAccepted = true; + DiagnosticsStart(); + + StartDiagnosticsRequest* request = start_diagnostics_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); + ustrncpy (DiagnosticsToken, requestContainer->token,36); + start_diagnostics_request__free_unpacked(request,NULL); + return status; +} +uint32_t StopDiagnosticsRequestFunc(MessageContainer* requestContainer) +{ + + MessageContainer responseContainer; + + StopDiagnosticsRequest* request = stop_diagnostics_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data); + + StopDiagnosticsResponse response = STOP_DIAGNOSTICS_RESPONSE__INIT; + + DiagnosticsStop(); + + DiagnosticRequestAccepted = false; + + responseContainer = createContainer(MESSAGE_TYPE__StopDiagnosticsResponse, requestContainer->token, false, &response, &stop_diagnostics_response__pack, &stop_diagnostics_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 Diagnostics_ControlTrigger(uint32_t IfIndex, uint32_t ReadValue) +{ + DiagnosticsMessageStruc Message; + + //send message to the Millisec task + Message.messageId = DiagnosticsTrigger; +// Message.tick = UsersysTickGet(); + Message.msglen = sizeof(DiagnosticsMessageStruc); + if (DiagnosticsMsgQ != NULL) + Mailbox_post(DiagnosticsMsgQ , &Message, BIOS_NO_WAIT); + + + return OK; +} + + +/****************************************************************************** + * ======== messageTsk ======== + * Task for this function is created statically. See the project's .cfg file. + * this message task is created statically in system initialization, + ******************************************************************************/ +void DiagnosticsTask(UArg arg0, UArg arg1) +{ + DiagnosticsMessageStruc Message; + //char str[60]; + //uint16_t length; + //Clock_setTimeout(HostKAClock, 1000); + //Clock_start(HostKAClock); + DiagnosticsInit(); + Diagnostics_Task_Handle = Task_self(); + while(1) + { + Mailbox_pend(DiagnosticsMsgQ , &Message, BIOS_WAIT_FOREVER); + switch (Message.messageId) + { + case DiagnosticsTrigger: + DiagnosticCollection(); + break; + default: + break; + } + } +} 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 +#include "include.h" + +#include +#include + +#include "drivers/Motors/Motor.h" +#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h" +#include "drivers/FPGA/FPGA.h" + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#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; +} -- cgit v1.3.1