Arkadi Rafalovich / Mbed 2 deprecated Thesis_Rotating_Platform

Thesis Rotating Platform, Uart Control

Dependencies:   BufferedSerial X_NUCLEO_IHM01A1_Disabled_Control mbed

Fork of Demo_IHM01A1_3-Motors by Arkadi Rafalovich

main.cpp@26:09a541810137, 2017-06-07 (annotated)

Committer:
Arkadi
Date:
Wed Jun 07 09:15:03 2017 +0000
Revision:
26:09a541810137
Parent:
25:716a21ab5fd3
Child:
27:8e0acf4ae4fd
Position Control updated

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Arkadi 23:1bcf834fb859 1 ////////////////////////////////////////
Arkadi 23:1bcf834fb859 2 // Rotating Platform //
Arkadi 23:1bcf834fb859 3 // Arkadiraf@gmail.com - 24/05/2017 //
Arkadi 23:1bcf834fb859 4 ////////////////////////////////////////
Arkadi 23:1bcf834fb859 5
Arkadi 23:1bcf834fb859 6 /*
Arkadi 23:1bcf834fb859 7 Parts:
Arkadi 23:1bcf834fb859 8 Nucleo STM32F401RE
Arkadi 23:1bcf834fb859 9 X-NUCLEO-IHM01A1 - 3 Stepper motor controller
Arkadi 23:1bcf834fb859 10 */
Arkadi 23:1bcf834fb859 11
Arkadi 23:1bcf834fb859 12 /*
Arkadi 26:09a541810137 13 Improvements:
Arkadi 26:09a541810137 14 Move all constant parameters to #define,
Arkadi 26:09a541810137 15 Implement control loop (other than proportional)
Arkadi 26:09a541810137 16 Implement Velocity command saturation with position control (implements max speed limits)
Arkadi 26:09a541810137 17 Implement better stop condition for position control
Arkadi 26:09a541810137 18 */
Arkadi 26:09a541810137 19 /*
Arkadi 23:1bcf834fb859 20 Pinout:
Arkadi 23:1bcf834fb859 21 Nucleo STM32F401RE
Arkadi 23:1bcf834fb859 22 PA_5 --> led (DigitalOut)
Arkadi 23:1bcf834fb859 23
Arkadi 23:1bcf834fb859 24 PC - Serial 2
Arkadi 23:1bcf834fb859 25 PA_2 (Tx) --> STLINK
Arkadi 23:1bcf834fb859 26 PA_3 (Rx) --> STLINK
Arkadi 23:1bcf834fb859 27
Arkadi 23:1bcf834fb859 28 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)
Arkadi 23:1bcf834fb859 29 SPI:
Arkadi 23:1bcf834fb859 30 PA_7 (D11) --> mosi
Arkadi 23:1bcf834fb859 31 PA_9 (D12) --> miso
Arkadi 23:1bcf834fb859 32 PA_8 (D13) --> sclk
Arkadi 23:1bcf834fb859 33
Arkadi 23:1bcf834fb859 34 Motor 1
Arkadi 23:1bcf834fb859 35 PA_10(D2) --> flag_irq (DigitalOut)
Arkadi 23:1bcf834fb859 36 PA_9 (D8) --> Standby (DigitalOut)
Arkadi 23:1bcf834fb859 37 PA_8 (D7) --> MOT1Dir (DigitalOut)
Arkadi 23:1bcf834fb859 38 PC_7 (D9) --> MOT1Step (PWM)
Arkadi 23:1bcf834fb859 39 PB_6 (D10)--> ssel (DigitalOut)
Arkadi 23:1bcf834fb859 40
Arkadi 23:1bcf834fb859 41 Motor 2
Arkadi 23:1bcf834fb859 42 PA_10(D2) --> flag_irq (DigitalOut)
Arkadi 23:1bcf834fb859 43 PA_9 (D8) --> Standby (DigitalOut)
Arkadi 23:1bcf834fb859 44 PB_5 (D4) --> MOT2Dir (DigitalOut)
Arkadi 23:1bcf834fb859 45 PB_3 (D3) --> MOT2Step (PWM)
Arkadi 23:1bcf834fb859 46 PB_6 (D10)--> ssel (DigitalOut)
Arkadi 23:1bcf834fb859 47
Arkadi 23:1bcf834fb859 48 Motor 3
Arkadi 23:1bcf834fb859 49 PA_10(D2) --> flag_irq (DigitalOut)
Arkadi 23:1bcf834fb859 50 PA_9 (D8) --> Standby (DigitalOut)
Arkadi 23:1bcf834fb859 51 PB_4 (D5) --> MOT3Dir (DigitalOut)
Arkadi 23:1bcf834fb859 52 PB_10(D6) --> MOT3Step (PWM)
Arkadi 23:1bcf834fb859 53 PB_6 (D10)--> ssel (DigitalOut)
Davidroid 0:e6a49a092e2a 54
Davidroid 0:e6a49a092e2a 55
Arkadi 23:1bcf834fb859 56 */
Davidroid 0:e6a49a092e2a 57
Arkadi 23:1bcf834fb859 58 ///////////////
Arkadi 23:1bcf834fb859 59 // Libraries //
Arkadi 23:1bcf834fb859 60 ///////////////
Arkadi 23:1bcf834fb859 61 #include "mbed.h"
Arkadi 23:1bcf834fb859 62 #include "BufferedSerial.h" // solves issues of loosing data. alternative doing it yourself
Arkadi 23:1bcf834fb859 63 #include "l6474_class.h" // stepper library
Davidroid 0:e6a49a092e2a 64
Davidroid 0:e6a49a092e2a 65
Arkadi 23:1bcf834fb859 66 ///////////////
Arkadi 23:1bcf834fb859 67 // #defines //
Arkadi 23:1bcf834fb859 68 ///////////////
Arkadi 23:1bcf834fb859 69 #define DEBUG_MSG
Arkadi 25:716a21ab5fd3 70 #define Motor_Control_Interval 10000 // 100Hz
Arkadi 23:1bcf834fb859 71 #define TimeoutCommand 2000 // 2 second (ms units)
Arkadi 23:1bcf834fb859 72 #define STEPS2ROTATION 3200.0f // Number of steps for rotation at 16 microstepping
Davidroid 0:e6a49a092e2a 73
Arkadi 26:09a541810137 74 // control loop
Arkadi 26:09a541810137 75
Arkadi 26:09a541810137 76 #define PROPORTIONAL_GAIN 0.0027777f // 1*1/360; (// gain for rotations/sec
Davidroid 0:e6a49a092e2a 77
Arkadi 23:1bcf834fb859 78 /////////////
Arkadi 23:1bcf834fb859 79 // Objects //
Arkadi 23:1bcf834fb859 80 /////////////
Arkadi 23:1bcf834fb859 81
Arkadi 23:1bcf834fb859 82 // X-NUCLEO-IHM01A1
Arkadi 23:1bcf834fb859 83 DigitalOut MOT1Dir(D7);
Arkadi 23:1bcf834fb859 84 DigitalOut MOT1Step(D9);
Arkadi 23:1bcf834fb859 85 DigitalOut MOT2Dir(D4);
Arkadi 23:1bcf834fb859 86 DigitalOut MOT2Step(D3);
Arkadi 23:1bcf834fb859 87 DigitalOut MOT3Dir(D5);
Arkadi 23:1bcf834fb859 88 DigitalOut MOT3Step(D6);
Davidroid 0:e6a49a092e2a 89
Davidroid 0:e6a49a092e2a 90 /* Motor Control Component. */
Davidroid 3:02d9ec4f88b2 91 L6474 *motor1;
Davidroid 3:02d9ec4f88b2 92 L6474 *motor2;
Arkadi 21:ed054abddfe4 93 L6474 *motor3;
Davidroid 0:e6a49a092e2a 94
Arkadi 23:1bcf834fb859 95 // Led
Arkadi 23:1bcf834fb859 96 DigitalOut led(LED1);
Arkadi 23:1bcf834fb859 97
Arkadi 23:1bcf834fb859 98 // serial
Arkadi 23:1bcf834fb859 99 BufferedSerial pc(USBTX, USBRX);
Arkadi 23:1bcf834fb859 100
Arkadi 23:1bcf834fb859 101 // Define Ticker for Motor Motion Control Ticker
Arkadi 23:1bcf834fb859 102 Ticker Platform_Control_Ticker;
Arkadi 23:1bcf834fb859 103
Arkadi 23:1bcf834fb859 104 // Define Ticker for Steps control
Arkadi 23:1bcf834fb859 105 Ticker Motor1_Step_Ticker;
Arkadi 23:1bcf834fb859 106 Ticker Motor2_Step_Ticker;
Arkadi 23:1bcf834fb859 107 Ticker Motor3_Step_Ticker;
Arkadi 23:1bcf834fb859 108
Arkadi 23:1bcf834fb859 109 // Timer for clock counter
Arkadi 23:1bcf834fb859 110 Timer time_timer;
Arkadi 23:1bcf834fb859 111
Arkadi 23:1bcf834fb859 112
Arkadi 23:1bcf834fb859 113 ///////////////
Arkadi 23:1bcf834fb859 114 // variables //
Arkadi 23:1bcf834fb859 115 ///////////////
Arkadi 23:1bcf834fb859 116
Arkadi 23:1bcf834fb859 117 // Driver Flag status, enabled / disabled
Arkadi 23:1bcf834fb859 118 bool EN_Stepper_Flag=0;
Arkadi 23:1bcf834fb859 119
Arkadi 26:09a541810137 120 // flag to indicate control mode; 1 - SPD , 0 - Position
Arkadi 26:09a541810137 121 volatile bool SpdPos_Flag=0;
Arkadi 26:09a541810137 122
Arkadi 23:1bcf834fb859 123 // Motor Speed control
Arkadi 26:09a541810137 124 volatile float CMD_Motor_SPD[3]= {0}; // rotations / sec
Arkadi 26:09a541810137 125 volatile float Motor_SPD[3]= {0}; // rotations / sec
Davidroid 0:e6a49a092e2a 126
Arkadi 23:1bcf834fb859 127 // Motor Position control
Arkadi 26:09a541810137 128 volatile float CMD_Motor_Pos[3]= {0}; // command motor angle in degrees
Arkadi 26:09a541810137 129 volatile float Motor_Pos[3]= {0}; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
Arkadi 23:1bcf834fb859 130
Arkadi 25:716a21ab5fd3 131 // variable to store motor position
Arkadi 26:09a541810137 132 volatile int Motor_Steps[3] = {0}; // motor steps performed
Arkadi 25:716a21ab5fd3 133
Arkadi 23:1bcf834fb859 134 // timeout command time stamp
Arkadi 23:1bcf834fb859 135 volatile int CMDTimeStamp=0;
Arkadi 23:1bcf834fb859 136
Arkadi 23:1bcf834fb859 137 ///////////////
Arkadi 23:1bcf834fb859 138 // Functions //
Arkadi 23:1bcf834fb859 139 ///////////////
Arkadi 23:1bcf834fb859 140
Arkadi 23:1bcf834fb859 141 // Incoming Message parser
Arkadi 23:1bcf834fb859 142 void Parse_Message(char inbyte);
Arkadi 23:1bcf834fb859 143
Arkadi 23:1bcf834fb859 144 // Init Platform
Arkadi 23:1bcf834fb859 145 void Platform_Init();
Arkadi 23:1bcf834fb859 146
Arkadi 23:1bcf834fb859 147 // Platform Motion Set
Arkadi 26:09a541810137 148 void Platform_Motion_Set(float Set_M1, float Set_M2, float Set_M3);
Arkadi 23:1bcf834fb859 149
Arkadi 23:1bcf834fb859 150 // Platform Motion Control
Arkadi 23:1bcf834fb859 151 void Platform_Motion_Control();
Arkadi 23:1bcf834fb859 152
Arkadi 23:1bcf834fb859 153 // Motor1 Step Control
Arkadi 23:1bcf834fb859 154 void Motor1_Step_Control();
Arkadi 23:1bcf834fb859 155
Arkadi 23:1bcf834fb859 156 // Motor2 Step Control
Arkadi 23:1bcf834fb859 157 void Motor2_Step_Control();
Arkadi 23:1bcf834fb859 158
Arkadi 23:1bcf834fb859 159 // Motor3 Step Control
Arkadi 23:1bcf834fb859 160 void Motor3_Step_Control();
Arkadi 23:1bcf834fb859 161
Arkadi 23:1bcf834fb859 162 ////////////////////////
Arkadi 23:1bcf834fb859 163 // Main Code Setup : //
Arkadi 23:1bcf834fb859 164 ////////////////////////
Davidroid 0:e6a49a092e2a 165 int main()
Davidroid 0:e6a49a092e2a 166 {
Arkadi 23:1bcf834fb859 167 //Initializing SPI bus.
Davidroid 0:e6a49a092e2a 168 DevSPI dev_spi(D11, D12, D13);
Arkadi 25:716a21ab5fd3 169
Arkadi 26:09a541810137 170 //Initializing Motor Control Components.
Arkadi 25:716a21ab5fd3 171 motor1 = new L6474(D2, D8, D10, dev_spi);
Arkadi 25:716a21ab5fd3 172 motor2 = new L6474(D2, D8, D10, dev_spi);
Arkadi 25:716a21ab5fd3 173 motor3 = new L6474(D2, D8, D10, dev_spi);
Arkadi 25:716a21ab5fd3 174
Arkadi 23:1bcf834fb859 175 // Setup serial
Arkadi 25:716a21ab5fd3 176 pc.baud(115200);
Arkadi 23:1bcf834fb859 177
Arkadi 23:1bcf834fb859 178 // Init shika shuka
Arkadi 23:1bcf834fb859 179 Platform_Init();
Arkadi 25:716a21ab5fd3 180
Arkadi 23:1bcf834fb859 181 // initil time timer:
Arkadi 23:1bcf834fb859 182 time_timer.start();
Arkadi 23:1bcf834fb859 183
Arkadi 23:1bcf834fb859 184 ///////////////////////
Arkadi 23:1bcf834fb859 185 // Main Code Loop : //
Arkadi 23:1bcf834fb859 186 ///////////////////////
Arkadi 25:716a21ab5fd3 187 while(1) {
Arkadi 23:1bcf834fb859 188 // loop time stamp
Arkadi 25:716a21ab5fd3 189 int Timer_TimeStamp_ms=time_timer.read_ms();
Arkadi 25:716a21ab5fd3 190
Arkadi 23:1bcf834fb859 191 // receive Motor Command
Arkadi 25:716a21ab5fd3 192 while (pc.readable()) {
Arkadi 25:716a21ab5fd3 193 char InChar=pc.getc();
Arkadi 25:716a21ab5fd3 194 pc.printf("%c" ,InChar); // debug check/
Arkadi 23:1bcf834fb859 195 Parse_Message(InChar);
Arkadi 23:1bcf834fb859 196 }//end serial
Arkadi 25:716a21ab5fd3 197
Arkadi 23:1bcf834fb859 198 // set time out on commad and stop motion
Arkadi 23:1bcf834fb859 199 if (abs(Timer_TimeStamp_ms-CMDTimeStamp)>TimeoutCommand){
Arkadi 23:1bcf834fb859 200 #ifdef DEBUG_MSG
Arkadi 25:716a21ab5fd3 201 // pc.printf("Timer_TimeStamp_ms %d CMDTimeStamp %d\r\n", Timer_TimeStamp_ms,CMDTimeStamp);
Arkadi 23:1bcf834fb859 202 #endif /* DEBUG_MSG */
Arkadi 23:1bcf834fb859 203 CMDTimeStamp=Timer_TimeStamp_ms;
Arkadi 25:716a21ab5fd3 204 motor1->Disable();
Arkadi 25:716a21ab5fd3 205 motor2->Disable();
Arkadi 25:716a21ab5fd3 206 motor3->Disable();
Arkadi 23:1bcf834fb859 207 CMD_Motor_SPD[0]=0;
Arkadi 23:1bcf834fb859 208 CMD_Motor_SPD[1]=0;
Arkadi 25:716a21ab5fd3 209 CMD_Motor_SPD[2]=0;
Arkadi 23:1bcf834fb859 210 Motor_SPD[0]=0;
Arkadi 23:1bcf834fb859 211 Motor_SPD[1]=0;
Arkadi 25:716a21ab5fd3 212 Motor_SPD[2]=0;
Arkadi 23:1bcf834fb859 213 EN_Stepper_Flag=0;
Arkadi 26:09a541810137 214
Arkadi 26:09a541810137 215 // reset motor position
Arkadi 26:09a541810137 216 Motor_Steps[0]=0;
Arkadi 26:09a541810137 217 Motor_Steps[1]=0;
Arkadi 26:09a541810137 218 Motor_Steps[2]=0;
Arkadi 26:09a541810137 219 CMD_Motor_Pos[0]=0;
Arkadi 26:09a541810137 220 CMD_Motor_Pos[1]=0;
Arkadi 26:09a541810137 221 CMD_Motor_Pos[2]=0;
Arkadi 25:716a21ab5fd3 222 } // end timeout
Arkadi 23:1bcf834fb859 223 }// End Main Loop
Arkadi 23:1bcf834fb859 224 }// End Main
Arkadi 23:1bcf834fb859 225
Arkadi 23:1bcf834fb859 226
Arkadi 23:1bcf834fb859 227 ///////////////
Arkadi 23:1bcf834fb859 228 // Functions //
Arkadi 23:1bcf834fb859 229 ///////////////
Arkadi 23:1bcf834fb859 230 // Incoming Message parser Format: "$<SPD_M1>,<SPD_M2>,<SPD_YAW>,<PITCH_ANGLE>\r\n" // up to /r/n
Arkadi 23:1bcf834fb859 231 void Parse_Message(char inbyte)
Arkadi 23:1bcf834fb859 232 {
Arkadi 23:1bcf834fb859 233 static const uint16_t BufferCMDSize=32;
Arkadi 23:1bcf834fb859 234 static const uint16_t BufferCMD_ValuesSize=4;
Arkadi 23:1bcf834fb859 235
Arkadi 23:1bcf834fb859 236 static float CMD_Values[BufferCMD_ValuesSize]= {0};
Arkadi 23:1bcf834fb859 237 static char BufferCMD[BufferCMDSize]= {0};
Arkadi 23:1bcf834fb859 238 static uint16_t BufferIndex=0;
Arkadi 23:1bcf834fb859 239 static uint16_t Values_Index=0;
Arkadi 23:1bcf834fb859 240
Arkadi 23:1bcf834fb859 241 BufferIndex=BufferIndex%(BufferCMDSize); // simple overflow handler
Arkadi 23:1bcf834fb859 242 Values_Index=Values_Index%(BufferCMD_ValuesSize); // simple overflow handler
Arkadi 23:1bcf834fb859 243
Arkadi 23:1bcf834fb859 244 BufferCMD[BufferIndex]=inbyte;
Arkadi 23:1bcf834fb859 245 BufferIndex++;
Arkadi 23:1bcf834fb859 246
Arkadi 23:1bcf834fb859 247 // parse incoming message
Arkadi 23:1bcf834fb859 248 if (inbyte=='$') { // start of message
Arkadi 23:1bcf834fb859 249 BufferIndex=0; // initialize to start of parser
Arkadi 23:1bcf834fb859 250 Values_Index=0; // index for values position
Arkadi 23:1bcf834fb859 251 } else if (inbyte==',') { // seperator char
Arkadi 23:1bcf834fb859 252 CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values
Arkadi 23:1bcf834fb859 253 BufferIndex=0; // initialize to start of parser
Arkadi 23:1bcf834fb859 254 Values_Index++;
Arkadi 23:1bcf834fb859 255 } else if(inbyte=='\r') { // end of message // parse message
Arkadi 23:1bcf834fb859 256 // Update last value
Arkadi 23:1bcf834fb859 257 CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values
Arkadi 23:1bcf834fb859 258
Arkadi 23:1bcf834fb859 259 BufferIndex=0; // initialize to start of parser
Arkadi 23:1bcf834fb859 260 Values_Index=0; // reset values index
Arkadi 25:716a21ab5fd3 261
Arkadi 23:1bcf834fb859 262 // set time stamp on time out commad
Arkadi 23:1bcf834fb859 263 CMDTimeStamp=time_timer.read_ms();
Arkadi 25:716a21ab5fd3 264
Arkadi 25:716a21ab5fd3 265 //0 - Speed Control 1 - Position Control
Arkadi 26:09a541810137 266 SpdPos_Flag=(bool)CMD_Values[0];
Arkadi 25:716a21ab5fd3 267
Arkadi 23:1bcf834fb859 268 // update command
Arkadi 26:09a541810137 269 Platform_Motion_Set(CMD_Values[1],CMD_Values[2],CMD_Values[3]);
Arkadi 23:1bcf834fb859 270
Arkadi 23:1bcf834fb859 271 #ifdef DEBUG_MSG
Arkadi 25:716a21ab5fd3 272 pc.printf("CMD: %d ,%.3f ,%.3f ,%.3f \r\n" ,SpdPos_Flag,CMD_Values[1],CMD_Values[2],CMD_Values[3]); // debug check/
Arkadi 23:1bcf834fb859 273 //pc.printf("CMD: %.3f ,%.3f \r\n" ,CMD_Values[0],CMD_Values[1]); // debug check/
Arkadi 23:1bcf834fb859 274 led = !led;
Arkadi 23:1bcf834fb859 275 #endif /* DEBUG_MSG */
Arkadi 23:1bcf834fb859 276
Arkadi 23:1bcf834fb859 277 }//end parser
Arkadi 23:1bcf834fb859 278 }// end parser function
Arkadi 23:1bcf834fb859 279
Arkadi 23:1bcf834fb859 280
Arkadi 23:1bcf834fb859 281 // Init shika shuka
Arkadi 23:1bcf834fb859 282 void Platform_Init()
Arkadi 23:1bcf834fb859 283 {
Arkadi 23:1bcf834fb859 284 //Initializing Motor Control Components.
Davidroid 14:fcd452b03d1a 285 if (motor1->Init() != COMPONENT_OK)
Davidroid 14:fcd452b03d1a 286 exit(EXIT_FAILURE);
Davidroid 14:fcd452b03d1a 287 if (motor2->Init() != COMPONENT_OK)
Davidroid 14:fcd452b03d1a 288 exit(EXIT_FAILURE);
Arkadi 21:ed054abddfe4 289 if (motor3->Init() != COMPONENT_OK)
Arkadi 21:ed054abddfe4 290 exit(EXIT_FAILURE);
Arkadi 25:716a21ab5fd3 291
Arkadi 21:ed054abddfe4 292 /*----- Changing motor setting. -----*/
Arkadi 21:ed054abddfe4 293 /* Setting High Impedance State to update L6474's registers. */
Arkadi 21:ed054abddfe4 294 motor1->SoftHiZ();
Arkadi 21:ed054abddfe4 295 motor2->SoftHiZ();
Arkadi 21:ed054abddfe4 296 motor3->SoftHiZ();
Arkadi 21:ed054abddfe4 297 // Disabling motor
Arkadi 21:ed054abddfe4 298 motor1->Disable();
Arkadi 21:ed054abddfe4 299 motor2->Disable();
Arkadi 21:ed054abddfe4 300 motor3->Disable();
Arkadi 21:ed054abddfe4 301 /* Changing step mode. */
Arkadi 21:ed054abddfe4 302 motor1->SetStepMode(STEP_MODE_1_16);
Arkadi 21:ed054abddfe4 303 motor2->SetStepMode(STEP_MODE_1_16);
Arkadi 21:ed054abddfe4 304 motor3->SetStepMode(STEP_MODE_1_16);
Arkadi 21:ed054abddfe4 305
Arkadi 23:1bcf834fb859 306 /* Increasing the torque regulation current. */
Arkadi 23:1bcf834fb859 307 motor1->SetParameter(L6474_TVAL, 1250); // Limit 2.0A
Arkadi 23:1bcf834fb859 308 motor2->SetParameter(L6474_TVAL, 1650); // Limit 1.7A
Arkadi 25:716a21ab5fd3 309 motor3->SetParameter(L6474_TVAL, 300); // Limit 0.28A
Arkadi 23:1bcf834fb859 310
Arkadi 23:1bcf834fb859 311 /* Increasing the max threshold overcurrent. */
Arkadi 25:716a21ab5fd3 312 motor1->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA);
Arkadi 23:1bcf834fb859 313 motor2->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA);
Arkadi 23:1bcf834fb859 314 motor3->SetParameter(L6474_OCD_TH, L6474_OCD_TH_750mA);
Arkadi 25:716a21ab5fd3 315
Arkadi 21:ed054abddfe4 316 // Enabling motor
Arkadi 21:ed054abddfe4 317 motor1->Enable();
Arkadi 21:ed054abddfe4 318 motor2->Enable();
Arkadi 21:ed054abddfe4 319 motor3->Enable();
Arkadi 25:716a21ab5fd3 320
Arkadi 23:1bcf834fb859 321 // Initialize Control Pins
Arkadi 23:1bcf834fb859 322 MOT1Dir.write(0);
Arkadi 23:1bcf834fb859 323 MOT1Step.write(0);
Arkadi 23:1bcf834fb859 324 MOT2Dir.write(0);
Arkadi 23:1bcf834fb859 325 MOT2Step.write(0);
Arkadi 23:1bcf834fb859 326 MOT3Dir.write(0);
Arkadi 23:1bcf834fb859 327 MOT3Step.write(0);
Davidroid 0:e6a49a092e2a 328
Arkadi 25:716a21ab5fd3 329 // Start Moition Control // need implementation
Arkadi 25:716a21ab5fd3 330 Platform_Control_Ticker.attach_us(&Platform_Motion_Control, Motor_Control_Interval);
Arkadi 25:716a21ab5fd3 331
Arkadi 23:1bcf834fb859 332 }// End Init shika shuka
Arkadi 25:716a21ab5fd3 333
Arkadi 23:1bcf834fb859 334 // ShikaShuka Motion Set
Arkadi 26:09a541810137 335 void Platform_Motion_Set(float Set_M1, float Set_M2, float Set_M3)
Arkadi 23:1bcf834fb859 336 {
Arkadi 23:1bcf834fb859 337 static const float MaxSPDCMD=5.0f;
Arkadi 26:09a541810137 338 static const float DeadZoneCMD=0.0001f;
Arkadi 26:09a541810137 339 static const float MaxAngle=180.0f;
Arkadi 26:09a541810137 340
Arkadi 25:716a21ab5fd3 341 // Velocity control
Arkadi 25:716a21ab5fd3 342 if(SpdPos_Flag) {
Arkadi 25:716a21ab5fd3 343 // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
Arkadi 25:716a21ab5fd3 344 if (Set_M1 > MaxSPDCMD) Set_M1 = MaxSPDCMD;
Arkadi 25:716a21ab5fd3 345 if (Set_M1 < -MaxSPDCMD) Set_M1 = -MaxSPDCMD;
Arkadi 25:716a21ab5fd3 346 if (abs(Set_M1) < DeadZoneCMD) Set_M1 = 0;
Arkadi 23:1bcf834fb859 347
Arkadi 25:716a21ab5fd3 348 if (Set_M2 > MaxSPDCMD) Set_M2 = MaxSPDCMD;
Arkadi 25:716a21ab5fd3 349 if (Set_M2 < -MaxSPDCMD) Set_M2 = -MaxSPDCMD;
Arkadi 25:716a21ab5fd3 350 if (abs(Set_M2) < DeadZoneCMD) Set_M2 = 0;
Arkadi 25:716a21ab5fd3 351
Arkadi 25:716a21ab5fd3 352 if (Set_M3 > MaxSPDCMD) Set_M3 = MaxSPDCMD;
Arkadi 25:716a21ab5fd3 353 if (Set_M3 < -MaxSPDCMD) Set_M3 = -MaxSPDCMD;
Arkadi 25:716a21ab5fd3 354 if (abs(Set_M3) < DeadZoneCMD) Set_M3 = 0;
Arkadi 23:1bcf834fb859 355 // enable stepper drivers
Arkadi 25:716a21ab5fd3 356 if (EN_Stepper_Flag==0) {
Arkadi 25:716a21ab5fd3 357 motor1->Enable();
Arkadi 25:716a21ab5fd3 358 motor2->Enable();
Arkadi 25:716a21ab5fd3 359 motor3->Enable();
Arkadi 23:1bcf834fb859 360 EN_Stepper_Flag=1;
Arkadi 23:1bcf834fb859 361 }
Arkadi 23:1bcf834fb859 362 // update motor speed command
Arkadi 25:716a21ab5fd3 363 CMD_Motor_SPD[0]=Set_M1;
Arkadi 25:716a21ab5fd3 364 CMD_Motor_SPD[1]=Set_M2;
Arkadi 25:716a21ab5fd3 365 CMD_Motor_SPD[2]=Set_M3;
Davidroid 8:cec4c2c03a27 366
Arkadi 26:09a541810137 367 } else { // end velocity control - Start position control
Arkadi 25:716a21ab5fd3 368
Arkadi 25:716a21ab5fd3 369 // calculate position based on steps:
Arkadi 26:09a541810137 370 // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
Arkadi 26:09a541810137 371 if (Set_M1 > MaxAngle) Set_M1 = MaxAngle;
Arkadi 26:09a541810137 372 if (Set_M1 < -MaxAngle) Set_M1 = -MaxAngle;
Arkadi 26:09a541810137 373
Arkadi 26:09a541810137 374 if (Set_M2 > MaxAngle) Set_M2 = MaxAngle;
Arkadi 26:09a541810137 375 if (Set_M2 < -MaxAngle) Set_M2 = -MaxAngle;
Arkadi 25:716a21ab5fd3 376
Arkadi 26:09a541810137 377 if (Set_M3 > MaxAngle) Set_M3 = MaxAngle;
Arkadi 26:09a541810137 378 if (Set_M3 < -MaxAngle) Set_M3 = -MaxAngle;
Arkadi 26:09a541810137 379 // enable stepper drivers
Arkadi 26:09a541810137 380 if (EN_Stepper_Flag==0) {
Arkadi 26:09a541810137 381 motor1->Enable();
Arkadi 26:09a541810137 382 motor2->Enable();
Arkadi 26:09a541810137 383 motor3->Enable();
Arkadi 26:09a541810137 384 EN_Stepper_Flag=1;
Arkadi 26:09a541810137 385 }
Arkadi 26:09a541810137 386 // update motor speed command
Arkadi 26:09a541810137 387 CMD_Motor_Pos[0]=Set_M1;
Arkadi 26:09a541810137 388 CMD_Motor_Pos[1]=Set_M2;
Arkadi 26:09a541810137 389 CMD_Motor_Pos[2]=Set_M3;
Arkadi 26:09a541810137 390
Arkadi 25:716a21ab5fd3 391 }// end position control
Arkadi 25:716a21ab5fd3 392 }// End Platform Motion Set
Arkadi 25:716a21ab5fd3 393
Arkadi 25:716a21ab5fd3 394 // Platform Motion Control
Arkadi 25:716a21ab5fd3 395 void Platform_Motion_Control()
Arkadi 23:1bcf834fb859 396 {
Arkadi 23:1bcf834fb859 397 // variable limits: (-100>SPD_M>100)
Arkadi 25:716a21ab5fd3 398 static const float MaxSPD=0.5f; // rounds per second
Arkadi 26:09a541810137 399 static const float DeadZone=0.0001f;
Arkadi 25:716a21ab5fd3 400
Arkadi 25:716a21ab5fd3 401 // update max acceleration calculation !!!!!!
Arkadi 25:716a21ab5fd3 402 static const float MaxACC=0.5f/(1000000/Motor_Control_Interval); //acceleration set as val/sec here for open loop it is %/sec
Arkadi 25:716a21ab5fd3 403
Arkadi 25:716a21ab5fd3 404 static float SetMotorSPD[3]= {0}; // the actual command set frequency in Hz
Arkadi 26:09a541810137 405
Arkadi 26:09a541810137 406 // calculate motor pos:
Arkadi 26:09a541810137 407 Motor_Pos[0]= (((float)Motor_Steps[0]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
Arkadi 26:09a541810137 408 Motor_Pos[1]= (((float)Motor_Steps[1]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
Arkadi 26:09a541810137 409 Motor_Pos[2]= (((float)Motor_Steps[2]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
Arkadi 26:09a541810137 410
Arkadi 26:09a541810137 411 // position control
Arkadi 26:09a541810137 412 if (SpdPos_Flag == 0){
Arkadi 26:09a541810137 413 // implement control loop based on desired position and current position
Arkadi 26:09a541810137 414 // update velocity commands based on position control loop
Arkadi 26:09a541810137 415 CMD_Motor_SPD[0]=(CMD_Motor_Pos[0]-Motor_Pos[0]) * PROPORTIONAL_GAIN;
Arkadi 26:09a541810137 416 CMD_Motor_SPD[1]=(CMD_Motor_Pos[1]-Motor_Pos[1]) * PROPORTIONAL_GAIN;
Arkadi 26:09a541810137 417 CMD_Motor_SPD[2]=(CMD_Motor_Pos[2]-Motor_Pos[2]) * PROPORTIONAL_GAIN;
Arkadi 26:09a541810137 418
Arkadi 26:09a541810137 419 }
Arkadi 26:09a541810137 420 // update velocity commands
Arkadi 23:1bcf834fb859 421 // implement control loop here: (basic control with max acceleration limit)
Arkadi 23:1bcf834fb859 422 if(1) {
Arkadi 23:1bcf834fb859 423 if (abs(CMD_Motor_SPD[0]-Motor_SPD[0])> MaxACC) {
Arkadi 23:1bcf834fb859 424 CMD_Motor_SPD[0]>Motor_SPD[0] ? Motor_SPD[0]+=MaxACC : Motor_SPD[0]-=MaxACC;
Arkadi 23:1bcf834fb859 425 } else {
Arkadi 23:1bcf834fb859 426 Motor_SPD[0]=CMD_Motor_SPD[0];
Arkadi 23:1bcf834fb859 427 }
Arkadi 23:1bcf834fb859 428 if (abs(CMD_Motor_SPD[1]-Motor_SPD[1])> MaxACC) {
Arkadi 23:1bcf834fb859 429 CMD_Motor_SPD[1]>Motor_SPD[1] ? Motor_SPD[1]+=MaxACC : Motor_SPD[1]-=MaxACC;
Arkadi 23:1bcf834fb859 430 } else {
Arkadi 23:1bcf834fb859 431 Motor_SPD[1]=CMD_Motor_SPD[1];
Arkadi 23:1bcf834fb859 432 }
Arkadi 25:716a21ab5fd3 433 if (abs(CMD_Motor_SPD[2]-Motor_SPD[2])> MaxACC) {
Arkadi 25:716a21ab5fd3 434 CMD_Motor_SPD[2]>Motor_SPD[2] ? Motor_SPD[2]+=MaxACC : Motor_SPD[2]-=MaxACC;
Arkadi 25:716a21ab5fd3 435 } else {
Arkadi 25:716a21ab5fd3 436 Motor_SPD[2]=CMD_Motor_SPD[2];
Arkadi 25:716a21ab5fd3 437 }
Arkadi 25:716a21ab5fd3 438 }
Arkadi 25:716a21ab5fd3 439 // update driver frequency
Arkadi 23:1bcf834fb859 440 if (1) {
Arkadi 25:716a21ab5fd3 441 // Start Moition Control
Arkadi 23:1bcf834fb859 442 // motor 1
Arkadi 25:716a21ab5fd3 443
Arkadi 25:716a21ab5fd3 444 // update driver direction
Arkadi 25:716a21ab5fd3 445 if (Motor_SPD[0]>0) {
Arkadi 25:716a21ab5fd3 446 MOT1Dir.write(1);
Arkadi 25:716a21ab5fd3 447 } else {
Arkadi 25:716a21ab5fd3 448 MOT1Dir.write(0);
Arkadi 25:716a21ab5fd3 449 }
Arkadi 25:716a21ab5fd3 450
Arkadi 25:716a21ab5fd3 451 // check if SPD is higher than minimum value
Arkadi 23:1bcf834fb859 452 if (abs(Motor_SPD[0])<DeadZone) {
Arkadi 25:716a21ab5fd3 453 // disable pulsing, Set speed to zero
Arkadi 25:716a21ab5fd3 454 Motor1_Step_Ticker.detach();
Arkadi 25:716a21ab5fd3 455 SetMotorSPD[0]=0;
Arkadi 25:716a21ab5fd3 456
Arkadi 23:1bcf834fb859 457 } else {
Arkadi 25:716a21ab5fd3 458 // Set Pulse rate based on pulses per second
Arkadi 25:716a21ab5fd3 459 SetMotorSPD[0]=(abs(Motor_SPD[0])*STEPS2ROTATION);
Arkadi 25:716a21ab5fd3 460 if (SetMotorSPD[0]>STEPS2ROTATION*MaxSPD) SetMotorSPD[1]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
Arkadi 25:716a21ab5fd3 461 if (SetMotorSPD[0]<1) SetMotorSPD[0]=1; // make sure minimum frequency
Arkadi 25:716a21ab5fd3 462 Motor1_Step_Ticker.attach(&Motor1_Step_Control, (0.5f/(SetMotorSPD[0])));
Arkadi 23:1bcf834fb859 463 }
Arkadi 25:716a21ab5fd3 464
Arkadi 25:716a21ab5fd3 465 // motor 2
Arkadi 25:716a21ab5fd3 466
Arkadi 25:716a21ab5fd3 467 // update driver direction
Arkadi 25:716a21ab5fd3 468 if (Motor_SPD[1]>0) {
Arkadi 25:716a21ab5fd3 469 MOT2Dir.write(1);
Arkadi 23:1bcf834fb859 470 } else {
Arkadi 25:716a21ab5fd3 471 MOT2Dir.write(0);
Arkadi 25:716a21ab5fd3 472 }
Arkadi 25:716a21ab5fd3 473
Arkadi 25:716a21ab5fd3 474 // check if SPD is higher than minimum value
Arkadi 25:716a21ab5fd3 475 if (abs(Motor_SPD[1])<DeadZone) {
Arkadi 25:716a21ab5fd3 476 // disable pulsing, Set speed to zero
Arkadi 25:716a21ab5fd3 477 Motor2_Step_Ticker.detach();
Arkadi 25:716a21ab5fd3 478 SetMotorSPD[1]=0;
Arkadi 25:716a21ab5fd3 479
Arkadi 25:716a21ab5fd3 480 } else {
Arkadi 25:716a21ab5fd3 481 // Set Pulse rate based on pulses per second
Arkadi 25:716a21ab5fd3 482 SetMotorSPD[1]=(abs(Motor_SPD[1])*STEPS2ROTATION);
Arkadi 25:716a21ab5fd3 483 if (SetMotorSPD[1]>STEPS2ROTATION*MaxSPD) SetMotorSPD[1]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
Arkadi 25:716a21ab5fd3 484 if (SetMotorSPD[1]<1) SetMotorSPD[1]=1; // make sure minimum frequency
Arkadi 25:716a21ab5fd3 485 Motor2_Step_Ticker.attach(&Motor2_Step_Control, (0.5f/(SetMotorSPD[1])));
Arkadi 23:1bcf834fb859 486 }
Arkadi 25:716a21ab5fd3 487
Arkadi 25:716a21ab5fd3 488 // Motor 3
Arkadi 25:716a21ab5fd3 489 // update driver direction
Arkadi 25:716a21ab5fd3 490 if (Motor_SPD[2]>0) {
Arkadi 25:716a21ab5fd3 491 MOT3Dir.write(1);
Arkadi 23:1bcf834fb859 492 } else {
Arkadi 25:716a21ab5fd3 493 MOT3Dir.write(0);
Arkadi 25:716a21ab5fd3 494 }
Arkadi 25:716a21ab5fd3 495
Arkadi 25:716a21ab5fd3 496 // check if SPD is higher than minimum value
Arkadi 25:716a21ab5fd3 497 if (abs(Motor_SPD[2])<DeadZone) {
Arkadi 25:716a21ab5fd3 498 // disable pulsing, Set speed to zero
Arkadi 25:716a21ab5fd3 499 Motor3_Step_Ticker.detach();
Arkadi 25:716a21ab5fd3 500 SetMotorSPD[2]=0;
Arkadi 25:716a21ab5fd3 501
Arkadi 25:716a21ab5fd3 502 } else {
Arkadi 25:716a21ab5fd3 503 // Set Pulse rate based on pulses per second
Arkadi 25:716a21ab5fd3 504 SetMotorSPD[2]=(abs(Motor_SPD[2])*STEPS2ROTATION);
Arkadi 25:716a21ab5fd3 505 if (SetMotorSPD[2]>STEPS2ROTATION*MaxSPD) SetMotorSPD[2]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
Arkadi 25:716a21ab5fd3 506 if (SetMotorSPD[2]<1) SetMotorSPD[2]=1; // make sure minimum frequency
Arkadi 25:716a21ab5fd3 507 Motor3_Step_Ticker.attach(&Motor3_Step_Control, (0.5f/(SetMotorSPD[2])));
Arkadi 23:1bcf834fb859 508 }
Arkadi 25:716a21ab5fd3 509 }
Arkadi 25:716a21ab5fd3 510 #ifdef DEBUG_MSG
Arkadi 25:716a21ab5fd3 511 //pc.printf("SPD: %.3f ,%.3f,%.3f \r\n" ,SetMotorSPD[0],SetMotorSPD[1],SetMotorSPD[2]); // debug check/
Arkadi 25:716a21ab5fd3 512 led = !led;
Arkadi 25:716a21ab5fd3 513 #endif /* DEBUG_MSG */
Arkadi 26:09a541810137 514
Arkadi 26:09a541810137 515 // update position
Arkadi 26:09a541810137 516 pc.printf("POS: %d ,%d, %d\r\n" ,Motor_Steps[0],Motor_Steps[1],Motor_Steps[2]); // send motors position
Arkadi 25:716a21ab5fd3 517
Arkadi 25:716a21ab5fd3 518 }// End Platform Motion Control
Arkadi 25:716a21ab5fd3 519
Arkadi 25:716a21ab5fd3 520
Arkadi 25:716a21ab5fd3 521 // Motor 1 Ticker step control
Arkadi 25:716a21ab5fd3 522 void Motor1_Step_Control()
Arkadi 25:716a21ab5fd3 523 {
Arkadi 25:716a21ab5fd3 524 MOT1Step=!MOT1Step;
Arkadi 25:716a21ab5fd3 525 if (MOT1Step) {
Arkadi 25:716a21ab5fd3 526 MOT1Dir ? Motor_Steps[0]++ : Motor_Steps[0]--;
Davidroid 0:e6a49a092e2a 527 }
Arkadi 25:716a21ab5fd3 528 }// end motor 1 Step control
Arkadi 25:716a21ab5fd3 529
Arkadi 25:716a21ab5fd3 530 // Motor 2 Ticker step control
Arkadi 25:716a21ab5fd3 531 void Motor2_Step_Control()
Arkadi 25:716a21ab5fd3 532 {
Arkadi 25:716a21ab5fd3 533 MOT2Step=!MOT2Step;
Arkadi 25:716a21ab5fd3 534 if (MOT2Step) {
Arkadi 25:716a21ab5fd3 535 MOT2Dir ? Motor_Steps[1]++ : Motor_Steps[1]--;
Arkadi 25:716a21ab5fd3 536 }
Arkadi 25:716a21ab5fd3 537 }// end motor 2 Step control
Arkadi 25:716a21ab5fd3 538
Arkadi 25:716a21ab5fd3 539 // Motor 3 Ticker step control
Arkadi 25:716a21ab5fd3 540 void Motor3_Step_Control()
Arkadi 25:716a21ab5fd3 541 {
Arkadi 25:716a21ab5fd3 542 MOT3Step=!MOT3Step;
Arkadi 25:716a21ab5fd3 543 if (MOT3Step) {
Arkadi 25:716a21ab5fd3 544 MOT3Dir ? Motor_Steps[2]++ : Motor_Steps[2]--;
Arkadi 25:716a21ab5fd3 545 }
Arkadi 25:716a21ab5fd3 546 }// end motor 3 Step control
Arkadi 25:716a21ab5fd3 547