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@27:8e0acf4ae4fd, 2017-06-07 (annotated)

Committer:
Arkadi
Date:
Wed Jun 07 15:05:01 2017 +0000
Revision:
27:8e0acf4ae4fd
Parent:
26:09a541810137
Child:
28:32001ee473e0
Added Matlab Support for matlab control

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 27:8e0acf4ae4fd 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 27:8e0acf4ae4fd 116 // Send pos update
Arkadi 27:8e0acf4ae4fd 117 bool Pos_Update_Flag=0;
Arkadi 23:1bcf834fb859 118
Arkadi 23:1bcf834fb859 119 // Driver Flag status, enabled / disabled
Arkadi 23:1bcf834fb859 120 bool EN_Stepper_Flag=0;
Arkadi 23:1bcf834fb859 121
Arkadi 26:09a541810137 122 // flag to indicate control mode; 1 - SPD , 0 - Position
Arkadi 26:09a541810137 123 volatile bool SpdPos_Flag=0;
Arkadi 26:09a541810137 124
Arkadi 23:1bcf834fb859 125 // Motor Speed control
Arkadi 26:09a541810137 126 volatile float CMD_Motor_SPD[3]= {0}; // rotations / sec
Arkadi 26:09a541810137 127 volatile float Motor_SPD[3]= {0}; // rotations / sec
Davidroid 0:e6a49a092e2a 128
Arkadi 23:1bcf834fb859 129 // Motor Position control
Arkadi 26:09a541810137 130 volatile float CMD_Motor_Pos[3]= {0}; // command motor angle in degrees
Arkadi 26:09a541810137 131 volatile float Motor_Pos[3]= {0}; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
Arkadi 23:1bcf834fb859 132
Arkadi 25:716a21ab5fd3 133 // variable to store motor position
Arkadi 26:09a541810137 134 volatile int Motor_Steps[3] = {0}; // motor steps performed
Arkadi 25:716a21ab5fd3 135
Arkadi 23:1bcf834fb859 136 // timeout command time stamp
Arkadi 23:1bcf834fb859 137 volatile int CMDTimeStamp=0;
Arkadi 23:1bcf834fb859 138
Arkadi 23:1bcf834fb859 139 ///////////////
Arkadi 23:1bcf834fb859 140 // Functions //
Arkadi 23:1bcf834fb859 141 ///////////////
Arkadi 23:1bcf834fb859 142
Arkadi 23:1bcf834fb859 143 // Incoming Message parser
Arkadi 23:1bcf834fb859 144 void Parse_Message(char inbyte);
Arkadi 23:1bcf834fb859 145
Arkadi 23:1bcf834fb859 146 // Init Platform
Arkadi 23:1bcf834fb859 147 void Platform_Init();
Arkadi 23:1bcf834fb859 148
Arkadi 23:1bcf834fb859 149 // Platform Motion Set
Arkadi 26:09a541810137 150 void Platform_Motion_Set(float Set_M1, float Set_M2, float Set_M3);
Arkadi 23:1bcf834fb859 151
Arkadi 23:1bcf834fb859 152 // Platform Motion Control
Arkadi 23:1bcf834fb859 153 void Platform_Motion_Control();
Arkadi 23:1bcf834fb859 154
Arkadi 23:1bcf834fb859 155 // Motor1 Step Control
Arkadi 23:1bcf834fb859 156 void Motor1_Step_Control();
Arkadi 23:1bcf834fb859 157
Arkadi 23:1bcf834fb859 158 // Motor2 Step Control
Arkadi 23:1bcf834fb859 159 void Motor2_Step_Control();
Arkadi 23:1bcf834fb859 160
Arkadi 23:1bcf834fb859 161 // Motor3 Step Control
Arkadi 23:1bcf834fb859 162 void Motor3_Step_Control();
Arkadi 23:1bcf834fb859 163
Arkadi 23:1bcf834fb859 164 ////////////////////////
Arkadi 23:1bcf834fb859 165 // Main Code Setup : //
Arkadi 23:1bcf834fb859 166 ////////////////////////
Davidroid 0:e6a49a092e2a 167 int main()
Davidroid 0:e6a49a092e2a 168 {
Arkadi 23:1bcf834fb859 169 //Initializing SPI bus.
Davidroid 0:e6a49a092e2a 170 DevSPI dev_spi(D11, D12, D13);
Arkadi 25:716a21ab5fd3 171
Arkadi 27:8e0acf4ae4fd 172 //Initializing Motor Control Components.
Arkadi 25:716a21ab5fd3 173 motor1 = new L6474(D2, D8, D10, dev_spi);
Arkadi 25:716a21ab5fd3 174 motor2 = new L6474(D2, D8, D10, dev_spi);
Arkadi 25:716a21ab5fd3 175 motor3 = new L6474(D2, D8, D10, dev_spi);
Arkadi 25:716a21ab5fd3 176
Arkadi 23:1bcf834fb859 177 // Setup serial
Arkadi 27:8e0acf4ae4fd 178 pc.baud(256000);
Arkadi 23:1bcf834fb859 179
Arkadi 23:1bcf834fb859 180 // Init shika shuka
Arkadi 23:1bcf834fb859 181 Platform_Init();
Arkadi 25:716a21ab5fd3 182
Arkadi 23:1bcf834fb859 183 // initil time timer:
Arkadi 23:1bcf834fb859 184 time_timer.start();
Arkadi 23:1bcf834fb859 185
Arkadi 23:1bcf834fb859 186 ///////////////////////
Arkadi 23:1bcf834fb859 187 // Main Code Loop : //
Arkadi 23:1bcf834fb859 188 ///////////////////////
Arkadi 27:8e0acf4ae4fd 189 while(1) {
Arkadi 23:1bcf834fb859 190 // loop time stamp
Arkadi 25:716a21ab5fd3 191 int Timer_TimeStamp_ms=time_timer.read_ms();
Arkadi 25:716a21ab5fd3 192
Arkadi 23:1bcf834fb859 193 // receive Motor Command
Arkadi 25:716a21ab5fd3 194 while (pc.readable()) {
Arkadi 25:716a21ab5fd3 195 char InChar=pc.getc();
Arkadi 25:716a21ab5fd3 196 pc.printf("%c" ,InChar); // debug check/
Arkadi 23:1bcf834fb859 197 Parse_Message(InChar);
Arkadi 23:1bcf834fb859 198 }//end serial
Arkadi 25:716a21ab5fd3 199
Arkadi 23:1bcf834fb859 200 // set time out on commad and stop motion
Arkadi 27:8e0acf4ae4fd 201 if (abs(Timer_TimeStamp_ms-CMDTimeStamp)>TimeoutCommand) {
Arkadi 23:1bcf834fb859 202 #ifdef DEBUG_MSG
Arkadi 27:8e0acf4ae4fd 203 // pc.printf("Timer_TimeStamp_ms %d CMDTimeStamp %d\r\n", Timer_TimeStamp_ms,CMDTimeStamp);
Arkadi 23:1bcf834fb859 204 #endif /* DEBUG_MSG */
Arkadi 27:8e0acf4ae4fd 205 CMDTimeStamp=Timer_TimeStamp_ms;
Arkadi 27:8e0acf4ae4fd 206 motor1->Disable();
Arkadi 27:8e0acf4ae4fd 207 motor2->Disable();
Arkadi 27:8e0acf4ae4fd 208 motor3->Disable();
Arkadi 27:8e0acf4ae4fd 209 CMD_Motor_SPD[0]=0;
Arkadi 27:8e0acf4ae4fd 210 CMD_Motor_SPD[1]=0;
Arkadi 27:8e0acf4ae4fd 211 CMD_Motor_SPD[2]=0;
Arkadi 27:8e0acf4ae4fd 212 Motor_SPD[0]=0;
Arkadi 27:8e0acf4ae4fd 213 Motor_SPD[1]=0;
Arkadi 27:8e0acf4ae4fd 214 Motor_SPD[2]=0;
Arkadi 27:8e0acf4ae4fd 215 EN_Stepper_Flag=0;
Arkadi 27:8e0acf4ae4fd 216
Arkadi 27:8e0acf4ae4fd 217 // reset motor position
Arkadi 27:8e0acf4ae4fd 218 Motor_Steps[0]=0;
Arkadi 27:8e0acf4ae4fd 219 Motor_Steps[1]=0;
Arkadi 27:8e0acf4ae4fd 220 Motor_Steps[2]=0;
Arkadi 27:8e0acf4ae4fd 221 CMD_Motor_Pos[0]=0;
Arkadi 27:8e0acf4ae4fd 222 CMD_Motor_Pos[1]=0;
Arkadi 27:8e0acf4ae4fd 223 CMD_Motor_Pos[2]=0;
Arkadi 25:716a21ab5fd3 224 } // end timeout
Arkadi 27:8e0acf4ae4fd 225 if (Pos_Update_Flag) {
Arkadi 27:8e0acf4ae4fd 226 Pos_Update_Flag=0;
Arkadi 27:8e0acf4ae4fd 227 int Temp_TimeStamp_ms=time_timer.read_ms();
Arkadi 27:8e0acf4ae4fd 228 static uint32_t MSG_Counter=0;
Arkadi 27:8e0acf4ae4fd 229 MSG_Counter++;
Arkadi 27:8e0acf4ae4fd 230 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
Arkadi 27:8e0acf4ae4fd 231 }// end position update
Arkadi 23:1bcf834fb859 232 }// End Main Loop
Arkadi 23:1bcf834fb859 233 }// End Main
Arkadi 23:1bcf834fb859 234
Arkadi 23:1bcf834fb859 235
Arkadi 23:1bcf834fb859 236 ///////////////
Arkadi 23:1bcf834fb859 237 // Functions //
Arkadi 23:1bcf834fb859 238 ///////////////
Arkadi 23:1bcf834fb859 239 // Incoming Message parser Format: "$<SPD_M1>,<SPD_M2>,<SPD_YAW>,<PITCH_ANGLE>\r\n" // up to /r/n
Arkadi 23:1bcf834fb859 240 void Parse_Message(char inbyte)
Arkadi 23:1bcf834fb859 241 {
Arkadi 23:1bcf834fb859 242 static const uint16_t BufferCMDSize=32;
Arkadi 23:1bcf834fb859 243 static const uint16_t BufferCMD_ValuesSize=4;
Arkadi 23:1bcf834fb859 244
Arkadi 23:1bcf834fb859 245 static float CMD_Values[BufferCMD_ValuesSize]= {0};
Arkadi 23:1bcf834fb859 246 static char BufferCMD[BufferCMDSize]= {0};
Arkadi 23:1bcf834fb859 247 static uint16_t BufferIndex=0;
Arkadi 23:1bcf834fb859 248 static uint16_t Values_Index=0;
Arkadi 23:1bcf834fb859 249
Arkadi 23:1bcf834fb859 250 BufferIndex=BufferIndex%(BufferCMDSize); // simple overflow handler
Arkadi 23:1bcf834fb859 251 Values_Index=Values_Index%(BufferCMD_ValuesSize); // simple overflow handler
Arkadi 23:1bcf834fb859 252
Arkadi 23:1bcf834fb859 253 BufferCMD[BufferIndex]=inbyte;
Arkadi 23:1bcf834fb859 254 BufferIndex++;
Arkadi 23:1bcf834fb859 255
Arkadi 23:1bcf834fb859 256 // parse incoming message
Arkadi 23:1bcf834fb859 257 if (inbyte=='$') { // start of message
Arkadi 23:1bcf834fb859 258 BufferIndex=0; // initialize to start of parser
Arkadi 23:1bcf834fb859 259 Values_Index=0; // index for values position
Arkadi 23:1bcf834fb859 260 } else if (inbyte==',') { // seperator char
Arkadi 23:1bcf834fb859 261 CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values
Arkadi 23:1bcf834fb859 262 BufferIndex=0; // initialize to start of parser
Arkadi 23:1bcf834fb859 263 Values_Index++;
Arkadi 23:1bcf834fb859 264 } else if(inbyte=='\r') { // end of message // parse message
Arkadi 23:1bcf834fb859 265 // Update last value
Arkadi 23:1bcf834fb859 266 CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values
Arkadi 23:1bcf834fb859 267
Arkadi 23:1bcf834fb859 268 BufferIndex=0; // initialize to start of parser
Arkadi 23:1bcf834fb859 269 Values_Index=0; // reset values index
Arkadi 25:716a21ab5fd3 270
Arkadi 23:1bcf834fb859 271 // set time stamp on time out commad
Arkadi 23:1bcf834fb859 272 CMDTimeStamp=time_timer.read_ms();
Arkadi 25:716a21ab5fd3 273
Arkadi 25:716a21ab5fd3 274 //0 - Speed Control 1 - Position Control
Arkadi 26:09a541810137 275 SpdPos_Flag=(bool)CMD_Values[0];
Arkadi 25:716a21ab5fd3 276
Arkadi 23:1bcf834fb859 277 // update command
Arkadi 26:09a541810137 278 Platform_Motion_Set(CMD_Values[1],CMD_Values[2],CMD_Values[3]);
Arkadi 27:8e0acf4ae4fd 279
Arkadi 23:1bcf834fb859 280 #ifdef DEBUG_MSG
Arkadi 25:716a21ab5fd3 281 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 282 //pc.printf("CMD: %.3f ,%.3f \r\n" ,CMD_Values[0],CMD_Values[1]); // debug check/
Arkadi 23:1bcf834fb859 283 led = !led;
Arkadi 23:1bcf834fb859 284 #endif /* DEBUG_MSG */
Arkadi 23:1bcf834fb859 285
Arkadi 23:1bcf834fb859 286 }//end parser
Arkadi 23:1bcf834fb859 287 }// end parser function
Arkadi 23:1bcf834fb859 288
Arkadi 23:1bcf834fb859 289
Arkadi 23:1bcf834fb859 290 // Init shika shuka
Arkadi 23:1bcf834fb859 291 void Platform_Init()
Arkadi 23:1bcf834fb859 292 {
Arkadi 23:1bcf834fb859 293 //Initializing Motor Control Components.
Davidroid 14:fcd452b03d1a 294 if (motor1->Init() != COMPONENT_OK)
Davidroid 14:fcd452b03d1a 295 exit(EXIT_FAILURE);
Davidroid 14:fcd452b03d1a 296 if (motor2->Init() != COMPONENT_OK)
Davidroid 14:fcd452b03d1a 297 exit(EXIT_FAILURE);
Arkadi 21:ed054abddfe4 298 if (motor3->Init() != COMPONENT_OK)
Arkadi 21:ed054abddfe4 299 exit(EXIT_FAILURE);
Arkadi 25:716a21ab5fd3 300
Arkadi 21:ed054abddfe4 301 /*----- Changing motor setting. -----*/
Arkadi 21:ed054abddfe4 302 /* Setting High Impedance State to update L6474's registers. */
Arkadi 21:ed054abddfe4 303 motor1->SoftHiZ();
Arkadi 21:ed054abddfe4 304 motor2->SoftHiZ();
Arkadi 21:ed054abddfe4 305 motor3->SoftHiZ();
Arkadi 21:ed054abddfe4 306 // Disabling motor
Arkadi 21:ed054abddfe4 307 motor1->Disable();
Arkadi 21:ed054abddfe4 308 motor2->Disable();
Arkadi 21:ed054abddfe4 309 motor3->Disable();
Arkadi 21:ed054abddfe4 310 /* Changing step mode. */
Arkadi 21:ed054abddfe4 311 motor1->SetStepMode(STEP_MODE_1_16);
Arkadi 21:ed054abddfe4 312 motor2->SetStepMode(STEP_MODE_1_16);
Arkadi 21:ed054abddfe4 313 motor3->SetStepMode(STEP_MODE_1_16);
Arkadi 21:ed054abddfe4 314
Arkadi 23:1bcf834fb859 315 /* Increasing the torque regulation current. */
Arkadi 23:1bcf834fb859 316 motor1->SetParameter(L6474_TVAL, 1250); // Limit 2.0A
Arkadi 23:1bcf834fb859 317 motor2->SetParameter(L6474_TVAL, 1650); // Limit 1.7A
Arkadi 25:716a21ab5fd3 318 motor3->SetParameter(L6474_TVAL, 300); // Limit 0.28A
Arkadi 23:1bcf834fb859 319
Arkadi 23:1bcf834fb859 320 /* Increasing the max threshold overcurrent. */
Arkadi 25:716a21ab5fd3 321 motor1->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA);
Arkadi 23:1bcf834fb859 322 motor2->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA);
Arkadi 23:1bcf834fb859 323 motor3->SetParameter(L6474_OCD_TH, L6474_OCD_TH_750mA);
Arkadi 25:716a21ab5fd3 324
Arkadi 21:ed054abddfe4 325 // Enabling motor
Arkadi 21:ed054abddfe4 326 motor1->Enable();
Arkadi 21:ed054abddfe4 327 motor2->Enable();
Arkadi 21:ed054abddfe4 328 motor3->Enable();
Arkadi 25:716a21ab5fd3 329
Arkadi 23:1bcf834fb859 330 // Initialize Control Pins
Arkadi 23:1bcf834fb859 331 MOT1Dir.write(0);
Arkadi 23:1bcf834fb859 332 MOT1Step.write(0);
Arkadi 23:1bcf834fb859 333 MOT2Dir.write(0);
Arkadi 23:1bcf834fb859 334 MOT2Step.write(0);
Arkadi 23:1bcf834fb859 335 MOT3Dir.write(0);
Arkadi 23:1bcf834fb859 336 MOT3Step.write(0);
Davidroid 0:e6a49a092e2a 337
Arkadi 25:716a21ab5fd3 338 // Start Moition Control // need implementation
Arkadi 25:716a21ab5fd3 339 Platform_Control_Ticker.attach_us(&Platform_Motion_Control, Motor_Control_Interval);
Arkadi 25:716a21ab5fd3 340
Arkadi 23:1bcf834fb859 341 }// End Init shika shuka
Arkadi 25:716a21ab5fd3 342
Arkadi 23:1bcf834fb859 343 // ShikaShuka Motion Set
Arkadi 26:09a541810137 344 void Platform_Motion_Set(float Set_M1, float Set_M2, float Set_M3)
Arkadi 23:1bcf834fb859 345 {
Arkadi 23:1bcf834fb859 346 static const float MaxSPDCMD=5.0f;
Arkadi 26:09a541810137 347 static const float DeadZoneCMD=0.0001f;
Arkadi 26:09a541810137 348 static const float MaxAngle=180.0f;
Arkadi 27:8e0acf4ae4fd 349
Arkadi 25:716a21ab5fd3 350 // Velocity control
Arkadi 25:716a21ab5fd3 351 if(SpdPos_Flag) {
Arkadi 25:716a21ab5fd3 352 // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
Arkadi 25:716a21ab5fd3 353 if (Set_M1 > MaxSPDCMD) Set_M1 = MaxSPDCMD;
Arkadi 25:716a21ab5fd3 354 if (Set_M1 < -MaxSPDCMD) Set_M1 = -MaxSPDCMD;
Arkadi 25:716a21ab5fd3 355 if (abs(Set_M1) < DeadZoneCMD) Set_M1 = 0;
Arkadi 23:1bcf834fb859 356
Arkadi 25:716a21ab5fd3 357 if (Set_M2 > MaxSPDCMD) Set_M2 = MaxSPDCMD;
Arkadi 25:716a21ab5fd3 358 if (Set_M2 < -MaxSPDCMD) Set_M2 = -MaxSPDCMD;
Arkadi 25:716a21ab5fd3 359 if (abs(Set_M2) < DeadZoneCMD) Set_M2 = 0;
Arkadi 25:716a21ab5fd3 360
Arkadi 25:716a21ab5fd3 361 if (Set_M3 > MaxSPDCMD) Set_M3 = MaxSPDCMD;
Arkadi 25:716a21ab5fd3 362 if (Set_M3 < -MaxSPDCMD) Set_M3 = -MaxSPDCMD;
Arkadi 25:716a21ab5fd3 363 if (abs(Set_M3) < DeadZoneCMD) Set_M3 = 0;
Arkadi 23:1bcf834fb859 364 // enable stepper drivers
Arkadi 25:716a21ab5fd3 365 if (EN_Stepper_Flag==0) {
Arkadi 25:716a21ab5fd3 366 motor1->Enable();
Arkadi 25:716a21ab5fd3 367 motor2->Enable();
Arkadi 25:716a21ab5fd3 368 motor3->Enable();
Arkadi 23:1bcf834fb859 369 EN_Stepper_Flag=1;
Arkadi 23:1bcf834fb859 370 }
Arkadi 23:1bcf834fb859 371 // update motor speed command
Arkadi 25:716a21ab5fd3 372 CMD_Motor_SPD[0]=Set_M1;
Arkadi 25:716a21ab5fd3 373 CMD_Motor_SPD[1]=Set_M2;
Arkadi 25:716a21ab5fd3 374 CMD_Motor_SPD[2]=Set_M3;
Davidroid 8:cec4c2c03a27 375
Arkadi 26:09a541810137 376 } else { // end velocity control - Start position control
Arkadi 27:8e0acf4ae4fd 377
Arkadi 27:8e0acf4ae4fd 378 // calculate position based on steps:
Arkadi 26:09a541810137 379 // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
Arkadi 26:09a541810137 380 if (Set_M1 > MaxAngle) Set_M1 = MaxAngle;
Arkadi 26:09a541810137 381 if (Set_M1 < -MaxAngle) Set_M1 = -MaxAngle;
Arkadi 26:09a541810137 382
Arkadi 26:09a541810137 383 if (Set_M2 > MaxAngle) Set_M2 = MaxAngle;
Arkadi 26:09a541810137 384 if (Set_M2 < -MaxAngle) Set_M2 = -MaxAngle;
Arkadi 25:716a21ab5fd3 385
Arkadi 26:09a541810137 386 if (Set_M3 > MaxAngle) Set_M3 = MaxAngle;
Arkadi 26:09a541810137 387 if (Set_M3 < -MaxAngle) Set_M3 = -MaxAngle;
Arkadi 26:09a541810137 388 // enable stepper drivers
Arkadi 26:09a541810137 389 if (EN_Stepper_Flag==0) {
Arkadi 26:09a541810137 390 motor1->Enable();
Arkadi 26:09a541810137 391 motor2->Enable();
Arkadi 26:09a541810137 392 motor3->Enable();
Arkadi 26:09a541810137 393 EN_Stepper_Flag=1;
Arkadi 26:09a541810137 394 }
Arkadi 26:09a541810137 395 // update motor speed command
Arkadi 26:09a541810137 396 CMD_Motor_Pos[0]=Set_M1;
Arkadi 26:09a541810137 397 CMD_Motor_Pos[1]=Set_M2;
Arkadi 26:09a541810137 398 CMD_Motor_Pos[2]=Set_M3;
Arkadi 27:8e0acf4ae4fd 399
Arkadi 25:716a21ab5fd3 400 }// end position control
Arkadi 25:716a21ab5fd3 401 }// End Platform Motion Set
Arkadi 25:716a21ab5fd3 402
Arkadi 25:716a21ab5fd3 403 // Platform Motion Control
Arkadi 25:716a21ab5fd3 404 void Platform_Motion_Control()
Arkadi 23:1bcf834fb859 405 {
Arkadi 23:1bcf834fb859 406 // variable limits: (-100>SPD_M>100)
Arkadi 25:716a21ab5fd3 407 static const float MaxSPD=0.5f; // rounds per second
Arkadi 26:09a541810137 408 static const float DeadZone=0.0001f;
Arkadi 25:716a21ab5fd3 409
Arkadi 25:716a21ab5fd3 410 // update max acceleration calculation !!!!!!
Arkadi 25:716a21ab5fd3 411 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 412
Arkadi 25:716a21ab5fd3 413 static float SetMotorSPD[3]= {0}; // the actual command set frequency in Hz
Arkadi 27:8e0acf4ae4fd 414
Arkadi 26:09a541810137 415 // calculate motor pos:
Arkadi 26:09a541810137 416 Motor_Pos[0]= (((float)Motor_Steps[0]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
Arkadi 26:09a541810137 417 Motor_Pos[1]= (((float)Motor_Steps[1]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
Arkadi 26:09a541810137 418 Motor_Pos[2]= (((float)Motor_Steps[2]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
Arkadi 27:8e0acf4ae4fd 419
Arkadi 26:09a541810137 420 // position control
Arkadi 27:8e0acf4ae4fd 421 if (SpdPos_Flag == 0) {
Arkadi 27:8e0acf4ae4fd 422 // implement control loop based on desired position and current position
Arkadi 27:8e0acf4ae4fd 423 // update velocity commands based on position control loop
Arkadi 27:8e0acf4ae4fd 424 CMD_Motor_SPD[0]=(CMD_Motor_Pos[0]-Motor_Pos[0]) * PROPORTIONAL_GAIN;
Arkadi 27:8e0acf4ae4fd 425 CMD_Motor_SPD[1]=(CMD_Motor_Pos[1]-Motor_Pos[1]) * PROPORTIONAL_GAIN;
Arkadi 27:8e0acf4ae4fd 426 CMD_Motor_SPD[2]=(CMD_Motor_Pos[2]-Motor_Pos[2]) * PROPORTIONAL_GAIN;
Arkadi 27:8e0acf4ae4fd 427
Arkadi 26:09a541810137 428 }
Arkadi 26:09a541810137 429 // update velocity commands
Arkadi 23:1bcf834fb859 430 // implement control loop here: (basic control with max acceleration limit)
Arkadi 23:1bcf834fb859 431 if(1) {
Arkadi 23:1bcf834fb859 432 if (abs(CMD_Motor_SPD[0]-Motor_SPD[0])> MaxACC) {
Arkadi 23:1bcf834fb859 433 CMD_Motor_SPD[0]>Motor_SPD[0] ? Motor_SPD[0]+=MaxACC : Motor_SPD[0]-=MaxACC;
Arkadi 23:1bcf834fb859 434 } else {
Arkadi 23:1bcf834fb859 435 Motor_SPD[0]=CMD_Motor_SPD[0];
Arkadi 23:1bcf834fb859 436 }
Arkadi 23:1bcf834fb859 437 if (abs(CMD_Motor_SPD[1]-Motor_SPD[1])> MaxACC) {
Arkadi 23:1bcf834fb859 438 CMD_Motor_SPD[1]>Motor_SPD[1] ? Motor_SPD[1]+=MaxACC : Motor_SPD[1]-=MaxACC;
Arkadi 23:1bcf834fb859 439 } else {
Arkadi 23:1bcf834fb859 440 Motor_SPD[1]=CMD_Motor_SPD[1];
Arkadi 23:1bcf834fb859 441 }
Arkadi 25:716a21ab5fd3 442 if (abs(CMD_Motor_SPD[2]-Motor_SPD[2])> MaxACC) {
Arkadi 25:716a21ab5fd3 443 CMD_Motor_SPD[2]>Motor_SPD[2] ? Motor_SPD[2]+=MaxACC : Motor_SPD[2]-=MaxACC;
Arkadi 25:716a21ab5fd3 444 } else {
Arkadi 25:716a21ab5fd3 445 Motor_SPD[2]=CMD_Motor_SPD[2];
Arkadi 25:716a21ab5fd3 446 }
Arkadi 25:716a21ab5fd3 447 }
Arkadi 25:716a21ab5fd3 448 // update driver frequency
Arkadi 23:1bcf834fb859 449 if (1) {
Arkadi 25:716a21ab5fd3 450 // Start Moition Control
Arkadi 23:1bcf834fb859 451 // motor 1
Arkadi 25:716a21ab5fd3 452
Arkadi 25:716a21ab5fd3 453 // update driver direction
Arkadi 25:716a21ab5fd3 454 if (Motor_SPD[0]>0) {
Arkadi 25:716a21ab5fd3 455 MOT1Dir.write(1);
Arkadi 25:716a21ab5fd3 456 } else {
Arkadi 25:716a21ab5fd3 457 MOT1Dir.write(0);
Arkadi 25:716a21ab5fd3 458 }
Arkadi 25:716a21ab5fd3 459
Arkadi 25:716a21ab5fd3 460 // check if SPD is higher than minimum value
Arkadi 23:1bcf834fb859 461 if (abs(Motor_SPD[0])<DeadZone) {
Arkadi 25:716a21ab5fd3 462 // disable pulsing, Set speed to zero
Arkadi 25:716a21ab5fd3 463 Motor1_Step_Ticker.detach();
Arkadi 25:716a21ab5fd3 464 SetMotorSPD[0]=0;
Arkadi 25:716a21ab5fd3 465
Arkadi 23:1bcf834fb859 466 } else {
Arkadi 25:716a21ab5fd3 467 // Set Pulse rate based on pulses per second
Arkadi 25:716a21ab5fd3 468 SetMotorSPD[0]=(abs(Motor_SPD[0])*STEPS2ROTATION);
Arkadi 25:716a21ab5fd3 469 if (SetMotorSPD[0]>STEPS2ROTATION*MaxSPD) SetMotorSPD[1]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
Arkadi 25:716a21ab5fd3 470 if (SetMotorSPD[0]<1) SetMotorSPD[0]=1; // make sure minimum frequency
Arkadi 25:716a21ab5fd3 471 Motor1_Step_Ticker.attach(&Motor1_Step_Control, (0.5f/(SetMotorSPD[0])));
Arkadi 23:1bcf834fb859 472 }
Arkadi 25:716a21ab5fd3 473
Arkadi 25:716a21ab5fd3 474 // motor 2
Arkadi 25:716a21ab5fd3 475
Arkadi 25:716a21ab5fd3 476 // update driver direction
Arkadi 25:716a21ab5fd3 477 if (Motor_SPD[1]>0) {
Arkadi 25:716a21ab5fd3 478 MOT2Dir.write(1);
Arkadi 23:1bcf834fb859 479 } else {
Arkadi 25:716a21ab5fd3 480 MOT2Dir.write(0);
Arkadi 25:716a21ab5fd3 481 }
Arkadi 25:716a21ab5fd3 482
Arkadi 25:716a21ab5fd3 483 // check if SPD is higher than minimum value
Arkadi 25:716a21ab5fd3 484 if (abs(Motor_SPD[1])<DeadZone) {
Arkadi 25:716a21ab5fd3 485 // disable pulsing, Set speed to zero
Arkadi 25:716a21ab5fd3 486 Motor2_Step_Ticker.detach();
Arkadi 25:716a21ab5fd3 487 SetMotorSPD[1]=0;
Arkadi 25:716a21ab5fd3 488
Arkadi 25:716a21ab5fd3 489 } else {
Arkadi 25:716a21ab5fd3 490 // Set Pulse rate based on pulses per second
Arkadi 25:716a21ab5fd3 491 SetMotorSPD[1]=(abs(Motor_SPD[1])*STEPS2ROTATION);
Arkadi 25:716a21ab5fd3 492 if (SetMotorSPD[1]>STEPS2ROTATION*MaxSPD) SetMotorSPD[1]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
Arkadi 25:716a21ab5fd3 493 if (SetMotorSPD[1]<1) SetMotorSPD[1]=1; // make sure minimum frequency
Arkadi 25:716a21ab5fd3 494 Motor2_Step_Ticker.attach(&Motor2_Step_Control, (0.5f/(SetMotorSPD[1])));
Arkadi 23:1bcf834fb859 495 }
Arkadi 25:716a21ab5fd3 496
Arkadi 25:716a21ab5fd3 497 // Motor 3
Arkadi 25:716a21ab5fd3 498 // update driver direction
Arkadi 25:716a21ab5fd3 499 if (Motor_SPD[2]>0) {
Arkadi 25:716a21ab5fd3 500 MOT3Dir.write(1);
Arkadi 23:1bcf834fb859 501 } else {
Arkadi 25:716a21ab5fd3 502 MOT3Dir.write(0);
Arkadi 25:716a21ab5fd3 503 }
Arkadi 25:716a21ab5fd3 504
Arkadi 25:716a21ab5fd3 505 // check if SPD is higher than minimum value
Arkadi 25:716a21ab5fd3 506 if (abs(Motor_SPD[2])<DeadZone) {
Arkadi 25:716a21ab5fd3 507 // disable pulsing, Set speed to zero
Arkadi 25:716a21ab5fd3 508 Motor3_Step_Ticker.detach();
Arkadi 25:716a21ab5fd3 509 SetMotorSPD[2]=0;
Arkadi 25:716a21ab5fd3 510
Arkadi 25:716a21ab5fd3 511 } else {
Arkadi 25:716a21ab5fd3 512 // Set Pulse rate based on pulses per second
Arkadi 25:716a21ab5fd3 513 SetMotorSPD[2]=(abs(Motor_SPD[2])*STEPS2ROTATION);
Arkadi 25:716a21ab5fd3 514 if (SetMotorSPD[2]>STEPS2ROTATION*MaxSPD) SetMotorSPD[2]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
Arkadi 25:716a21ab5fd3 515 if (SetMotorSPD[2]<1) SetMotorSPD[2]=1; // make sure minimum frequency
Arkadi 25:716a21ab5fd3 516 Motor3_Step_Ticker.attach(&Motor3_Step_Control, (0.5f/(SetMotorSPD[2])));
Arkadi 23:1bcf834fb859 517 }
Arkadi 25:716a21ab5fd3 518 }
Arkadi 25:716a21ab5fd3 519 #ifdef DEBUG_MSG
Arkadi 27:8e0acf4ae4fd 520 //pc.printf("SPD: %.3f ,%.3f,%.3f \r\n" ,SetMotorSPD[0],SetMotorSPD[1],SetMotorSPD[2]); // debug check/
Arkadi 27:8e0acf4ae4fd 521 led = !led;
Arkadi 25:716a21ab5fd3 522 #endif /* DEBUG_MSG */
Arkadi 26:09a541810137 523 // update position
Arkadi 27:8e0acf4ae4fd 524 Pos_Update_Flag=1;
Arkadi 25:716a21ab5fd3 525 }// End Platform Motion Control
Arkadi 25:716a21ab5fd3 526
Arkadi 25:716a21ab5fd3 527
Arkadi 25:716a21ab5fd3 528 // Motor 1 Ticker step control
Arkadi 25:716a21ab5fd3 529 void Motor1_Step_Control()
Arkadi 25:716a21ab5fd3 530 {
Arkadi 25:716a21ab5fd3 531 MOT1Step=!MOT1Step;
Arkadi 25:716a21ab5fd3 532 if (MOT1Step) {
Arkadi 27:8e0acf4ae4fd 533 MOT1Dir ? Motor_Steps[0]++ : Motor_Steps[0]--;
Davidroid 0:e6a49a092e2a 534 }
Arkadi 25:716a21ab5fd3 535 }// end motor 1 Step control
Arkadi 25:716a21ab5fd3 536
Arkadi 25:716a21ab5fd3 537 // Motor 2 Ticker step control
Arkadi 25:716a21ab5fd3 538 void Motor2_Step_Control()
Arkadi 25:716a21ab5fd3 539 {
Arkadi 25:716a21ab5fd3 540 MOT2Step=!MOT2Step;
Arkadi 25:716a21ab5fd3 541 if (MOT2Step) {
Arkadi 27:8e0acf4ae4fd 542 MOT2Dir ? Motor_Steps[1]++ : Motor_Steps[1]--;
Arkadi 25:716a21ab5fd3 543 }
Arkadi 25:716a21ab5fd3 544 }// end motor 2 Step control
Arkadi 25:716a21ab5fd3 545
Arkadi 25:716a21ab5fd3 546 // Motor 3 Ticker step control
Arkadi 25:716a21ab5fd3 547 void Motor3_Step_Control()
Arkadi 25:716a21ab5fd3 548 {
Arkadi 25:716a21ab5fd3 549 MOT3Step=!MOT3Step;
Arkadi 25:716a21ab5fd3 550 if (MOT3Step) {
Arkadi 27:8e0acf4ae4fd 551 MOT3Dir ? Motor_Steps[2]++ : Motor_Steps[2]--;
Arkadi 25:716a21ab5fd3 552 }
Arkadi 25:716a21ab5fd3 553 }// end motor 3 Step control
Arkadi 25:716a21ab5fd3 554