Revision 25:716a21ab5fd3, committed 2017-05-24

Comitter:

Arkadi

Date:

Wed May 24 13:49:59 2017 +0000

Parent:

24:7fb3a424a57f

Child:

26:09a541810137

Commit message:

Velocity control implemented with timout on command

Changed in this revision

--- a/X_NUCLEO_IHM01A1.lib	Wed May 24 10:03:21 2017 +0000
+++ b/X_NUCLEO_IHM01A1.lib	Wed May 24 13:49:59 2017 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/Arkadi/code/X_NUCLEO_IHM01A1_Disabled_Control/#9933118b7fc7
+https://developer.mbed.org/users/Arkadi/code/X_NUCLEO_IHM01A1_Disabled_Control/#e2049a15fb15

--- a/main.cpp	Wed May 24 10:03:21 2017 +0000
+++ b/main.cpp	Wed May 24 13:49:59 2017 +0000
@@ -53,7 +53,6 @@
 ///////////////
 #include "mbed.h"
 #include "BufferedSerial.h"  // solves issues of loosing data. alternative doing it yourself
-#include "FastPWM.h" // high frequency pwm with good resolution
 #include "l6474_class.h" // stepper library
 
 
@@ -61,7 +60,7 @@
 // #defines  //
 ///////////////
 #define DEBUG_MSG
-#define Motor_Control_Interval 2000 // 500Hz
+#define Motor_Control_Interval 10000 // 100Hz
 #define TimeoutCommand 2000 // 2 second (ms units)
 #define STEPS2ROTATION 3200.0f // Number of steps for rotation at 16 microstepping
 
@@ -116,6 +115,9 @@
 volatile float CMD_Motor_Pos[3]= {0};
 volatile float Motor_Pos[3]= {0};
 
+// variable to store motor position
+volatile int Motor_Steps[3] = {0};
+
 // timeout command time stamp
 volatile int CMDTimeStamp=0;
 
@@ -130,7 +132,7 @@
 void Platform_Init();
 
 // Platform Motion Set
-void Platform_Motion_Set(float Set_SPD_M1, float Set_SPD_M2, float Set_SPD_M3);
+void Platform_Motion_Set(bool SpdPos_Flag,float Set_M1, float Set_M2, float Set_M3);
 
 // Platform Motion Control
 void Platform_Motion_Control();
@@ -151,78 +153,56 @@
 {
     //Initializing SPI bus.
     DevSPI dev_spi(D11, D12, D13);
-    
+
     //Initializing Motor Control Components.
-    motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
-    motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
-    motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);  
+//    motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
+//    motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
+//    motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);
     
+    motor1 = new L6474(D2, D8, D10, dev_spi);
+    motor2 = new L6474(D2, D8, D10, dev_spi);
+    motor3 = new L6474(D2, D8, D10, dev_spi);
+
     // Setup serial
-    pc.baud(57600);
+    pc.baud(115200);
 
     // Init shika shuka
     Platform_Init();
-      
+
     // initil time timer:
     time_timer.start();
 
     ///////////////////////
     //  Main Code Loop : //
     ///////////////////////
-    while(1) { 
+    while(1) {      
         // loop time stamp
-        int Timer_TimeStamp_ms=time_timer.read_ms();  
-        static int ADC_Read_time=0;
-        
+        int Timer_TimeStamp_ms=time_timer.read_ms();
+
         // receive Motor Command
-        while (InMSG.readable()) {
-            char InChar=InMSG.getc();
-            //pc.printf("%c" ,InChar); // debug check/
+        while (pc.readable()) {
+            char InChar=pc.getc();
+            pc.printf("%c" ,InChar); // debug check/
             Parse_Message(InChar);
         }//end serial
- 
+
         // set time out on commad and stop motion
         if (abs(Timer_TimeStamp_ms-CMDTimeStamp)>TimeoutCommand){
 #ifdef DEBUG_MSG
-           // pc.printf("Timer_TimeStamp_ms %d CMDTimeStamp %d\r\n", Timer_TimeStamp_ms,CMDTimeStamp);    
+           // pc.printf("Timer_TimeStamp_ms %d CMDTimeStamp %d\r\n", Timer_TimeStamp_ms,CMDTimeStamp);
 #endif /* DEBUG_MSG */
            CMDTimeStamp=Timer_TimeStamp_ms;
-           PitchMotor->Disable();
-           RollMotor->Disable();
+           motor1->Disable();
+           motor2->Disable();
+           motor3->Disable();
            CMD_Motor_SPD[0]=0;
            CMD_Motor_SPD[1]=0;
+           CMD_Motor_SPD[2]=0;
            Motor_SPD[0]=0;
            Motor_SPD[1]=0;
+           Motor_SPD[2]=0;
            EN_Stepper_Flag=0;
-        }
-          
-        // read ADC value
-        if (abs(Timer_TimeStamp_ms-ADC_Read_time)>VccReadDelay) {
-           ADC_Read_time=Timer_TimeStamp_ms;
-           // LPF on value
-           float ReadVoltage=Vcc_11.read();
-           ReadVoltage=ReadVoltage* 3.3f*11.0f; // 1/11 voltage divider
-           
-           static float dt_VCC_Read=((float)VccReadDelay)/1000.0f;
-           static float Alpha_LPF=dt_VCC_Read/(1.0f+dt_VCC_Read) ; // α := dt / (RC + dt)
-           
-           // LPF: y[i] := y[i-1] + α * (x[i] - y[i-1])   
-           VCC_Voltage = VCC_Voltage + Alpha_LPF * (ReadVoltage - VCC_Voltage);
-           // disable motion if voltage too low
-           if (VCC_Voltage<VCC_Thresh) {
-                PitchMotor->Disable();
-                RollMotor->Disable();
-                Motor_SPD[0]=0;
-                Motor_SPD[1]=0;
-                EN_Stepper_Flag=0;
-            }
-           
-#ifdef DEBUG_MSG
-            //pc.printf("CMD: %.3f ,%.3f ,%.3f ,%.3f \r\n" ,CMD_Values[0],CMD_Values[1],CMD_Values[2],CMD_Values[3]); // debug check/
-            //pc.printf("VCC = %.2f LPF_V %.2f V \r\n", VCC_Voltage,ReadVoltage);
-            
-#endif /* DEBUG_MSG */
-        }
+        } // end timeout
     }// End Main Loop
 }// End Main
 
@@ -261,17 +241,24 @@
 
         BufferIndex=0; // initialize to start of parser
         Values_Index=0; // reset values index
-        
+
         // set time stamp on time out commad
         CMDTimeStamp=time_timer.read_ms();
+
+        //0 - Speed Control 1 - Position Control
+        bool SpdPos_Flag=(bool)CMD_Values[0];
+
         // update command
-        ShikaShuka_Motion_Set(CMD_Values[2],CMD_Values[3]);
+        Platform_Motion_Set(SpdPos_Flag,CMD_Values[1],CMD_Values[2],CMD_Values[3]);
         
-        // send out the remaining message for the brushed controller
-        OutMSG.printf("$%.3f,%.3f\r\n",CMD_Values[0],CMD_Values[1]);
-        
+        // motor test while control is developed
+        if (0){
+            Motor1_Step_Ticker.attach(&Motor1_Step_Control, (0.5f/CMD_Values[1]));
+            Motor2_Step_Ticker.attach(&Motor2_Step_Control, (0.5f/CMD_Values[2]));
+            Motor3_Step_Ticker.attach(&Motor3_Step_Control, (0.5f/CMD_Values[3]));
+        }
 #ifdef DEBUG_MSG
-        pc.printf("CMD: %.3f ,%.3f ,%.3f ,%.3f \r\n" ,CMD_Values[0],CMD_Values[1],CMD_Values[2],CMD_Values[3]); // debug check/
+        pc.printf("CMD: %d ,%.3f ,%.3f ,%.3f \r\n" ,SpdPos_Flag,CMD_Values[1],CMD_Values[2],CMD_Values[3]); // debug check/
         //pc.printf("CMD: %.3f ,%.3f \r\n" ,CMD_Values[0],CMD_Values[1]); // debug check/
         led = !led;
 #endif /* DEBUG_MSG */
@@ -290,7 +277,7 @@
         exit(EXIT_FAILURE);
     if (motor3->Init() != COMPONENT_OK)
         exit(EXIT_FAILURE);
-    
+
     /*----- Changing motor setting. -----*/
     /* Setting High Impedance State to update L6474's registers. */
     motor1->SoftHiZ();
@@ -308,18 +295,18 @@
     /* Increasing the torque regulation current. */
     motor1->SetParameter(L6474_TVAL, 1250); // Limit 2.0A
     motor2->SetParameter(L6474_TVAL, 1650); // Limit 1.7A
-    motor3->SetParameter(L6474_TVAL, 250);  // Limit 0.28A
+    motor3->SetParameter(L6474_TVAL, 300);  // Limit 0.28A
 
     /* Increasing the max threshold overcurrent. */
-    motor1->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA); 
+    motor1->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA);
     motor2->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA);
     motor3->SetParameter(L6474_OCD_TH, L6474_OCD_TH_750mA);
-    
+
     // Enabling motor
     motor1->Enable();
     motor2->Enable();
     motor3->Enable();
-      
+
     // Initialize Control Pins
     MOT1Dir.write(0);
     MOT1Step.write(0);
@@ -328,59 +315,66 @@
     MOT3Dir.write(0);
     MOT3Step.write(0);
 
-    // Start Moition Control
-    Platform_Control_Ticker.attach_us(&ShikaShuka_Motion_Control, ShikaShuka_Control_Interval); 
-    
+    // Start Moition Control  // need implementation
+    Platform_Control_Ticker.attach_us(&Platform_Motion_Control, Motor_Control_Interval);
+
 }// End Init shika shuka
-    
+
 // ShikaShuka Motion Set
-void ShikaShuka_Motion_Set(float Set_SPD_M1, float Set_SPD_M2) 
+void Platform_Motion_Set(bool SpdPos_Flag,float Set_M1, float Set_M2, float Set_M3)
 {
     static const float MaxSPDCMD=5.0f;
     static const float DeadZoneCMD=0.001f;
+    // Velocity control
+    if(SpdPos_Flag) {
+        // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
+        if (Set_M1 > MaxSPDCMD)  Set_M1 = MaxSPDCMD;
+        if (Set_M1 < -MaxSPDCMD) Set_M1 = -MaxSPDCMD;
+        if (abs(Set_M1)  < DeadZoneCMD)  Set_M1 = 0;
 
-    // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
-    if (Set_SPD_M1 > MaxSPDCMD)  Set_SPD_M1 = MaxSPDCMD;
-    if (Set_SPD_M1 < -MaxSPDCMD) Set_SPD_M1 = -MaxSPDCMD;
-    if (abs(Set_SPD_M1)  < DeadZoneCMD)  Set_SPD_M1 = 0;
-    
-    if (Set_SPD_M2 > MaxSPDCMD)  Set_SPD_M2 = MaxSPDCMD;
-    if (Set_SPD_M2 < -MaxSPDCMD) Set_SPD_M2 = -MaxSPDCMD;
-    if (abs(Set_SPD_M2)  < DeadZoneCMD)  Set_SPD_M2 = 0;
-    // verify voltage level:
-    if (VCC_Voltage>VCC_Thresh) {
+        if (Set_M2 > MaxSPDCMD)  Set_M2 = MaxSPDCMD;
+        if (Set_M2 < -MaxSPDCMD) Set_M2 = -MaxSPDCMD;
+        if (abs(Set_M2)  < DeadZoneCMD)  Set_M2 = 0;
+
+        if (Set_M3 > MaxSPDCMD)  Set_M3 = MaxSPDCMD;
+        if (Set_M3 < -MaxSPDCMD) Set_M3 = -MaxSPDCMD;
+        if (abs(Set_M3)  < DeadZoneCMD)  Set_M3 = 0;
         // enable stepper drivers
-        if (EN_Stepper_Flag==0){
-            PitchMotor->Enable();
-            RollMotor->Enable();
+        if (EN_Stepper_Flag==0) {
+            motor1->Enable();
+            motor2->Enable();
+            motor3->Enable();
             EN_Stepper_Flag=1;
         }
         // update motor speed command
-        CMD_Motor_SPD[0]=Set_SPD_M1;
-        CMD_Motor_SPD[1]=Set_SPD_M2;
-    } else {
-        // disable motion if voltage too low
-        if (EN_Stepper_Flag==1){
-            PitchMotor->Disable();
-            RollMotor->Disable();
-            Motor_SPD[0]=0;
-            Motor_SPD[1]=0;
-            EN_Stepper_Flag=0;
-        }
-        CMD_Motor_SPD[0]=0;
-        CMD_Motor_SPD[1]=0;
-    }
-}// End ShikaShuka Motion Set
+        CMD_Motor_SPD[0]=Set_M1;
+        CMD_Motor_SPD[1]=Set_M2;
+        CMD_Motor_SPD[2]=Set_M3;
 
-// ShikaShuka Motion Control
-void ShikaShuka_Motion_Control()
+    } else { // end velocity control 
+    // position control
+    
+    // calculate position based on steps:
+    
+    // implement control loop based on desired position and current position
+    
+    update velocity commands
+    
+
+    }// end position control
+}// End Platform Motion Set
+
+// Platform Motion Control
+void Platform_Motion_Control()
 {
     // variable limits: (-100>SPD_M>100)
-    static const float MaxSPD=5.0f;
+    static const float MaxSPD=0.5f; // rounds per second
     static const float DeadZone=0.001f;
-    static const float MaxACC=0.5f/(1000000/ShikaShuka_Control_Interval); //acceleration set as val/sec here for open loop it is %/sec
-    
-    static float SetMotorSPDPWM[2]= {0}; // the actual command set frequency in Hz
+
+    // update max acceleration calculation !!!!!!
+    static const float MaxACC=0.5f/(1000000/Motor_Control_Interval); //acceleration set as val/sec here for open loop it is %/sec
+
+    static float SetMotorSPD[3]= {0}; // the actual command set frequency in Hz
 
     // implement control loop here: (basic control with max acceleration limit)
     if(1) {
@@ -394,48 +388,118 @@
         } else {
             Motor_SPD[1]=CMD_Motor_SPD[1];
         }
-    } 
-    // update driver PWM frequency
+        if (abs(CMD_Motor_SPD[2]-Motor_SPD[2])> MaxACC) {
+            CMD_Motor_SPD[2]>Motor_SPD[2] ? Motor_SPD[2]+=MaxACC : Motor_SPD[2]-=MaxACC;
+        } else {
+            Motor_SPD[2]=CMD_Motor_SPD[2];
+        }
+
+    }
+
+    // update driver frequency
     if (1) {
+        // Start Moition Control
         // motor 1
+
+        // update driver direction
+        if (Motor_SPD[0]>0) {
+            MOT1Dir.write(1);
+        } else {
+            MOT1Dir.write(0);
+        }
+
+        // check if SPD is higher than minimum value
         if (abs(Motor_SPD[0])<DeadZone) {
-            SetMotorSPDPWM[0]=1;
-            PitchStep.write(0); // disable pulsing, euqal to stop
+            // disable pulsing, Set speed to zero
+            Motor1_Step_Ticker.detach();
+            SetMotorSPD[0]=0;
+
         } else {
-            // Set Pulse rate based on pulses per second  
-            SetMotorSPDPWM[0]=(abs(Motor_SPD[0])*STEPS2ROTATION); // rotation per second, to pulses with duty cycle of 0.5 
-            PitchStep.write(0.5f); // enable pulsing
+            // Set Pulse rate based on pulses per second
+            SetMotorSPD[0]=(abs(Motor_SPD[0])*STEPS2ROTATION);
+            if (SetMotorSPD[0]>STEPS2ROTATION*MaxSPD) SetMotorSPD[1]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
+            if (SetMotorSPD[0]<1) SetMotorSPD[0]=1; // make sure minimum frequency
+            Motor1_Step_Ticker.attach(&Motor1_Step_Control, (0.5f/(SetMotorSPD[0])));
         }
-        // update driver PWM based on direction
-        if (SetMotorSPDPWM[0]>STEPS2ROTATION*MaxSPD) SetMotorSPDPWM[0]=STEPS2ROTATION*MaxSPD; // make sure pwm command is trimmed
-        if (SetMotorSPDPWM[0]<1) SetMotorSPDPWM[0]=1; // make sure pwm command is trimmed
-        
-        if (Motor_SPD[0]>0) {
-            PitchDir.write(1); 
+
+        // motor 2
+
+        // update driver direction
+        if (Motor_SPD[1]>0) {
+            MOT2Dir.write(1);
         } else {
-            PitchDir.write(0);            
+            MOT2Dir.write(0);
+        }
+
+        // check if SPD is higher than minimum value
+        if (abs(Motor_SPD[1])<DeadZone) {
+            // disable pulsing, Set speed to zero
+            Motor2_Step_Ticker.detach();
+            SetMotorSPD[1]=0;
+
+        } else {
+            // Set Pulse rate based on pulses per second
+            SetMotorSPD[1]=(abs(Motor_SPD[1])*STEPS2ROTATION);
+            if (SetMotorSPD[1]>STEPS2ROTATION*MaxSPD) SetMotorSPD[1]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
+            if (SetMotorSPD[1]<1) SetMotorSPD[1]=1; // make sure minimum frequency
+            Motor2_Step_Ticker.attach(&Motor2_Step_Control, (0.5f/(SetMotorSPD[1])));
         }
-        PitchStep.period(1.0f/(SetMotorSPDPWM[0]));
-        
-        // motor 2
-        if (abs(Motor_SPD[1])<DeadZone) {
-            SetMotorSPDPWM[1]=1;
-            RollStep.write(0); // disable pulsing, euqal to stop
+
+        // Motor 3
+        // update driver direction
+        if (Motor_SPD[2]>0) {
+            MOT3Dir.write(1);
         } else {
-            // Set Pulse rate based on pulses per second  
-            SetMotorSPDPWM[1]=(abs(Motor_SPD[1])*STEPS2ROTATION); // rotation per second, to pulses with duty cycle of 0.5 
-            RollStep.write(0.5f); // enable pulsing
+            MOT3Dir.write(0);
+        }
+
+        // check if SPD is higher than minimum value
+        if (abs(Motor_SPD[2])<DeadZone) {
+            // disable pulsing, Set speed to zero
+            Motor3_Step_Ticker.detach();
+            SetMotorSPD[2]=0;
+
+        } else {
+            // Set Pulse rate based on pulses per second
+            SetMotorSPD[2]=(abs(Motor_SPD[2])*STEPS2ROTATION);
+            if (SetMotorSPD[2]>STEPS2ROTATION*MaxSPD) SetMotorSPD[2]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
+            if (SetMotorSPD[2]<1) SetMotorSPD[2]=1; // make sure minimum frequency
+            Motor3_Step_Ticker.attach(&Motor3_Step_Control, (0.5f/(SetMotorSPD[2])));
         }
-        // update driver PWM based on direction
-        if (SetMotorSPDPWM[1]>STEPS2ROTATION*MaxSPD) SetMotorSPDPWM[1]=STEPS2ROTATION*MaxSPD; // make sure pwm command is trimmed
-        if (SetMotorSPDPWM[1]<1) SetMotorSPDPWM[1]=1; // make sure pwm command is trimmed
-        
-        if (Motor_SPD[1]>0) {
-            RollDir.write(1); 
-        } else {
-            RollDir.write(0);            
-        }
-        RollStep.period(1.0f/(SetMotorSPDPWM[1]));
+    }
+#ifdef DEBUG_MSG
+        //pc.printf("SPD: %.3f ,%.3f,%.3f \r\n" ,SetMotorSPD[0],SetMotorSPD[1],SetMotorSPD[2]); // debug check/
+        //pc.printf("POS: %d ,%d, %d\r\n" ,Motor_Steps[0],Motor_Steps[1],Motor_Steps[2]); // debug check/
+        led = !led;
+#endif /* DEBUG_MSG */
+    
+}// End Platform Motion Control
+
+
+// Motor 1 Ticker step control
+void Motor1_Step_Control()
+{
+    MOT1Step=!MOT1Step;
+    if (MOT1Step) {
+         MOT1Dir ? Motor_Steps[0]++ : Motor_Steps[0]--;
     }
-    //pc.printf("CMD: %.3f ,%.3f \r\n" ,SetMotorSPDPWM[0],SetMotorSPDPWM[1]); // debug check/
-}// End ShikaShuka Motion Control
+}// end motor 1 Step control
+
+// Motor 2 Ticker step control
+void Motor2_Step_Control()
+{
+    MOT2Step=!MOT2Step;
+    if (MOT2Step) {
+         MOT2Dir ? Motor_Steps[1]++ : Motor_Steps[1]--;
+    }
+}// end motor 2 Step control
+
+// Motor 3 Ticker step control
+void Motor3_Step_Control()
+{
+    MOT3Step=!MOT3Step;
+    if (MOT3Step) {
+         MOT3Dir ? Motor_Steps[2]++ : Motor_Steps[2]--;
+    }
+}// end motor 3 Step control
+