IDS: Set muicrostep at presegment, set speed from job data

5 files changed, 44 insertions, 5 deletions

diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
index 4ee88833f..9fc17fa37 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.c
@@ -583,7 +583,31 @@ void FPGA_SetMotMaxSpeed(TimerMotors_t _motorId)//Mov
         speed_calc = 0x3FF;
     temp |= speed_calc<<8;
 
-    MillisecWriteToMotor(_motorId, temp, 4, FPGA_MotorConfig_callback);
+    MillisecWriteToMotor(_motorId, temp, 4, NULL);
+}
+
+
+void FPGA_SetMotMicroStep(TimerMotors_t _motorId)//Mov
+{
+    char MultiStep[8] = {1,2,4,8,16,32,64,128};
+    char i;
+    char good = 0xFF;
+    uint32_t temp;
+    for (i = 0; i < 8; i++)
+    {
+        if (MotorsCfg[_motorId].microstep == MultiStep[i])
+        {
+            good = i;
+            break;
+        }
+    }
+    if(good != 0xFF)
+    {
+        temp = x_SET_PARAM | x_STEP_MODE;
+        temp = temp << 24;
+        temp |= (!x_SYNC_EN | good | x_SYNC_SEL_1)<<16;
+        MillisecWriteToMotor(_motorId, temp, 4, NULL);
+    }
 }
 
 void FPGA_SetMotPosition(TimerMotors_t _motorId)//Mov
diff --git a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
index ac126042a..046d3e5d9 100644
--- a/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
+++ b/Software/Embedded_SW/Embedded/Drivers/FPGA/FPGA_SPI_Comm.h
@@ -21,6 +21,7 @@ void FPGA_SetMotStop(TimerMotors_t _motorId);
 void FPGA_GetMotMicroSteps_Cmd(TimerMotors_t _motorId);
 void FPGA_GetMotPosition_Cmd(TimerMotors_t _motorId);
 void FPGA_SetMotMaxSpeed(TimerMotors_t _motorId);
+void FPGA_SetMotMicroStep(TimerMotors_t _motorId);
 void FPGA_SetMotPosition(TimerMotors_t _motorId);
 void FPGA_SetMotSpeed(TimerMotors_t _motorId);
 void FPGA_SetMotorsInit();
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
index 761e2e935..9dce895b9 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.c
@@ -319,6 +319,13 @@ uint32_t MotorSetMaxSpeed(TimerMotors_t _motorId, uint32_t MaxSpeed)
     return OK;
 }
 
+uint32_t MotorSetMicroStep(TimerMotors_t _motorId, uint32_t microstep)
+{
+    MotorsCfg[_motorId].microstep = microstep;
+    FPGA_SetMotMicroStep(_motorId);
+
+    return OK;
+}
 
 uint32_t MotorMove(TimerMotors_t _motorId,bool direction, uint32_t Steps)
 {
diff --git a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
index b7e11d21d..e7ebbf0da 100644
--- a/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
+++ b/Software/Embedded_SW/Embedded/Drivers/Motors/Motor.h
@@ -124,6 +124,8 @@ uint32_t MotorSetSpeed(TimerMotors_t _motorId, uint32_t _freq);
 
 uint32_t MotorSetMaxSpeed(TimerMotors_t _motorId, uint32_t MaxSpeed);
 
+uint32_t MotorSetMicroStep(TimerMotors_t _motorId, uint32_t microstep);
+
 uint32_t MotorGetSpeed(TimerMotors_t _motorId);
 
 uint32_t MotorGetPosition(TimerMotors_t _motorId);
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index 1c9323254..f4ddc0fbd 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -158,7 +158,9 @@ uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId)
     int Dispenser_i;
     TimerMotors_t HW_Motor_Id;
     int segmentfirst_speed;
-int CurrentSegment = 0;
+    int CurrentSegment = 0;
+
+    JobBrushStopId = 0;
 
     // activate control fr all motors
     /* wait for all dispensers  to get to the required pressure
@@ -168,6 +170,10 @@ int CurrentSegment = 0;
     {
         HW_Motor_Id = DispenserIdToMotorId[Dispenser_i];
         MotorStop(HW_Motor_Id,Hard_Stop);
+        if (JobTicket->segments[SegmentId]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->has_dispenserstepdivision)
+            MotorSetMicroStep(HW_Motor_Id,JobTicket->segments[SegmentId]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->dispenserstepdivision);
+        else
+            MotorSetMicroStep(HW_Motor_Id,MotorsCfg[HW_Motor_Id].microstep);
     }
     //call the job state machine when the thread system is ready
     PreSegmentReady(Module_IDS,ModuleDone);
@@ -181,16 +187,15 @@ int CurrentSegment = 0;
      JobTicket* JobTicket = JobDetails;
      int Dispenser_i;
      TimerMotors_t HW_Motor_Id;
-     int segmentfirst_speed;
+     double 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;
+         segmentfirst_speed = JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->nanolitterpersecond/JobTicket->segments[CurrentSegment]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->nanoliterperpulse;
 //         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;