Diff: main.cpp

Revision:

27:8e0acf4ae4fd

Parent:

26:09a541810137

Child:

28:32001ee473e0

--- a/main.cpp	Wed Jun 07 09:15:03 2017 +0000
+++ b/main.cpp	Wed Jun 07 15:05:01 2017 +0000
@@ -71,7 +71,7 @@
 #define TimeoutCommand 2000 // 2 second (ms units)
 #define STEPS2ROTATION 3200.0f // Number of steps for rotation at 16 microstepping
 
-// control loop 
+// control loop
 
 #define PROPORTIONAL_GAIN 0.0027777f // 1*1/360; (// gain for rotations/sec
 
@@ -113,6 +113,8 @@
 ///////////////
 // variables //
 ///////////////
+// Send pos update
+bool Pos_Update_Flag=0;
 
 // Driver Flag status, enabled / disabled
 bool EN_Stepper_Flag=0;
@@ -167,13 +169,13 @@
     //Initializing SPI bus.
     DevSPI dev_spi(D11, D12, D13);
 
-    //Initializing Motor Control Components. 
+    //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);
 
     // Setup serial
-    pc.baud(115200);
+    pc.baud(256000);
 
     // Init shika shuka
     Platform_Init();
@@ -184,7 +186,7 @@
     ///////////////////////
     //  Main Code Loop : //
     ///////////////////////
-    while(1) {      
+    while(1) {
         // loop time stamp
         int Timer_TimeStamp_ms=time_timer.read_ms();
 
@@ -196,30 +198,37 @@
         }//end serial
 
         // set time out on commad and stop motion
-        if (abs(Timer_TimeStamp_ms-CMDTimeStamp)>TimeoutCommand){
+        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;
-           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;
-           
-           // 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;
+            CMDTimeStamp=Timer_TimeStamp_ms;
+            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;
+
+            // 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
+        if (Pos_Update_Flag) {
+            Pos_Update_Flag=0;
+            int Temp_TimeStamp_ms=time_timer.read_ms();
+            static uint32_t MSG_Counter=0;
+            MSG_Counter++;
+            pc.printf("POS:%d,%d,%d,%d,%d\r\n" ,Temp_TimeStamp_ms,MSG_Counter,Motor_Steps[0],Motor_Steps[1],Motor_Steps[2]); // send motors position
+        }// end position update
     }// End Main Loop
 }// End Main
 
@@ -267,7 +276,7 @@
 
         // update command
         Platform_Motion_Set(CMD_Values[1],CMD_Values[2],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/
@@ -337,7 +346,7 @@
     static const float MaxSPDCMD=5.0f;
     static const float DeadZoneCMD=0.0001f;
     static const float MaxAngle=180.0f;
-    
+
     // Velocity control
     if(SpdPos_Flag) {
         // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
@@ -365,8 +374,8 @@
         CMD_Motor_SPD[2]=Set_M3;
 
     } else { // end velocity control -  Start position control
-    
-    // calculate position based on steps:
+
+        // calculate position based on steps:
         // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
         if (Set_M1 > MaxAngle)  Set_M1 = MaxAngle;
         if (Set_M1 < -MaxAngle) Set_M1 = -MaxAngle;
@@ -387,7 +396,7 @@
         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
 
@@ -402,20 +411,20 @@
     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;
-    
+    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)
@@ -508,13 +517,11 @@
         }
     }
 #ifdef DEBUG_MSG
-        //pc.printf("SPD: %.3f ,%.3f,%.3f \r\n" ,SetMotorSPD[0],SetMotorSPD[1],SetMotorSPD[2]); // debug check/
-        led = !led;
+    //pc.printf("SPD: %.3f ,%.3f,%.3f \r\n" ,SetMotorSPD[0],SetMotorSPD[1],SetMotorSPD[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
-    
+    Pos_Update_Flag=1;
 }// End Platform Motion Control
 
 
@@ -523,7 +530,7 @@
 {
     MOT1Step=!MOT1Step;
     if (MOT1Step) {
-         MOT1Dir ? Motor_Steps[0]++ : Motor_Steps[0]--;
+        MOT1Dir ? Motor_Steps[0]++ : Motor_Steps[0]--;
     }
 }// end motor 1 Step control
 
@@ -532,7 +539,7 @@
 {
     MOT2Step=!MOT2Step;
     if (MOT2Step) {
-         MOT2Dir ? Motor_Steps[1]++ : Motor_Steps[1]--;
+        MOT2Dir ? Motor_Steps[1]++ : Motor_Steps[1]--;
     }
 }// end motor 2 Step control
 
@@ -541,7 +548,7 @@
 {
     MOT3Step=!MOT3Step;
     if (MOT3Step) {
-         MOT3Dir ? Motor_Steps[2]++ : Motor_Steps[2]--;
+        MOT3Dir ? Motor_Steps[2]++ : Motor_Steps[2]--;
     }
 }// end motor 3 Step control