main.cpp

00001 ////////////////////////////////////////
00002 // Control Stepper motor using matlab //
00003 // Arkadiraf@gmail.com - 24/1/2016    //
00004 ////////////////////////////////////////
00005 /*
00006    Parts:
00007     Nucleo STM32F401RE
00008     X-Nucleo-IHM01A1 Stepper Driver shield     
00009     
00010 */
00011 
00012 // Libraries:
00013 #include "mbed.h"
00014 #include "BufferedSerial.h"  // solves issues of loosing data. alternative doing it yourself
00015 #include "l6474_class.h" // stepper library
00016 
00017 /////////////
00018 // Objects //
00019 ////////////
00020 
00021 // create serial object
00022 //Serial pc(SERIAL_TX, SERIAL_RX);
00023 BufferedSerial pc(USBTX, USBRX);
00024 
00025 // Pinouts:
00026 DigitalOut myled(LED1);
00027 
00028 // Stepper Pins
00029 DigitalOut StepDir(D7);
00030 DigitalOut StepPWM(D9);
00031 
00032 // Define Stepper Ticker
00033 Ticker StepperTicker;
00034 
00035 // Define Ticker
00036 Ticker SensorSampleTicker;
00037 
00038 // define millis ticker
00039 volatile uint32_t MillisValue = 0;
00040 Ticker MillisTicket;
00041 
00042 ///////////////
00043 // variables //
00044 ///////////////
00045 char BufferCMD[64]={0};
00046 uint16_t BufferIndex=0;
00047 volatile bool Sensor_Sample_Flag=0;
00048 #define Sample_Interval 5000 // interval in us, (5ms, 200Hz)
00049 float CMDValue=0; // CMD at which speed to run
00050 char inbyte=' ';
00051 
00052 // Stepper Variables:
00053 #define STEPS2ROTATION 3200.0f // 200 steps rotation 16 microsteps
00054 /* Initialization parameters. */
00055 // not sure if it acctually uses it
00056 L6474_InitTypeDef init =
00057 {
00058     50,                              /* Acceleration rate in step/s2. Range: (0..+inf). */
00059     50,                              /* Deceleration rate in step/s2. Range: (0..+inf). */
00060     3500,                             /* Maximum speed in step/s. Range: (30..10000]. */
00061     30,                              /* Minimum speed in step/s. Range: [30..10000). */
00062     500,                              /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */
00063     L6474_OCD_TH_1500mA,              /* Overcurrent threshold (OCD_TH register). */
00064     L6474_CONFIG_OC_SD_ENABLE,        /* Overcurrent shutwdown (OC_SD field of CONFIG register). */
00065     L6474_CONFIG_EN_TQREG_TVAL_USED,  /* Torque regulation method (EN_TQREG field of CONFIG register). */
00066     L6474_STEP_SEL_1_16,              /* Step selection (STEP_SEL field of STEP_MODE register). */
00067     L6474_SYNC_SEL_1_2,               /* Sync selection (SYNC_SEL field of STEP_MODE register). */
00068     L6474_FAST_STEP_12us,             /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */
00069     L6474_TOFF_FAST_8us,              /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */
00070     3,                                /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */
00071     21,                               /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */
00072     L6474_CONFIG_TOFF_044us,          /* Target Swicthing Period (field TOFF of CONFIG register). */
00073     L6474_CONFIG_SR_320V_us,          /* Slew rate (POW_SR field of CONFIG register). */
00074     L6474_CONFIG_INT_16MHZ,           /* Clock setting (OSC_CLK_SEL field of CONFIG register). */
00075     L6474_ALARM_EN_OVERCURRENT |
00076     L6474_ALARM_EN_THERMAL_SHUTDOWN |
00077     L6474_ALARM_EN_THERMAL_WARNING |
00078     L6474_ALARM_EN_UNDERVOLTAGE |
00079     L6474_ALARM_EN_SW_TURN_ON |
00080     L6474_ALARM_EN_WRONG_NPERF_CMD    /* Alarm (ALARM_EN register). */
00081 };
00082 
00083 
00084 /* Motor Control Component. */
00085 L6474 *StepperMotor;
00086 
00087 // variable to store stepper position
00088 volatile int32_t StepperPos=0;
00089 
00090 
00091 ////////////////
00092 // Functions: //
00093 ////////////////
00094 // Ticker function for stepper speed control, not very eficient.
00095 void Stepper_PWM() {
00096     StepPWM=!StepPWM;
00097     if (StepPWM==1){ //If change to high, step performed Update Stepper Position
00098      if (StepDir==1){ 
00099          StepperPos++;
00100      }else{
00101         StepperPos--;
00102      }
00103     }
00104 }
00105 
00106 // Ticker function
00107 void Sensor_Sample() {
00108     Sensor_Sample_Flag=1;
00109 }
00110 
00111 // millis ticker // its roughly gets the job done, so hell why not... (Better solution is to use SysTick)
00112 void MillisCounter ()
00113 {
00114     MillisValue++;
00115 }
00116 
00117 // Main Code:
00118 int main() {
00119     // Setup:
00120     pc.baud(921600); 
00121     
00122     //Attach ticker object:   
00123     SensorSampleTicker.attach_us(&Sensor_Sample, Sample_Interval); // create Sample Ticker
00124     
00125     // Attach millis ticker:
00126     MillisTicket.attach_us(&MillisCounter, 1000); // create 1Khz Ticker (1ms increment) 
00127     
00128     // Initialize Stepper:
00129         /* Initializing SPI bus. */
00130     DevSPI dev_spi(D11, D12, D13);
00131 
00132     /* Initializing Motor Control Component. */
00133     //StepperMotor = new L6474(D2, D8, D7, D9, D10, dev_spi);
00134     StepperMotor = new L6474(D2, D8, D3, D4, D10, dev_spi); // initialization with different Step pin and Dir Pin, Interference with external control
00135     if (StepperMotor->Init() != COMPONENT_OK)
00136         exit(EXIT_FAILURE);
00137     
00138     /*----- Changing motor setting. -----*/
00139     
00140     /* Setting High Impedance State to update L6474's registers. */
00141     StepperMotor->SoftHiZ();
00142     // Disabling motor
00143     StepperMotor->Disable();
00144     /* Changing step mode. */
00145     StepperMotor->SetStepMode(STEP_MODE_1_16);
00146     /* Increasing the torque regulation current to 500mA. */
00147     StepperMotor->SetParameter(L6474_TVAL, 500);
00148     
00149     // Enabling motor
00150     StepperMotor->Enable();
00151     
00152     /* Waiting 1 second. */
00153     wait_ms(1000);
00154     
00155     ////////////
00156     // Loop : //
00157     ////////////
00158     while(1){ 
00159     
00160         // receive Motor Command
00161         while (pc.readable()) {
00162             inbyte=pc.getc();
00163             //pc.printf("%c" ,inbyte); // debug check/
00164             BufferCMD[BufferIndex]=inbyte;
00165             BufferIndex++;
00166             // parse incoming message format: "$<value>\r\n"    
00167             if (inbyte=='$'){ // start of message
00168                 BufferIndex=0; // initialize to start of parser   
00169             }else if(inbyte=='\r'){ // end of message
00170                CMDValue=atof(BufferCMD);
00171                BufferIndex=0;   
00172                //pc.printf("CMD: %f \r\n" ,CMDValue); // debug check/ 
00173                //pc.printf("CMD: %s \r\n" ,BufferCMD); // debug check/
00174                
00175                /* Update Stepper Commad */
00176                if (CMDValue>0){
00177                     StepDir=1;
00178                 }else{
00179                     StepDir=0;
00180                     CMDValue=-CMDValue;
00181                 }
00182                if (CMDValue<1.0f){ // Stops
00183                   StepperTicker.detach(); 
00184                }else{
00185                    StepperTicker.attach(&Stepper_PWM,0.5f/CMDValue); // Set Stepper Freq, 2.0f/Freq : 2 calls to ticker object performs a step
00186                }
00187      
00188             }//end parser  
00189         }//end serial
00190         
00191         if(Sensor_Sample_Flag){ // sample data and stream out.
00192             Sensor_Sample_Flag=0;
00193             /* Stream Data */
00194             pc.printf("VAL:%d,%d\r\n" ,MillisValue,StepperPos); // Stream data
00195         }// End sampling and stream data
00196     }// end loop
00197 }// end main
00198