Arkadi Rafalovich / Mbed 2 deprecated Stepper_Matlab_Control

Control Stepper Motor using Matlab via Serial port

Dependencies:   BufferedSerial X_NUCLEO_IHM01A1 mbed

main.cpp@1:fd0fbabd0c8f, 2016-01-26 (annotated)

Committer:
Arkadi
Date:
Tue Jan 26 15:06:37 2016 +0000
Revision:
1:fd0fbabd0c8f
Parent:
0:ad32f7e53ed6 
Small Fixes and updates

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Arkadi 0:ad32f7e53ed6 1 ////////////////////////////////////////
Arkadi 0:ad32f7e53ed6 2 // Control Stepper motor using matlab //
Arkadi 0:ad32f7e53ed6 3 // Arkadiraf@gmail.com - 24/1/2016 //
Arkadi 0:ad32f7e53ed6 4 ////////////////////////////////////////
Arkadi 0:ad32f7e53ed6 5 /*
Arkadi 0:ad32f7e53ed6 6 Parts:
Arkadi 0:ad32f7e53ed6 7 Nucleo STM32F401RE
Arkadi 0:ad32f7e53ed6 8 X-Nucleo-IHM01A1 Stepper Driver shield
Arkadi 0:ad32f7e53ed6 9
Arkadi 0:ad32f7e53ed6 10 */
Arkadi 0:ad32f7e53ed6 11
Arkadi 0:ad32f7e53ed6 12 // Libraries:
Arkadi 0:ad32f7e53ed6 13 #include "mbed.h"
Arkadi 0:ad32f7e53ed6 14 #include "BufferedSerial.h" // solves issues of loosing data. alternative doing it yourself
Arkadi 0:ad32f7e53ed6 15 #include "l6474_class.h" // stepper library
Arkadi 0:ad32f7e53ed6 16
Arkadi 0:ad32f7e53ed6 17 /////////////
Arkadi 0:ad32f7e53ed6 18 // Objects //
Arkadi 0:ad32f7e53ed6 19 ////////////
Arkadi 0:ad32f7e53ed6 20
Arkadi 0:ad32f7e53ed6 21 // create serial object
Arkadi 0:ad32f7e53ed6 22 //Serial pc(SERIAL_TX, SERIAL_RX);
Arkadi 0:ad32f7e53ed6 23 BufferedSerial pc(USBTX, USBRX);
Arkadi 0:ad32f7e53ed6 24
Arkadi 0:ad32f7e53ed6 25 // Pinouts:
Arkadi 0:ad32f7e53ed6 26 DigitalOut myled(LED1);
Arkadi 0:ad32f7e53ed6 27
Arkadi 0:ad32f7e53ed6 28 // Stepper Pins
Arkadi 0:ad32f7e53ed6 29 DigitalOut StepDir(D7);
Arkadi 0:ad32f7e53ed6 30 DigitalOut StepPWM(D9);
Arkadi 0:ad32f7e53ed6 31
Arkadi 0:ad32f7e53ed6 32 // Define Stepper Ticker
Arkadi 0:ad32f7e53ed6 33 Ticker StepperTicker;
Arkadi 0:ad32f7e53ed6 34
Arkadi 0:ad32f7e53ed6 35 // Define Ticker
Arkadi 0:ad32f7e53ed6 36 Ticker SensorSampleTicker;
Arkadi 0:ad32f7e53ed6 37
Arkadi 0:ad32f7e53ed6 38 // define millis ticker
Arkadi 0:ad32f7e53ed6 39 volatile uint32_t MillisValue = 0;
Arkadi 0:ad32f7e53ed6 40 Ticker MillisTicket;
Arkadi 0:ad32f7e53ed6 41
Arkadi 0:ad32f7e53ed6 42 ///////////////
Arkadi 0:ad32f7e53ed6 43 // variables //
Arkadi 0:ad32f7e53ed6 44 ///////////////
Arkadi 0:ad32f7e53ed6 45 char BufferCMD[64]={0};
Arkadi 0:ad32f7e53ed6 46 uint16_t BufferIndex=0;
Arkadi 0:ad32f7e53ed6 47 volatile bool Sensor_Sample_Flag=0;
Arkadi 1:fd0fbabd0c8f 48 #define Sample_Interval 5000 // interval in us, (5ms, 200Hz)
Arkadi 0:ad32f7e53ed6 49 float CMDValue=0; // CMD at which speed to run
Arkadi 0:ad32f7e53ed6 50 char inbyte=' ';
Arkadi 0:ad32f7e53ed6 51
Arkadi 0:ad32f7e53ed6 52 // Stepper Variables:
Arkadi 0:ad32f7e53ed6 53 #define STEPS2ROTATION 3200.0f // 200 steps rotation 16 microsteps
Arkadi 0:ad32f7e53ed6 54 /* Initialization parameters. */
Arkadi 0:ad32f7e53ed6 55 // not sure if it acctually uses it
Arkadi 0:ad32f7e53ed6 56 L6474_InitTypeDef init =
Arkadi 0:ad32f7e53ed6 57 {
Arkadi 0:ad32f7e53ed6 58 50, /* Acceleration rate in step/s2. Range: (0..+inf). */
Arkadi 0:ad32f7e53ed6 59 50, /* Deceleration rate in step/s2. Range: (0..+inf). */
Arkadi 0:ad32f7e53ed6 60 3500, /* Maximum speed in step/s. Range: (30..10000]. */
Arkadi 0:ad32f7e53ed6 61 30, /* Minimum speed in step/s. Range: [30..10000). */
Arkadi 0:ad32f7e53ed6 62 500, /* Torque regulation current in mA. Range: 31.25mA to 4000mA. */
Arkadi 0:ad32f7e53ed6 63 L6474_OCD_TH_1500mA, /* Overcurrent threshold (OCD_TH register). */
Arkadi 0:ad32f7e53ed6 64 L6474_CONFIG_OC_SD_ENABLE, /* Overcurrent shutwdown (OC_SD field of CONFIG register). */
Arkadi 0:ad32f7e53ed6 65 L6474_CONFIG_EN_TQREG_TVAL_USED, /* Torque regulation method (EN_TQREG field of CONFIG register). */
Arkadi 0:ad32f7e53ed6 66 L6474_STEP_SEL_1_16, /* Step selection (STEP_SEL field of STEP_MODE register). */
Arkadi 0:ad32f7e53ed6 67 L6474_SYNC_SEL_1_2, /* Sync selection (SYNC_SEL field of STEP_MODE register). */
Arkadi 0:ad32f7e53ed6 68 L6474_FAST_STEP_12us, /* Fall time value (T_FAST field of T_FAST register). Range: 2us to 32us. */
Arkadi 0:ad32f7e53ed6 69 L6474_TOFF_FAST_8us, /* Maximum fast decay time (T_OFF field of T_FAST register). Range: 2us to 32us. */
Arkadi 0:ad32f7e53ed6 70 3, /* Minimum ON time in us (TON_MIN register). Range: 0.5us to 64us. */
Arkadi 0:ad32f7e53ed6 71 21, /* Minimum OFF time in us (TOFF_MIN register). Range: 0.5us to 64us. */
Arkadi 0:ad32f7e53ed6 72 L6474_CONFIG_TOFF_044us, /* Target Swicthing Period (field TOFF of CONFIG register). */
Arkadi 0:ad32f7e53ed6 73 L6474_CONFIG_SR_320V_us, /* Slew rate (POW_SR field of CONFIG register). */
Arkadi 0:ad32f7e53ed6 74 L6474_CONFIG_INT_16MHZ, /* Clock setting (OSC_CLK_SEL field of CONFIG register). */
Arkadi 0:ad32f7e53ed6 75 L6474_ALARM_EN_OVERCURRENT |
Arkadi 0:ad32f7e53ed6 76 L6474_ALARM_EN_THERMAL_SHUTDOWN |
Arkadi 0:ad32f7e53ed6 77 L6474_ALARM_EN_THERMAL_WARNING |
Arkadi 0:ad32f7e53ed6 78 L6474_ALARM_EN_UNDERVOLTAGE |
Arkadi 0:ad32f7e53ed6 79 L6474_ALARM_EN_SW_TURN_ON |
Arkadi 0:ad32f7e53ed6 80 L6474_ALARM_EN_WRONG_NPERF_CMD /* Alarm (ALARM_EN register). */
Arkadi 0:ad32f7e53ed6 81 };
Arkadi 0:ad32f7e53ed6 82
Arkadi 0:ad32f7e53ed6 83
Arkadi 0:ad32f7e53ed6 84 /* Motor Control Component. */
Arkadi 0:ad32f7e53ed6 85 L6474 *StepperMotor;
Arkadi 0:ad32f7e53ed6 86
Arkadi 0:ad32f7e53ed6 87 // variable to store stepper position
Arkadi 0:ad32f7e53ed6 88 volatile int32_t StepperPos=0;
Arkadi 0:ad32f7e53ed6 89
Arkadi 0:ad32f7e53ed6 90
Arkadi 0:ad32f7e53ed6 91 ////////////////
Arkadi 0:ad32f7e53ed6 92 // Functions: //
Arkadi 0:ad32f7e53ed6 93 ////////////////
Arkadi 1:fd0fbabd0c8f 94 // Ticker function for stepper speed control, not very eficient.
Arkadi 0:ad32f7e53ed6 95 void Stepper_PWM() {
Arkadi 0:ad32f7e53ed6 96 StepPWM=!StepPWM;
Arkadi 0:ad32f7e53ed6 97 if (StepPWM==1){ //If change to high, step performed Update Stepper Position
Arkadi 0:ad32f7e53ed6 98 if (StepDir==1){
Arkadi 0:ad32f7e53ed6 99 StepperPos++;
Arkadi 0:ad32f7e53ed6 100 }else{
Arkadi 0:ad32f7e53ed6 101 StepperPos--;
Arkadi 0:ad32f7e53ed6 102 }
Arkadi 0:ad32f7e53ed6 103 }
Arkadi 0:ad32f7e53ed6 104 }
Arkadi 0:ad32f7e53ed6 105
Arkadi 0:ad32f7e53ed6 106 // Ticker function
Arkadi 0:ad32f7e53ed6 107 void Sensor_Sample() {
Arkadi 0:ad32f7e53ed6 108 Sensor_Sample_Flag=1;
Arkadi 0:ad32f7e53ed6 109 }
Arkadi 0:ad32f7e53ed6 110
Arkadi 0:ad32f7e53ed6 111 // millis ticker // its roughly gets the job done, so hell why not... (Better solution is to use SysTick)
Arkadi 0:ad32f7e53ed6 112 void MillisCounter ()
Arkadi 0:ad32f7e53ed6 113 {
Arkadi 0:ad32f7e53ed6 114 MillisValue++;
Arkadi 0:ad32f7e53ed6 115 }
Arkadi 0:ad32f7e53ed6 116
Arkadi 0:ad32f7e53ed6 117 // Main Code:
Arkadi 0:ad32f7e53ed6 118 int main() {
Arkadi 0:ad32f7e53ed6 119 // Setup:
Arkadi 0:ad32f7e53ed6 120 pc.baud(921600);
Arkadi 0:ad32f7e53ed6 121
Arkadi 0:ad32f7e53ed6 122 //Attach ticker object:
Arkadi 1:fd0fbabd0c8f 123 SensorSampleTicker.attach_us(&Sensor_Sample, Sample_Interval); // create Sample Ticker
Arkadi 0:ad32f7e53ed6 124
Arkadi 0:ad32f7e53ed6 125 // Attach millis ticker:
Arkadi 0:ad32f7e53ed6 126 MillisTicket.attach_us(&MillisCounter, 1000); // create 1Khz Ticker (1ms increment)
Arkadi 0:ad32f7e53ed6 127
Arkadi 0:ad32f7e53ed6 128 // Initialize Stepper:
Arkadi 0:ad32f7e53ed6 129 /* Initializing SPI bus. */
Arkadi 0:ad32f7e53ed6 130 DevSPI dev_spi(D11, D12, D13);
Arkadi 0:ad32f7e53ed6 131
Arkadi 0:ad32f7e53ed6 132 /* Initializing Motor Control Component. */
Arkadi 0:ad32f7e53ed6 133 //StepperMotor = new L6474(D2, D8, D7, D9, D10, dev_spi);
Arkadi 0:ad32f7e53ed6 134 StepperMotor = new L6474(D2, D8, D3, D4, D10, dev_spi); // initialization with different Step pin and Dir Pin, Interference with external control
Arkadi 0:ad32f7e53ed6 135 if (StepperMotor->Init() != COMPONENT_OK)
Arkadi 0:ad32f7e53ed6 136 exit(EXIT_FAILURE);
Arkadi 0:ad32f7e53ed6 137
Arkadi 0:ad32f7e53ed6 138 /*----- Changing motor setting. -----*/
Arkadi 0:ad32f7e53ed6 139
Arkadi 0:ad32f7e53ed6 140 /* Setting High Impedance State to update L6474's registers. */
Arkadi 0:ad32f7e53ed6 141 StepperMotor->SoftHiZ();
Arkadi 0:ad32f7e53ed6 142 // Disabling motor
Arkadi 0:ad32f7e53ed6 143 StepperMotor->Disable();
Arkadi 0:ad32f7e53ed6 144 /* Changing step mode. */
Arkadi 0:ad32f7e53ed6 145 StepperMotor->SetStepMode(STEP_MODE_1_16);
Arkadi 0:ad32f7e53ed6 146 /* Increasing the torque regulation current to 500mA. */
Arkadi 0:ad32f7e53ed6 147 StepperMotor->SetParameter(L6474_TVAL, 500);
Arkadi 0:ad32f7e53ed6 148
Arkadi 0:ad32f7e53ed6 149 // Enabling motor
Arkadi 0:ad32f7e53ed6 150 StepperMotor->Enable();
Arkadi 0:ad32f7e53ed6 151
Arkadi 0:ad32f7e53ed6 152 /* Waiting 1 second. */
Arkadi 0:ad32f7e53ed6 153 wait_ms(1000);
Arkadi 0:ad32f7e53ed6 154
Arkadi 0:ad32f7e53ed6 155 ////////////
Arkadi 0:ad32f7e53ed6 156 // Loop : //
Arkadi 0:ad32f7e53ed6 157 ////////////
Arkadi 0:ad32f7e53ed6 158 while(1){
Arkadi 0:ad32f7e53ed6 159
Arkadi 0:ad32f7e53ed6 160 // receive Motor Command
Arkadi 0:ad32f7e53ed6 161 while (pc.readable()) {
Arkadi 0:ad32f7e53ed6 162 inbyte=pc.getc();
Arkadi 0:ad32f7e53ed6 163 //pc.printf("%c" ,inbyte); // debug check/
Arkadi 0:ad32f7e53ed6 164 BufferCMD[BufferIndex]=inbyte;
Arkadi 0:ad32f7e53ed6 165 BufferIndex++;
Arkadi 0:ad32f7e53ed6 166 // parse incoming message format: "$<value>\r\n"
Arkadi 0:ad32f7e53ed6 167 if (inbyte=='$'){ // start of message
Arkadi 0:ad32f7e53ed6 168 BufferIndex=0; // initialize to start of parser
Arkadi 0:ad32f7e53ed6 169 }else if(inbyte=='\r'){ // end of message
Arkadi 0:ad32f7e53ed6 170 CMDValue=atof(BufferCMD);
Arkadi 0:ad32f7e53ed6 171 BufferIndex=0;
Arkadi 0:ad32f7e53ed6 172 //pc.printf("CMD: %f \r\n" ,CMDValue); // debug check/
Arkadi 0:ad32f7e53ed6 173 //pc.printf("CMD: %s \r\n" ,BufferCMD); // debug check/
Arkadi 0:ad32f7e53ed6 174
Arkadi 0:ad32f7e53ed6 175 /* Update Stepper Commad */
Arkadi 0:ad32f7e53ed6 176 if (CMDValue>0){
Arkadi 0:ad32f7e53ed6 177 StepDir=1;
Arkadi 0:ad32f7e53ed6 178 }else{
Arkadi 0:ad32f7e53ed6 179 StepDir=0;
Arkadi 0:ad32f7e53ed6 180 CMDValue=-CMDValue;
Arkadi 0:ad32f7e53ed6 181 }
Arkadi 0:ad32f7e53ed6 182 if (CMDValue<1.0f){ // Stops
Arkadi 0:ad32f7e53ed6 183 StepperTicker.detach();
Arkadi 0:ad32f7e53ed6 184 }else{
Arkadi 1:fd0fbabd0c8f 185 StepperTicker.attach(&Stepper_PWM,0.5f/CMDValue); // Set Stepper Freq, 2.0f/Freq : 2 calls to ticker object performs a step
Arkadi 0:ad32f7e53ed6 186 }
Arkadi 0:ad32f7e53ed6 187
Arkadi 0:ad32f7e53ed6 188 }//end parser
Arkadi 0:ad32f7e53ed6 189 }//end serial
Arkadi 0:ad32f7e53ed6 190
Arkadi 0:ad32f7e53ed6 191 if(Sensor_Sample_Flag){ // sample data and stream out.
Arkadi 0:ad32f7e53ed6 192 Sensor_Sample_Flag=0;
Arkadi 0:ad32f7e53ed6 193 /* Stream Data */
Arkadi 0:ad32f7e53ed6 194 pc.printf("VAL:%d,%d\r\n" ,MillisValue,StepperPos); // Stream data
Arkadi 0:ad32f7e53ed6 195 }// End sampling and stream data
Arkadi 0:ad32f7e53ed6 196 }// end loop
Arkadi 0:ad32f7e53ed6 197 }// end main
Arkadi 0:ad32f7e53ed6 198