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@28:32001ee473e0, 2017-06-14 (annotated)

Committer:
Arkadi
Date:
Wed Jun 14 11:47:40 2017 +0000
Revision:
28:32001ee473e0
Parent:
27:8e0acf4ae4fd 
Release version, Matlab integrated

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 28:32001ee473e0 225 // send position message
Arkadi 27:8e0acf4ae4fd 226 if (Pos_Update_Flag) {
Arkadi 27:8e0acf4ae4fd 227 Pos_Update_Flag=0;
Arkadi 27:8e0acf4ae4fd 228 int Temp_TimeStamp_ms=time_timer.read_ms();
Arkadi 27:8e0acf4ae4fd 229 static uint32_t MSG_Counter=0;
Arkadi 27:8e0acf4ae4fd 230 MSG_Counter++;
Arkadi 27:8e0acf4ae4fd 231 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 232 }// end position update
Arkadi 23:1bcf834fb859 233 }// End Main Loop
Arkadi 23:1bcf834fb859 234 }// End Main
Arkadi 23:1bcf834fb859 235
Arkadi 23:1bcf834fb859 236
Arkadi 23:1bcf834fb859 237 ///////////////
Arkadi 23:1bcf834fb859 238 // Functions //
Arkadi 23:1bcf834fb859 239 ///////////////
Arkadi 23:1bcf834fb859 240 // Incoming Message parser Format: "$<SPD_M1>,<SPD_M2>,<SPD_YAW>,<PITCH_ANGLE>\r\n" // up to /r/n
Arkadi 23:1bcf834fb859 241 void Parse_Message(char inbyte)
Arkadi 23:1bcf834fb859 242 {
Arkadi 23:1bcf834fb859 243 static const uint16_t BufferCMDSize=32;
Arkadi 23:1bcf834fb859 244 static const uint16_t BufferCMD_ValuesSize=4;
Arkadi 23:1bcf834fb859 245
Arkadi 23:1bcf834fb859 246 static float CMD_Values[BufferCMD_ValuesSize]= {0};
Arkadi 23:1bcf834fb859 247 static char BufferCMD[BufferCMDSize]= {0};
Arkadi 23:1bcf834fb859 248 static uint16_t BufferIndex=0;
Arkadi 23:1bcf834fb859 249 static uint16_t Values_Index=0;
Arkadi 23:1bcf834fb859 250
Arkadi 23:1bcf834fb859 251 BufferIndex=BufferIndex%(BufferCMDSize); // simple overflow handler
Arkadi 23:1bcf834fb859 252 Values_Index=Values_Index%(BufferCMD_ValuesSize); // simple overflow handler
Arkadi 23:1bcf834fb859 253
Arkadi 23:1bcf834fb859 254 BufferCMD[BufferIndex]=inbyte;
Arkadi 23:1bcf834fb859 255 BufferIndex++;
Arkadi 23:1bcf834fb859 256
Arkadi 23:1bcf834fb859 257 // parse incoming message
Arkadi 23:1bcf834fb859 258 if (inbyte=='$') { // start of message
Arkadi 23:1bcf834fb859 259 BufferIndex=0; // initialize to start of parser
Arkadi 23:1bcf834fb859 260 Values_Index=0; // index for values position
Arkadi 23:1bcf834fb859 261 } else if (inbyte==',') { // seperator char
Arkadi 23:1bcf834fb859 262 CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values
Arkadi 23:1bcf834fb859 263 BufferIndex=0; // initialize to start of parser
Arkadi 23:1bcf834fb859 264 Values_Index++;
Arkadi 23:1bcf834fb859 265 } else if(inbyte=='\r') { // end of message // parse message
Arkadi 23:1bcf834fb859 266 // Update last value
Arkadi 23:1bcf834fb859 267 CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values
Arkadi 23:1bcf834fb859 268
Arkadi 23:1bcf834fb859 269 BufferIndex=0; // initialize to start of parser
Arkadi 23:1bcf834fb859 270 Values_Index=0; // reset values index
Arkadi 25:716a21ab5fd3 271
Arkadi 23:1bcf834fb859 272 // set time stamp on time out commad
Arkadi 23:1bcf834fb859 273 CMDTimeStamp=time_timer.read_ms();
Arkadi 25:716a21ab5fd3 274
Arkadi 25:716a21ab5fd3 275 //0 - Speed Control 1 - Position Control
Arkadi 26:09a541810137 276 SpdPos_Flag=(bool)CMD_Values[0];
Arkadi 25:716a21ab5fd3 277
Arkadi 23:1bcf834fb859 278 // update command
Arkadi 26:09a541810137 279 Platform_Motion_Set(CMD_Values[1],CMD_Values[2],CMD_Values[3]);
Arkadi 27:8e0acf4ae4fd 280
Arkadi 23:1bcf834fb859 281 #ifdef DEBUG_MSG
Arkadi 25:716a21ab5fd3 282 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 283 //pc.printf("CMD: %.3f ,%.3f \r\n" ,CMD_Values[0],CMD_Values[1]); // debug check/
Arkadi 23:1bcf834fb859 284 led = !led;
Arkadi 23:1bcf834fb859 285 #endif /* DEBUG_MSG */
Arkadi 23:1bcf834fb859 286
Arkadi 23:1bcf834fb859 287 }//end parser
Arkadi 23:1bcf834fb859 288 }// end parser function
Arkadi 23:1bcf834fb859 289
Arkadi 23:1bcf834fb859 290
Arkadi 23:1bcf834fb859 291 // Init shika shuka
Arkadi 23:1bcf834fb859 292 void Platform_Init()
Arkadi 23:1bcf834fb859 293 {
Arkadi 23:1bcf834fb859 294 //Initializing Motor Control Components.
Davidroid 14:fcd452b03d1a 295 if (motor1->Init() != COMPONENT_OK)
Davidroid 14:fcd452b03d1a 296 exit(EXIT_FAILURE);
Davidroid 14:fcd452b03d1a 297 if (motor2->Init() != COMPONENT_OK)
Davidroid 14:fcd452b03d1a 298 exit(EXIT_FAILURE);
Arkadi 21:ed054abddfe4 299 if (motor3->Init() != COMPONENT_OK)
Arkadi 21:ed054abddfe4 300 exit(EXIT_FAILURE);
Arkadi 25:716a21ab5fd3 301
Arkadi 21:ed054abddfe4 302 /*----- Changing motor setting. -----*/
Arkadi 21:ed054abddfe4 303 /* Setting High Impedance State to update L6474's registers. */
Arkadi 21:ed054abddfe4 304 motor1->SoftHiZ();
Arkadi 21:ed054abddfe4 305 motor2->SoftHiZ();
Arkadi 21:ed054abddfe4 306 motor3->SoftHiZ();
Arkadi 21:ed054abddfe4 307 // Disabling motor
Arkadi 21:ed054abddfe4 308 motor1->Disable();
Arkadi 21:ed054abddfe4 309 motor2->Disable();
Arkadi 21:ed054abddfe4 310 motor3->Disable();
Arkadi 21:ed054abddfe4 311 /* Changing step mode. */
Arkadi 21:ed054abddfe4 312 motor1->SetStepMode(STEP_MODE_1_16);
Arkadi 21:ed054abddfe4 313 motor2->SetStepMode(STEP_MODE_1_16);
Arkadi 21:ed054abddfe4 314 motor3->SetStepMode(STEP_MODE_1_16);
Arkadi 21:ed054abddfe4 315
Arkadi 23:1bcf834fb859 316 /* Increasing the torque regulation current. */
Arkadi 23:1bcf834fb859 317 motor1->SetParameter(L6474_TVAL, 1250); // Limit 2.0A
Arkadi 23:1bcf834fb859 318 motor2->SetParameter(L6474_TVAL, 1650); // Limit 1.7A
Arkadi 25:716a21ab5fd3 319 motor3->SetParameter(L6474_TVAL, 300); // Limit 0.28A
Arkadi 23:1bcf834fb859 320
Arkadi 23:1bcf834fb859 321 /* Increasing the max threshold overcurrent. */
Arkadi 25:716a21ab5fd3 322 motor1->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA);
Arkadi 23:1bcf834fb859 323 motor2->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA);
Arkadi 23:1bcf834fb859 324 motor3->SetParameter(L6474_OCD_TH, L6474_OCD_TH_750mA);
Arkadi 25:716a21ab5fd3 325
Arkadi 21:ed054abddfe4 326 // Enabling motor
Arkadi 21:ed054abddfe4 327 motor1->Enable();
Arkadi 21:ed054abddfe4 328 motor2->Enable();
Arkadi 21:ed054abddfe4 329 motor3->Enable();
Arkadi 25:716a21ab5fd3 330
Arkadi 23:1bcf834fb859 331 // Initialize Control Pins
Arkadi 23:1bcf834fb859 332 MOT1Dir.write(0);
Arkadi 23:1bcf834fb859 333 MOT1Step.write(0);
Arkadi 23:1bcf834fb859 334 MOT2Dir.write(0);
Arkadi 23:1bcf834fb859 335 MOT2Step.write(0);
Arkadi 23:1bcf834fb859 336 MOT3Dir.write(0);
Arkadi 23:1bcf834fb859 337 MOT3Step.write(0);
Davidroid 0:e6a49a092e2a 338
Arkadi 25:716a21ab5fd3 339 // Start Moition Control // need implementation
Arkadi 25:716a21ab5fd3 340 Platform_Control_Ticker.attach_us(&Platform_Motion_Control, Motor_Control_Interval);
Arkadi 25:716a21ab5fd3 341
Arkadi 23:1bcf834fb859 342 }// End Init shika shuka
Arkadi 25:716a21ab5fd3 343
Arkadi 23:1bcf834fb859 344 // ShikaShuka Motion Set
Arkadi 26:09a541810137 345 void Platform_Motion_Set(float Set_M1, float Set_M2, float Set_M3)
Arkadi 23:1bcf834fb859 346 {
Arkadi 23:1bcf834fb859 347 static const float MaxSPDCMD=5.0f;
Arkadi 26:09a541810137 348 static const float DeadZoneCMD=0.0001f;
Arkadi 26:09a541810137 349 static const float MaxAngle=180.0f;
Arkadi 27:8e0acf4ae4fd 350
Arkadi 25:716a21ab5fd3 351 // Velocity control
Arkadi 25:716a21ab5fd3 352 if(SpdPos_Flag) {
Arkadi 25:716a21ab5fd3 353 // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
Arkadi 25:716a21ab5fd3 354 if (Set_M1 > MaxSPDCMD) Set_M1 = MaxSPDCMD;
Arkadi 25:716a21ab5fd3 355 if (Set_M1 < -MaxSPDCMD) Set_M1 = -MaxSPDCMD;
Arkadi 25:716a21ab5fd3 356 if (abs(Set_M1) < DeadZoneCMD) Set_M1 = 0;
Arkadi 23:1bcf834fb859 357
Arkadi 25:716a21ab5fd3 358 if (Set_M2 > MaxSPDCMD) Set_M2 = MaxSPDCMD;
Arkadi 25:716a21ab5fd3 359 if (Set_M2 < -MaxSPDCMD) Set_M2 = -MaxSPDCMD;
Arkadi 25:716a21ab5fd3 360 if (abs(Set_M2) < DeadZoneCMD) Set_M2 = 0;
Arkadi 25:716a21ab5fd3 361
Arkadi 25:716a21ab5fd3 362 if (Set_M3 > MaxSPDCMD) Set_M3 = MaxSPDCMD;
Arkadi 25:716a21ab5fd3 363 if (Set_M3 < -MaxSPDCMD) Set_M3 = -MaxSPDCMD;
Arkadi 25:716a21ab5fd3 364 if (abs(Set_M3) < DeadZoneCMD) Set_M3 = 0;
Arkadi 23:1bcf834fb859 365 // enable stepper drivers
Arkadi 25:716a21ab5fd3 366 if (EN_Stepper_Flag==0) {
Arkadi 25:716a21ab5fd3 367 motor1->Enable();
Arkadi 25:716a21ab5fd3 368 motor2->Enable();
Arkadi 25:716a21ab5fd3 369 motor3->Enable();
Arkadi 23:1bcf834fb859 370 EN_Stepper_Flag=1;
Arkadi 23:1bcf834fb859 371 }
Arkadi 23:1bcf834fb859 372 // update motor speed command
Arkadi 25:716a21ab5fd3 373 CMD_Motor_SPD[0]=Set_M1;
Arkadi 25:716a21ab5fd3 374 CMD_Motor_SPD[1]=Set_M2;
Arkadi 25:716a21ab5fd3 375 CMD_Motor_SPD[2]=Set_M3;
Davidroid 8:cec4c2c03a27 376
Arkadi 26:09a541810137 377 } else { // end velocity control - Start position control
Arkadi 27:8e0acf4ae4fd 378
Arkadi 27:8e0acf4ae4fd 379 // calculate position based on steps:
Arkadi 26:09a541810137 380 // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
Arkadi 26:09a541810137 381 if (Set_M1 > MaxAngle) Set_M1 = MaxAngle;
Arkadi 26:09a541810137 382 if (Set_M1 < -MaxAngle) Set_M1 = -MaxAngle;
Arkadi 26:09a541810137 383
Arkadi 26:09a541810137 384 if (Set_M2 > MaxAngle) Set_M2 = MaxAngle;
Arkadi 26:09a541810137 385 if (Set_M2 < -MaxAngle) Set_M2 = -MaxAngle;
Arkadi 25:716a21ab5fd3 386
Arkadi 26:09a541810137 387 if (Set_M3 > MaxAngle) Set_M3 = MaxAngle;
Arkadi 26:09a541810137 388 if (Set_M3 < -MaxAngle) Set_M3 = -MaxAngle;
Arkadi 26:09a541810137 389 // enable stepper drivers
Arkadi 26:09a541810137 390 if (EN_Stepper_Flag==0) {
Arkadi 26:09a541810137 391 motor1->Enable();
Arkadi 26:09a541810137 392 motor2->Enable();
Arkadi 26:09a541810137 393 motor3->Enable();
Arkadi 26:09a541810137 394 EN_Stepper_Flag=1;
Arkadi 26:09a541810137 395 }
Arkadi 26:09a541810137 396 // update motor speed command
Arkadi 26:09a541810137 397 CMD_Motor_Pos[0]=Set_M1;
Arkadi 26:09a541810137 398 CMD_Motor_Pos[1]=Set_M2;
Arkadi 26:09a541810137 399 CMD_Motor_Pos[2]=Set_M3;
Arkadi 27:8e0acf4ae4fd 400
Arkadi 25:716a21ab5fd3 401 }// end position control
Arkadi 25:716a21ab5fd3 402 }// End Platform Motion Set
Arkadi 25:716a21ab5fd3 403
Arkadi 25:716a21ab5fd3 404 // Platform Motion Control
Arkadi 25:716a21ab5fd3 405 void Platform_Motion_Control()
Arkadi 23:1bcf834fb859 406 {
Arkadi 23:1bcf834fb859 407 // variable limits: (-100>SPD_M>100)
Arkadi 25:716a21ab5fd3 408 static const float MaxSPD=0.5f; // rounds per second
Arkadi 26:09a541810137 409 static const float DeadZone=0.0001f;
Arkadi 25:716a21ab5fd3 410
Arkadi 25:716a21ab5fd3 411 // update max acceleration calculation !!!!!!
Arkadi 25:716a21ab5fd3 412 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 413
Arkadi 25:716a21ab5fd3 414 static float SetMotorSPD[3]= {0}; // the actual command set frequency in Hz
Arkadi 27:8e0acf4ae4fd 415
Arkadi 26:09a541810137 416 // calculate motor pos:
Arkadi 26:09a541810137 417 Motor_Pos[0]= (((float)Motor_Steps[0]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
Arkadi 26:09a541810137 418 Motor_Pos[1]= (((float)Motor_Steps[1]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
Arkadi 26:09a541810137 419 Motor_Pos[2]= (((float)Motor_Steps[2]) / STEPS2ROTATION) * 360.0f; // [deg] = Motor_Steps / STEPS2ROTATION *360.0f
Arkadi 27:8e0acf4ae4fd 420
Arkadi 26:09a541810137 421 // position control
Arkadi 27:8e0acf4ae4fd 422 if (SpdPos_Flag == 0) {
Arkadi 27:8e0acf4ae4fd 423 // implement control loop based on desired position and current position
Arkadi 27:8e0acf4ae4fd 424 // update velocity commands based on position control loop
Arkadi 27:8e0acf4ae4fd 425 CMD_Motor_SPD[0]=(CMD_Motor_Pos[0]-Motor_Pos[0]) * PROPORTIONAL_GAIN;
Arkadi 27:8e0acf4ae4fd 426 CMD_Motor_SPD[1]=(CMD_Motor_Pos[1]-Motor_Pos[1]) * PROPORTIONAL_GAIN;
Arkadi 27:8e0acf4ae4fd 427 CMD_Motor_SPD[2]=(CMD_Motor_Pos[2]-Motor_Pos[2]) * PROPORTIONAL_GAIN;
Arkadi 27:8e0acf4ae4fd 428
Arkadi 26:09a541810137 429 }
Arkadi 26:09a541810137 430 // update velocity commands
Arkadi 23:1bcf834fb859 431 // implement control loop here: (basic control with max acceleration limit)
Arkadi 23:1bcf834fb859 432 if(1) {
Arkadi 23:1bcf834fb859 433 if (abs(CMD_Motor_SPD[0]-Motor_SPD[0])> MaxACC) {
Arkadi 23:1bcf834fb859 434 CMD_Motor_SPD[0]>Motor_SPD[0] ? Motor_SPD[0]+=MaxACC : Motor_SPD[0]-=MaxACC;
Arkadi 23:1bcf834fb859 435 } else {
Arkadi 23:1bcf834fb859 436 Motor_SPD[0]=CMD_Motor_SPD[0];
Arkadi 23:1bcf834fb859 437 }
Arkadi 23:1bcf834fb859 438 if (abs(CMD_Motor_SPD[1]-Motor_SPD[1])> MaxACC) {
Arkadi 23:1bcf834fb859 439 CMD_Motor_SPD[1]>Motor_SPD[1] ? Motor_SPD[1]+=MaxACC : Motor_SPD[1]-=MaxACC;
Arkadi 23:1bcf834fb859 440 } else {
Arkadi 23:1bcf834fb859 441 Motor_SPD[1]=CMD_Motor_SPD[1];
Arkadi 23:1bcf834fb859 442 }
Arkadi 25:716a21ab5fd3 443 if (abs(CMD_Motor_SPD[2]-Motor_SPD[2])> MaxACC) {
Arkadi 25:716a21ab5fd3 444 CMD_Motor_SPD[2]>Motor_SPD[2] ? Motor_SPD[2]+=MaxACC : Motor_SPD[2]-=MaxACC;
Arkadi 25:716a21ab5fd3 445 } else {
Arkadi 25:716a21ab5fd3 446 Motor_SPD[2]=CMD_Motor_SPD[2];
Arkadi 25:716a21ab5fd3 447 }
Arkadi 25:716a21ab5fd3 448 }
Arkadi 25:716a21ab5fd3 449 // update driver frequency
Arkadi 23:1bcf834fb859 450 if (1) {
Arkadi 25:716a21ab5fd3 451 // Start Moition Control
Arkadi 23:1bcf834fb859 452 // motor 1
Arkadi 25:716a21ab5fd3 453
Arkadi 25:716a21ab5fd3 454 // update driver direction
Arkadi 25:716a21ab5fd3 455 if (Motor_SPD[0]>0) {
Arkadi 25:716a21ab5fd3 456 MOT1Dir.write(1);
Arkadi 25:716a21ab5fd3 457 } else {
Arkadi 25:716a21ab5fd3 458 MOT1Dir.write(0);
Arkadi 25:716a21ab5fd3 459 }
Arkadi 25:716a21ab5fd3 460
Arkadi 25:716a21ab5fd3 461 // check if SPD is higher than minimum value
Arkadi 23:1bcf834fb859 462 if (abs(Motor_SPD[0])<DeadZone) {
Arkadi 25:716a21ab5fd3 463 // disable pulsing, Set speed to zero
Arkadi 25:716a21ab5fd3 464 Motor1_Step_Ticker.detach();
Arkadi 25:716a21ab5fd3 465 SetMotorSPD[0]=0;
Arkadi 25:716a21ab5fd3 466
Arkadi 23:1bcf834fb859 467 } else {
Arkadi 25:716a21ab5fd3 468 // Set Pulse rate based on pulses per second
Arkadi 25:716a21ab5fd3 469 SetMotorSPD[0]=(abs(Motor_SPD[0])*STEPS2ROTATION);
Arkadi 25:716a21ab5fd3 470 if (SetMotorSPD[0]>STEPS2ROTATION*MaxSPD) SetMotorSPD[1]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
Arkadi 25:716a21ab5fd3 471 if (SetMotorSPD[0]<1) SetMotorSPD[0]=1; // make sure minimum frequency
Arkadi 25:716a21ab5fd3 472 Motor1_Step_Ticker.attach(&Motor1_Step_Control, (0.5f/(SetMotorSPD[0])));
Arkadi 23:1bcf834fb859 473 }
Arkadi 25:716a21ab5fd3 474
Arkadi 25:716a21ab5fd3 475 // motor 2
Arkadi 25:716a21ab5fd3 476
Arkadi 25:716a21ab5fd3 477 // update driver direction
Arkadi 25:716a21ab5fd3 478 if (Motor_SPD[1]>0) {
Arkadi 25:716a21ab5fd3 479 MOT2Dir.write(1);
Arkadi 23:1bcf834fb859 480 } else {
Arkadi 25:716a21ab5fd3 481 MOT2Dir.write(0);
Arkadi 25:716a21ab5fd3 482 }
Arkadi 25:716a21ab5fd3 483
Arkadi 25:716a21ab5fd3 484 // check if SPD is higher than minimum value
Arkadi 25:716a21ab5fd3 485 if (abs(Motor_SPD[1])<DeadZone) {
Arkadi 25:716a21ab5fd3 486 // disable pulsing, Set speed to zero
Arkadi 25:716a21ab5fd3 487 Motor2_Step_Ticker.detach();
Arkadi 25:716a21ab5fd3 488 SetMotorSPD[1]=0;
Arkadi 25:716a21ab5fd3 489
Arkadi 25:716a21ab5fd3 490 } else {
Arkadi 25:716a21ab5fd3 491 // Set Pulse rate based on pulses per second
Arkadi 25:716a21ab5fd3 492 SetMotorSPD[1]=(abs(Motor_SPD[1])*STEPS2ROTATION);
Arkadi 25:716a21ab5fd3 493 if (SetMotorSPD[1]>STEPS2ROTATION*MaxSPD) SetMotorSPD[1]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
Arkadi 25:716a21ab5fd3 494 if (SetMotorSPD[1]<1) SetMotorSPD[1]=1; // make sure minimum frequency
Arkadi 25:716a21ab5fd3 495 Motor2_Step_Ticker.attach(&Motor2_Step_Control, (0.5f/(SetMotorSPD[1])));
Arkadi 23:1bcf834fb859 496 }
Arkadi 25:716a21ab5fd3 497
Arkadi 25:716a21ab5fd3 498 // Motor 3
Arkadi 25:716a21ab5fd3 499 // update driver direction
Arkadi 25:716a21ab5fd3 500 if (Motor_SPD[2]>0) {
Arkadi 25:716a21ab5fd3 501 MOT3Dir.write(1);
Arkadi 23:1bcf834fb859 502 } else {
Arkadi 25:716a21ab5fd3 503 MOT3Dir.write(0);
Arkadi 25:716a21ab5fd3 504 }
Arkadi 25:716a21ab5fd3 505
Arkadi 25:716a21ab5fd3 506 // check if SPD is higher than minimum value
Arkadi 25:716a21ab5fd3 507 if (abs(Motor_SPD[2])<DeadZone) {
Arkadi 25:716a21ab5fd3 508 // disable pulsing, Set speed to zero
Arkadi 25:716a21ab5fd3 509 Motor3_Step_Ticker.detach();
Arkadi 25:716a21ab5fd3 510 SetMotorSPD[2]=0;
Arkadi 25:716a21ab5fd3 511
Arkadi 25:716a21ab5fd3 512 } else {
Arkadi 25:716a21ab5fd3 513 // Set Pulse rate based on pulses per second
Arkadi 25:716a21ab5fd3 514 SetMotorSPD[2]=(abs(Motor_SPD[2])*STEPS2ROTATION);
Arkadi 25:716a21ab5fd3 515 if (SetMotorSPD[2]>STEPS2ROTATION*MaxSPD) SetMotorSPD[2]=STEPS2ROTATION*MaxSPD; // make sure max speed is implemented
Arkadi 25:716a21ab5fd3 516 if (SetMotorSPD[2]<1) SetMotorSPD[2]=1; // make sure minimum frequency
Arkadi 25:716a21ab5fd3 517 Motor3_Step_Ticker.attach(&Motor3_Step_Control, (0.5f/(SetMotorSPD[2])));
Arkadi 23:1bcf834fb859 518 }
Arkadi 25:716a21ab5fd3 519 }
Arkadi 25:716a21ab5fd3 520 #ifdef DEBUG_MSG
Arkadi 27:8e0acf4ae4fd 521 //pc.printf("SPD: %.3f ,%.3f,%.3f \r\n" ,SetMotorSPD[0],SetMotorSPD[1],SetMotorSPD[2]); // debug check/
Arkadi 27:8e0acf4ae4fd 522 led = !led;
Arkadi 25:716a21ab5fd3 523 #endif /* DEBUG_MSG */
Arkadi 26:09a541810137 524 // update position
Arkadi 27:8e0acf4ae4fd 525 Pos_Update_Flag=1;
Arkadi 25:716a21ab5fd3 526 }// End Platform Motion Control
Arkadi 25:716a21ab5fd3 527
Arkadi 25:716a21ab5fd3 528
Arkadi 25:716a21ab5fd3 529 // Motor 1 Ticker step control
Arkadi 25:716a21ab5fd3 530 void Motor1_Step_Control()
Arkadi 25:716a21ab5fd3 531 {
Arkadi 25:716a21ab5fd3 532 MOT1Step=!MOT1Step;
Arkadi 25:716a21ab5fd3 533 if (MOT1Step) {
Arkadi 27:8e0acf4ae4fd 534 MOT1Dir ? Motor_Steps[0]++ : Motor_Steps[0]--;
Davidroid 0:e6a49a092e2a 535 }
Arkadi 25:716a21ab5fd3 536 }// end motor 1 Step control
Arkadi 25:716a21ab5fd3 537
Arkadi 25:716a21ab5fd3 538 // Motor 2 Ticker step control
Arkadi 25:716a21ab5fd3 539 void Motor2_Step_Control()
Arkadi 25:716a21ab5fd3 540 {
Arkadi 25:716a21ab5fd3 541 MOT2Step=!MOT2Step;
Arkadi 25:716a21ab5fd3 542 if (MOT2Step) {
Arkadi 27:8e0acf4ae4fd 543 MOT2Dir ? Motor_Steps[1]++ : Motor_Steps[1]--;
Arkadi 25:716a21ab5fd3 544 }
Arkadi 25:716a21ab5fd3 545 }// end motor 2 Step control
Arkadi 25:716a21ab5fd3 546
Arkadi 25:716a21ab5fd3 547 // Motor 3 Ticker step control
Arkadi 25:716a21ab5fd3 548 void Motor3_Step_Control()
Arkadi 25:716a21ab5fd3 549 {
Arkadi 25:716a21ab5fd3 550 MOT3Step=!MOT3Step;
Arkadi 25:716a21ab5fd3 551 if (MOT3Step) {
Arkadi 27:8e0acf4ae4fd 552 MOT3Dir ? Motor_Steps[2]++ : Motor_Steps[2]--;
Arkadi 25:716a21ab5fd3 553 }
Arkadi 25:716a21ab5fd3 554 }// end motor 3 Step control
Arkadi 25:716a21ab5fd3 555