Diff: main.cpp

Revision:

23:1bcf834fb859

Parent:

21:ed054abddfe4

Child:

25:716a21ab5fd3

--- a/main.cpp	Mon Mar 14 09:09:02 2016 +0000
+++ b/main.cpp	Wed May 24 10:01:41 2017 +0000
@@ -1,91 +1,297 @@
-/**
- ******************************************************************************
- * @file    main.cpp
- * @author  Davide Aliprandi, STMicroelectronics
- * @version V1.0.0
- * @date    October 14th, 2015
- * @brief   mbed test application for the STMicroelectronics X-NUCLEO-IHM01A1
- *          Motor Control Expansion Board: control of 2 motors.
- ******************************************************************************
- * @attention
- *
- * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *   1. Redistributions of source code must retain the above copyright notice,
- *      this list of conditions and the following disclaimer.
- *   2. Redistributions in binary form must reproduce the above copyright notice,
- *      this list of conditions and the following disclaimer in the documentation
- *      and/or other materials provided with the distribution.
- *   3. Neither the name of STMicroelectronics nor the names of its contributors
- *      may be used to endorse or promote products derived from this software
- *      without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- ******************************************************************************
- */
+////////////////////////////////////////
+//      Rotating Platform             //
+//  Arkadiraf@gmail.com - 24/05/2017  //
+////////////////////////////////////////
+
+/*
+   Parts:
+    Nucleo STM32F401RE
+    X-NUCLEO-IHM01A1 - 3 Stepper motor controller
+*/
+
+/*
+    Pinout:
+    Nucleo STM32F401RE
+    PA_5 --> led (DigitalOut)
+
+    PC - Serial 2
+    PA_2 (Tx) --> STLINK
+    PA_3 (Rx) --> STLINK
+
+    X-NUCLEO-IHM01A1 (http://www.st.com/content/ccc/resource/technical/document/data_brief/59/ff/d0/16/94/ff/49/85/DM00122463.pdf/files/DM00122463.pdf/jcr:content/translations/en.DM00122463.pdf)
+    SPI:
+    PA_7 (D11) --> mosi
+    PA_9 (D12) --> miso
+    PA_8 (D13) --> sclk
+
+    Motor 1
+    PA_10(D2) --> flag_irq  (DigitalOut)
+    PA_9 (D8) --> Standby   (DigitalOut)
+    PA_8 (D7) --> MOT1Dir  (DigitalOut)
+    PC_7 (D9) --> MOT1Step (PWM)
+    PB_6 (D10)--> ssel      (DigitalOut)
+
+    Motor 2
+    PA_10(D2) --> flag_irq  (DigitalOut)
+    PA_9 (D8) --> Standby   (DigitalOut)
+    PB_5 (D4) --> MOT2Dir   (DigitalOut)
+    PB_3 (D3) --> MOT2Step  (PWM)
+    PB_6 (D10)--> ssel      (DigitalOut)
+
+    Motor 3
+    PA_10(D2) --> flag_irq  (DigitalOut)
+    PA_9 (D8) --> Standby   (DigitalOut)
+    PB_4 (D5) --> MOT3Dir   (DigitalOut)
+    PB_10(D6) --> MOT3Step  (PWM)
+    PB_6 (D10)--> ssel      (DigitalOut)
 
 
-/* Includes ------------------------------------------------------------------*/
-
-/* mbed specific header files. */
-#include "mbed.h"
+*/
 
-/* Helper header files. */
-#include "DevSPI.h"
-
-/* Component specific header files. */
-#include "l6474_class.h"
+///////////////
+// Libraries //
+///////////////
+#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
 
 
-/* Definitions ---------------------------------------------------------------*/
-
-/* Number of steps to move. */
-#define STEPS 3200
+///////////////
+// #defines  //
+///////////////
+#define DEBUG_MSG
+#define Motor_Control_Interval 2000 // 500Hz
+#define TimeoutCommand 2000 // 2 second (ms units)
+#define STEPS2ROTATION 3200.0f // Number of steps for rotation at 16 microstepping
 
 
-/* Variables -----------------------------------------------------------------*/
+/////////////
+// Objects //
+/////////////
+
+// X-NUCLEO-IHM01A1
+DigitalOut MOT1Dir(D7);
+DigitalOut MOT1Step(D9);
+DigitalOut MOT2Dir(D4);
+DigitalOut MOT2Step(D3);
+DigitalOut MOT3Dir(D5);
+DigitalOut MOT3Step(D6);
 
 /* Motor Control Component. */
 L6474 *motor1;
 L6474 *motor2;
 L6474 *motor3;
 
-/* Main ----------------------------------------------------------------------*/
+// Led
+DigitalOut led(LED1);
+
+// serial
+BufferedSerial pc(USBTX, USBRX);
+
+// Define Ticker for Motor Motion Control Ticker
+Ticker Platform_Control_Ticker;
+
+// Define Ticker for Steps control
+Ticker Motor1_Step_Ticker;
+Ticker Motor2_Step_Ticker;
+Ticker Motor3_Step_Ticker;
+
+// Timer for clock counter
+Timer time_timer;
+
+
+///////////////
+// variables //
+///////////////
+
+// Driver Flag status, enabled / disabled
+bool EN_Stepper_Flag=0;
+
+// Motor Speed control
+volatile float CMD_Motor_SPD[3]= {0};
+volatile float Motor_SPD[3]= {0};
 
+// Motor Position control
+volatile float CMD_Motor_Pos[3]= {0};
+volatile float Motor_Pos[3]= {0};
+
+// timeout command time stamp
+volatile int CMDTimeStamp=0;
+
+///////////////
+// Functions //
+///////////////
+
+// Incoming Message parser
+void Parse_Message(char inbyte);
+
+// Init Platform
+void Platform_Init();
+
+// Platform Motion Set
+void Platform_Motion_Set(float Set_SPD_M1, float Set_SPD_M2, float Set_SPD_M3);
+
+// Platform Motion Control
+void Platform_Motion_Control();
+
+// Motor1 Step Control
+void Motor1_Step_Control();
+
+// Motor2 Step Control
+void Motor2_Step_Control();
+
+// Motor3 Step Control
+void Motor3_Step_Control();
+
+////////////////////////
+//  Main Code Setup : //
+////////////////////////
 int main()
 {
-    /*----- Initialization. -----*/
-
-    /* Initializing SPI bus. */
+    //Initializing SPI bus.
     DevSPI dev_spi(D11, D12, D13);
-
-    /* Initializing Motor Control Components. */
+    
+    //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);
+    motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);  
+    
+    // Setup serial
+    pc.baud(57600);
+
+    // Init shika shuka
+    Platform_Init();
+      
+    // initil time timer:
+    time_timer.start();
+
+    ///////////////////////
+    //  Main Code Loop : //
+    ///////////////////////
+    while(1) { 
+        // loop time stamp
+        int Timer_TimeStamp_ms=time_timer.read_ms();  
+        static int ADC_Read_time=0;
+        
+        // receive Motor Command
+        while (InMSG.readable()) {
+            char InChar=InMSG.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);    
+#endif /* DEBUG_MSG */
+           CMDTimeStamp=Timer_TimeStamp_ms;
+           PitchMotor->Disable();
+           RollMotor->Disable();
+           CMD_Motor_SPD[0]=0;
+           CMD_Motor_SPD[1]=0;
+           Motor_SPD[0]=0;
+           Motor_SPD[1]=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 Main Loop
+}// End Main
+
+
+///////////////
+// Functions //
+///////////////
+// Incoming Message parser  Format: "$<SPD_M1>,<SPD_M2>,<SPD_YAW>,<PITCH_ANGLE>\r\n" // up to /r/n
+void Parse_Message(char inbyte)
+{
+    static const uint16_t BufferCMDSize=32;
+    static const uint16_t BufferCMD_ValuesSize=4;
+
+    static float CMD_Values[BufferCMD_ValuesSize]= {0};
+    static char BufferCMD[BufferCMDSize]= {0};
+    static uint16_t BufferIndex=0;
+    static uint16_t Values_Index=0;
+
+    BufferIndex=BufferIndex%(BufferCMDSize); // simple overflow handler
+    Values_Index=Values_Index%(BufferCMD_ValuesSize); // simple overflow handler
+
+    BufferCMD[BufferIndex]=inbyte;
+    BufferIndex++;
+
+    // parse incoming message
+    if (inbyte=='$') { // start of message
+        BufferIndex=0; // initialize to start of parser
+        Values_Index=0; // index for values position
+    } else if (inbyte==',') { // seperator char
+        CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values
+        BufferIndex=0; // initialize to start of parser
+        Values_Index++;
+    } else if(inbyte=='\r') { // end of message // parse message
+        // Update last value
+        CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values
+
+        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();
+        // update command
+        ShikaShuka_Motion_Set(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]);
+        
+#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: %.3f ,%.3f \r\n" ,CMD_Values[0],CMD_Values[1]); // debug check/
+        led = !led;
+#endif /* DEBUG_MSG */
+
+    }//end parser
+}// end parser function
+
+
+// Init shika shuka
+void Platform_Init()
+{
+    //Initializing Motor Control Components.
     if (motor1->Init() != COMPONENT_OK)
         exit(EXIT_FAILURE);
     if (motor2->Init() != COMPONENT_OK)
         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();
     motor2->SoftHiZ();
@@ -98,89 +304,138 @@
     motor1->SetStepMode(STEP_MODE_1_16);
     motor2->SetStepMode(STEP_MODE_1_16);
     motor3->SetStepMode(STEP_MODE_1_16);
-    /* Increasing the torque regulation current to 500mA. */
-    motor1->SetParameter(L6474_TVAL, 1250); // Limit 2.0A
-    motor2->SetParameter(L6474_TVAL, 1700); // Limit 1.7A
-    motor3->SetParameter(L6474_TVAL, 850);  // Limit 1.0A
-    
-    /* Max speed to 2400 step/s. */
-    motor1->SetMaxSpeed(1000);
-    motor2->SetMaxSpeed(1000);
-    motor3->SetMaxSpeed(1000);
 
-    /* Min speed to 200 step/s. */
-    motor1->SetMinSpeed(100);
-    motor2->SetMinSpeed(100);
-    motor3->SetMinSpeed(100);
-    
-    /* set accelerations */
-    motor1->SetAcceleration(500);
-    motor2->SetAcceleration(500);
-    motor3->SetAcceleration(500);
-    motor1->SetDeceleration(500);
-    motor2->SetDeceleration(500);
-    motor3->SetDeceleration(500);
+    /* 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
+
+    /* Increasing the max threshold overcurrent. */
+    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);
+    MOT2Dir.write(0);
+    MOT2Step.write(0);
+    MOT3Dir.write(0);
+    MOT3Step.write(0);
 
-    /* Printing to the console. */
-    printf("Motor Control Application Example for 3 Motors\r\n\n");
-
-    /*----- Moving. -----*/
+    // Start Moition Control
+    Platform_Control_Ticker.attach_us(&ShikaShuka_Motion_Control, ShikaShuka_Control_Interval); 
     
-    /* Moving N steps in the forward direction. */
-    motor1->Move(StepperMotor::FWD, STEPS);
-    motor2->Move(StepperMotor::FWD, STEPS);
-    motor3->Move(StepperMotor::FWD, STEPS);
-    /* Waiting while the motor is active. */
-    motor1->WaitWhileActive();
-    motor2->WaitWhileActive();
-    motor3->WaitWhileActive();
- 
-     /* Waiting 2 seconds. */
-    wait_ms(2000);
- 
+}// End Init shika shuka
     
-    /* Moving N steps in the backward direction. */
-    motor1->Move(StepperMotor::BWD, STEPS);
-    motor2->Move(StepperMotor::BWD, STEPS);
-    motor3->Move(StepperMotor::BWD, STEPS);
-    /* Waiting while the motor is active. */
-    motor1->WaitWhileActive();
-    motor2->WaitWhileActive();
-    motor3->WaitWhileActive();
+// ShikaShuka Motion Set
+void ShikaShuka_Motion_Set(float Set_SPD_M1, float Set_SPD_M2) 
+{
+    static const float MaxSPDCMD=5.0f;
+    static const float DeadZoneCMD=0.001f;
+
+    // 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;
     
-    /* Waiting 2 seconds. */
-    wait_ms(2000);
+    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) {
+        // enable stepper drivers
+        if (EN_Stepper_Flag==0){
+            PitchMotor->Enable();
+            RollMotor->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
 
-    /* Infinite Loop. */
-    while(1)
-    {
-            /*----- Moving. -----*/
+// ShikaShuka Motion Control
+void ShikaShuka_Motion_Control()
+{
+    // variable limits: (-100>SPD_M>100)
+    static const float MaxSPD=5.0f;
+    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
     
-    /* Moving N steps in the forward direction. */
-    motor1->Move(StepperMotor::FWD, STEPS);
-    motor2->Move(StepperMotor::FWD, STEPS);
-    motor3->Move(StepperMotor::FWD, STEPS);
-    /* Waiting while the motor is active. */
-    motor1->WaitWhileActive();
-    motor2->WaitWhileActive();
-    motor3->WaitWhileActive();
-    
-    /* Moving N steps in the backward direction. */
-    motor1->Move(StepperMotor::BWD, STEPS);
-    motor2->Move(StepperMotor::BWD, STEPS);
-    motor3->Move(StepperMotor::BWD, STEPS);
-    /* Waiting while the motor is active. */
-    motor1->WaitWhileActive();
-    motor2->WaitWhileActive();
-    motor3->WaitWhileActive();
-    
-    /* Waiting 2 seconds. */
-    wait_ms(2000);
+    static float SetMotorSPDPWM[2]= {0}; // the actual command set frequency in Hz
+
+    // implement control loop here: (basic control with max acceleration limit)
+    if(1) {
+        if (abs(CMD_Motor_SPD[0]-Motor_SPD[0])> MaxACC) {
+            CMD_Motor_SPD[0]>Motor_SPD[0] ? Motor_SPD[0]+=MaxACC : Motor_SPD[0]-=MaxACC;
+        } else {
+            Motor_SPD[0]=CMD_Motor_SPD[0];
+        }
+        if (abs(CMD_Motor_SPD[1]-Motor_SPD[1])> MaxACC) {
+            CMD_Motor_SPD[1]>Motor_SPD[1] ? Motor_SPD[1]+=MaxACC : Motor_SPD[1]-=MaxACC;
+        } else {
+            Motor_SPD[1]=CMD_Motor_SPD[1];
+        }
+    } 
+    // update driver PWM frequency
+    if (1) {
+        // motor 1
+        if (abs(Motor_SPD[0])<DeadZone) {
+            SetMotorSPDPWM[0]=1;
+            PitchStep.write(0); // disable pulsing, euqal to stop
+        } 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
+        }
+        // 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); 
+        } else {
+            PitchDir.write(0);            
+        }
+        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
+        } 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
+        }
+        // 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]));
     }
-}
+    //pc.printf("CMD: %.3f ,%.3f \r\n" ,SetMotorSPDPWM[0],SetMotorSPDPWM[1]); // debug check/
+}// End ShikaShuka Motion Control