MotorStop, improve IDS, Dispensers pressure

8 files changed, 62 insertions, 24 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index a1de29b08..312f970d6 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -31,6 +31,7 @@
 #include "drivers/FPGA/FPGA.h"
 
 #include "modules/thread/thread_ex.h"
+#include "modules/ids/ids_ex.h"
 
 Task_Handle Millisecond_Task_Handle;
 /******************** Definitions  ********************************************/
@@ -134,7 +135,7 @@ void MillisecStart(void)
     ADCAcquireStart(0,1);
 }
 
-
+uint32_t msec_millisecondCounter = 0;
 void OneMilliSecondMillisecInterrupt(UArg arg0)
 {
     MillisecMessageStruc Message;
@@ -148,7 +149,7 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
 
     //send message to the Millisec task
     Message.messageId = OneMillisec;
-    Message.tick = UsersysTickGet();
+    Message.tick = msec_millisecondCounter++;;
     Message.msglen = sizeof(MillisecMessageStruc);
     if (MillisecMsgQ != NULL)
         Mailbox_post(MillisecMsgQ , &Message, BIOS_NO_WAIT);
@@ -226,9 +227,10 @@ uint32_t MillisecLoop(uint32_t tick)
     //call all modules Millisec functions
     //test dancers and speed encoders
     //check all callback units (state machine waiting for completion of a change)
-    bool Ten_msTick, Hundred_msTick, Onesecond_Tick;
+    bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick;
     Ten_msTick      =   (tick%eTenMilliSecond == 0)      ?true:false;
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
+    O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
     //gather Motor data from FPGA
 #ifndef EVALUATION_BOARD
@@ -342,10 +344,15 @@ uint32_t MillisecLoop(uint32_t tick)
             MillisecReadFromTempSensor(Sensor_i, NULL);
     }
 
-    if (Onesecond_Tick)
+    if (O900Millisecond_Tick)
     {
         ADC_TriggerCollection();
     }
+    if (Onesecond_Tick)
+    {
+        for (Sensor_i = 0;Sensor_i < MAX_SYSTEM_DISPENSERS;Sensor_i++)
+            CalculateDispenserPressure(Sensor_i);
+    }
 
     /*        OLD CODE
     MotorStatus_Data[MOTOR_DRYER_DRIVING] = MotorGetStatusFromFPGA(MOTOR_DRYER_DRIVING);
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h b/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h
index 2c3910140..c3499454d 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS.h
@@ -6,6 +6,7 @@
 #include "ids_ex.h"
 
 extern uint32_t DispenserIdToMotorId[MAX_SYSTEM_DISPENSERS];
+extern float DispenserPressure[MAX_SYSTEM_DISPENSERS];
 uint32_t DispenserConfigMessage(HardwareDispenser * request);
 
 extern HardwareDispenser DispensersCfg[ MAX_SYSTEM_DISPENSERS];
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
index feb38000f..bc9e11749 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
@@ -17,5 +17,6 @@ uint32_t IDSSegmentState(void *JobDetails, int SegmentId);
 
 uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request);
 
+float CalculateDispenserPressure (int DispenserId);
 
 #endif /* MODULES_IDS_IDS_EX_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c
index d53ab3d60..81efa5662 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_init.c
@@ -12,6 +12,7 @@
 #include "PMR/Printing/JobSpoolType.pb-c.h"
 
 #include "drivers/Motors/Motor.h"
+#include "drivers/ADC_Sampling/adc.h"
 
 #define MAX_CONTROL_SAMPLES 10
 
@@ -19,6 +20,22 @@ HardwareDispenser DispensersCfg[ MAX_SYSTEM_DISPENSERS];
 
 uint32_t DispenserIdToMotorId[MAX_SYSTEM_DISPENSERS] = {HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_2,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_3,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_4,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_5,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_6,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_7,HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8};
 
+float DispenserPressure[MAX_SYSTEM_DISPENSERS] = {0};
+
+float CalculateDispenserPressure (int DispenserId)
+{
+    unsigned short data = 0;
+    float temp= 0.0;
+
+    data = ADC_GetReading(DispenserIdToPressureSensorId[DispenserId]);
+    temp = 4096 - data;
+    temp *= 3;
+    temp *= 1000; //move to mv
+    temp /= 4096;
+    DispenserPressure[DispenserId] = 3000 - temp;
+    return( DispenserPressure[DispenserId]);
+
+}
 
 
 uint32_t DispenserConfigMessage(HardwareDispenser * request)
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index abd9794b8..1c9323254 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -159,24 +159,16 @@ uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId)
     TimerMotors_t HW_Motor_Id;
     int segmentfirst_speed;
 int CurrentSegment = 0;
-    JobBrushStopId = 0;
-
-    for (Dispenser_i = 0;Dispenser_i <= MAX_SYSTEM_DISPENSERS;Dispenser_i++)
-    {
-        HW_Motor_Id = DispenserIdToMotorId[Dispenser_i];
-        //(Speed*uStep*PPR)/((2*PI*Dispenser_Radius)
-        segmentfirst_speed = JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->pulsepersecond;
-        double Dispenser_speed = (segmentfirst_speed *  MotorsCfg[HW_Motor_Id].pulseperround)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius);
-        //DispenserControlConfig[Dispenser_i].m_SetParam = Dispenser_speed;
-        OriginalDispenserSpd_2PPS[Dispenser_i] = (int)Dispenser_speed;
-    }
-    //ControlStart();
-    MotorSetSpeed(HW_Motor_Id, OriginalDispenserSpd_2PPS[HW_Motor_Id]);
 
     // activate control fr all motors
     /* wait for all dispensers  to get to the required pressure
      * move the presegment ready when all dispensers are ready.
      */
+    for (Dispenser_i = 0;Dispenser_i <= MAX_SYSTEM_DISPENSERS;Dispenser_i++)
+    {
+        HW_Motor_Id = DispenserIdToMotorId[Dispenser_i];
+        MotorStop(HW_Motor_Id,Hard_Stop);
+    }
     //call the job state machine when the thread system is ready
     PreSegmentReady(Module_IDS,ModuleDone);
 
@@ -186,6 +178,24 @@ int CurrentSegment = 0;
 //********************************************************************************************************************
  uint32_t IDSSegmentState(void *JobDetails, int SegmentId)
 {
+     JobTicket* JobTicket = JobDetails;
+     int Dispenser_i;
+     TimerMotors_t HW_Motor_Id;
+     int segmentfirst_speed;
+     int CurrentSegment = 0;
+
+     JobBrushStopId = 0;
+
+     for (Dispenser_i = 0;Dispenser_i <= MAX_SYSTEM_DISPENSERS;Dispenser_i++)
+     {
+         HW_Motor_Id = DispenserIdToMotorId[Dispenser_i];
+         //(Speed*uStep*PPR)/((2*PI*Dispenser_Radius)
+         segmentfirst_speed = JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->pulsepersecond;
+//         double Dispenser_speed = (segmentfirst_speed *  MotorsCfg[HW_Motor_Id].pulseperround)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius);
+         //DispenserControlConfig[Dispenser_i].m_SetParam = Dispenser_speed;
+//         OriginalDispenserSpd_2PPS[Dispenser_i] = (int)Dispenser_speed;
+         MotorSetSpeed(HW_Motor_Id, segmentfirst_speed);
+     }
     return OK;
 }
 
@@ -195,7 +205,7 @@ int CurrentSegment = 0;
      int Motor_i;
      for ( Motor_i = 0;Motor_i < MAX_SYSTEM_DISPENSERS;Motor_i++)
      {
-        StopMotor(DispenserIdToMotorId[Motor_i],Hard_Hiz);
+        MotorStop(DispenserIdToMotorId[Motor_i],Hard_Hiz);
      }
 
     return OK;
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
index 01c91c72b..cef0e1270 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
@@ -294,7 +294,7 @@ void Stub_MotorStopRequest(MessageContainer* requestContainer)
     Mot_Stop();
 #else
     TimerMotors_t Motor_Id  = (TimerMotors_t)request->motor_id; //to remove warning
-    StopMotor(Motor_Id, Stop_Command);
+    MotorStop(Motor_Id, Stop_Command);
 #endif
 	
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index f80f26e53..e99d3df0b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -183,14 +183,14 @@ uint32_t Winder_End(void)
     //stop screw
     ScrewNumberOfSteps = 0;
     RemoveControlCallback(ScrewControlId,ScrewDirectionChange);
-    return StopMotor (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
+    return MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
 }
 void Winder_ScrewHomeLimitSwitchInterrupt(void)
 {
     uint32_t status;
     if  (Winder_ScrewHoming)
     {
-        StopMotor(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Stop); //stop ASAP
+        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Stop); //stop ASAP
     }
     status = MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 7b5da0e65..e9cc3ddde 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -37,6 +37,7 @@ HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANC
 uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF};
 uint32_t    SpeedControlId=0xFF;
 
+double DancerError[NUM_OF_DANCERS] = {0.0};
 int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
 uint32_t JobCounter = 0;
 typedef struct
@@ -304,6 +305,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 = PIDAlgorithmCalculation((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
@@ -544,10 +546,10 @@ uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
          {
              RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
          }
-        StopMotor(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
+        MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
      }
-     StopMotor(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
-     StopMotor(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);
+     MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
+     MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);
 
     return OK;
 }