Revision 26:09a541810137, committed 2017-06-07

Comitter:

Arkadi

Date:

Wed Jun 07 09:15:03 2017 +0000

Parent:

25:716a21ab5fd3

Child:

27:8e0acf4ae4fd

Commit message:

Position Control updated

Changed in this revision

--- a/main.cpp	Wed May 24 13:49:59 2017 +0000
+++ b/main.cpp	Wed Jun 07 09:15:03 2017 +0000
@@ -10,6 +10,13 @@
 */
 
 /*
+ Improvements:
+ Move all constant parameters to #define,
+ Implement control loop (other than proportional)
+ Implement Velocity command saturation with position control (implements max speed limits)
+ Implement better stop condition for position control
+*/
+/*
     Pinout:
     Nucleo STM32F401RE
     PA_5 --> led (DigitalOut)
@@ -64,6 +71,9 @@
 #define TimeoutCommand 2000 // 2 second (ms units)
 #define STEPS2ROTATION 3200.0f // Number of steps for rotation at 16 microstepping
 
+// control loop 
+
+#define PROPORTIONAL_GAIN 0.0027777f // 1*1/360; (// gain for rotations/sec
 
 /////////////
 // Objects //
@@ -107,16 +117,19 @@
 // Driver Flag status, enabled / disabled
 bool EN_Stepper_Flag=0;
 
+// flag to indicate control mode; 1 - SPD , 0 - Position
+volatile bool SpdPos_Flag=0;
+
 // Motor Speed control
-volatile float CMD_Motor_SPD[3]= {0};
-volatile float Motor_SPD[3]= {0};
+volatile float CMD_Motor_SPD[3]= {0}; // rotations / sec
+volatile float Motor_SPD[3]= {0}; // rotations / sec
 
 // Motor Position control
-volatile float CMD_Motor_Pos[3]= {0};
-volatile float Motor_Pos[3]= {0};
+volatile float CMD_Motor_Pos[3]= {0}; // command motor angle in degrees
+volatile float Motor_Pos[3]= {0}; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
 
 // variable to store motor position
-volatile int Motor_Steps[3] = {0};
+volatile int Motor_Steps[3] = {0}; // motor steps performed
 
 // timeout command time stamp
 volatile int CMDTimeStamp=0;
@@ -132,7 +145,7 @@
 void Platform_Init();
 
 // Platform Motion Set
-void Platform_Motion_Set(bool SpdPos_Flag,float Set_M1, float Set_M2, float Set_M3);
+void Platform_Motion_Set(float Set_M1, float Set_M2, float Set_M3);
 
 // Platform Motion Control
 void Platform_Motion_Control();
@@ -154,11 +167,7 @@
     //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);
-    
+    //Initializing Motor Control Components. 
     motor1 = new L6474(D2, D8, D10, dev_spi);
     motor2 = new L6474(D2, D8, D10, dev_spi);
     motor3 = new L6474(D2, D8, D10, dev_spi);
@@ -202,6 +211,14 @@
            Motor_SPD[1]=0;
            Motor_SPD[2]=0;
            EN_Stepper_Flag=0;
+           
+           // reset motor position
+           Motor_Steps[0]=0;
+           Motor_Steps[1]=0;
+           Motor_Steps[2]=0;
+           CMD_Motor_Pos[0]=0;
+           CMD_Motor_Pos[1]=0;
+           CMD_Motor_Pos[2]=0;
         } // end timeout
     }// End Main Loop
 }// End Main
@@ -246,17 +263,11 @@
         CMDTimeStamp=time_timer.read_ms();
 
         //0 - Speed Control 1 - Position Control
-        bool SpdPos_Flag=(bool)CMD_Values[0];
+        SpdPos_Flag=(bool)CMD_Values[0];
 
         // update command
-        Platform_Motion_Set(SpdPos_Flag,CMD_Values[1],CMD_Values[2],CMD_Values[3]);
+        Platform_Motion_Set(CMD_Values[1],CMD_Values[2],CMD_Values[3]);
         
-        // 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: %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/
@@ -321,10 +332,12 @@
 }// End Init shika shuka
 
 // ShikaShuka Motion Set
-void Platform_Motion_Set(bool SpdPos_Flag,float Set_M1, float Set_M2, float Set_M3)
+void Platform_Motion_Set(float Set_M1, float Set_M2, float Set_M3)
 {
     static const float MaxSPDCMD=5.0f;
-    static const float DeadZoneCMD=0.001f;
+    static const float DeadZoneCMD=0.0001f;
+    static const float MaxAngle=180.0f;
+    
     // Velocity control
     if(SpdPos_Flag) {
         // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
@@ -351,16 +364,30 @@
         CMD_Motor_SPD[1]=Set_M2;
         CMD_Motor_SPD[2]=Set_M3;
 
-    } else { // end velocity control 
-    // position control
+    } else { // end velocity control -  Start position control
     
     // calculate position based on steps:
-    
-    // implement control loop based on desired position and current position
-    
-    update velocity commands
-    
+        // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
+        if (Set_M1 > MaxAngle)  Set_M1 = MaxAngle;
+        if (Set_M1 < -MaxAngle) Set_M1 = -MaxAngle;
+
+        if (Set_M2 > MaxAngle)  Set_M2 = MaxAngle;
+        if (Set_M2 < -MaxAngle) Set_M2 = -MaxAngle;
 
+        if (Set_M3 > MaxAngle)  Set_M3 = MaxAngle;
+        if (Set_M3 < -MaxAngle) Set_M3 = -MaxAngle;
+        // enable stepper drivers
+        if (EN_Stepper_Flag==0) {
+            motor1->Enable();
+            motor2->Enable();
+            motor3->Enable();
+            EN_Stepper_Flag=1;
+        }
+        // update motor speed command
+        CMD_Motor_Pos[0]=Set_M1;
+        CMD_Motor_Pos[1]=Set_M2;
+        CMD_Motor_Pos[2]=Set_M3;
+        
     }// end position control
 }// End Platform Motion Set
 
@@ -369,13 +396,28 @@
 {
     // variable limits: (-100>SPD_M>100)
     static const float MaxSPD=0.5f; // rounds per second
-    static const float DeadZone=0.001f;
+    static const float DeadZone=0.0001f;
 
     // 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
-
+    
+    // calculate motor pos:
+    Motor_Pos[0]= (((float)Motor_Steps[0]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
+    Motor_Pos[1]= (((float)Motor_Steps[1]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
+    Motor_Pos[2]= (((float)Motor_Steps[2]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
+    
+    // position control
+    if (SpdPos_Flag == 0){
+    // implement control loop based on desired position and current position
+    // update velocity commands based on position control loop
+    CMD_Motor_SPD[0]=(CMD_Motor_Pos[0]-Motor_Pos[0]) * PROPORTIONAL_GAIN;
+    CMD_Motor_SPD[1]=(CMD_Motor_Pos[1]-Motor_Pos[1]) * PROPORTIONAL_GAIN;
+    CMD_Motor_SPD[2]=(CMD_Motor_Pos[2]-Motor_Pos[2]) * PROPORTIONAL_GAIN;
+    
+    }
+    //  update velocity commands
     // implement control loop here: (basic control with max acceleration limit)
     if(1) {
         if (abs(CMD_Motor_SPD[0]-Motor_SPD[0])> MaxACC) {
@@ -393,9 +435,7 @@
         } else {
             Motor_SPD[2]=CMD_Motor_SPD[2];
         }
-
     }
-
     // update driver frequency
     if (1) {
         // Start Moition Control
@@ -469,9 +509,11 @@
     }
 #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 */
+
+// update position
+pc.printf("POS: %d ,%d, %d\r\n" ,Motor_Steps[0],Motor_Steps[1],Motor_Steps[2]); // send motors position
     
 }// End Platform Motion Control