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@23:1bcf834fb859, 2017-05-24 (annotated)

Committer:
Arkadi
Date:
Wed May 24 10:01:41 2017 +0000
Revision:
23:1bcf834fb859
Parent:
21:ed054abddfe4
Child:
25:716a21ab5fd3
Thesis Rotating Platform, Uart 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 23:1bcf834fb859 13 Pinout:
Arkadi 23:1bcf834fb859 14 Nucleo STM32F401RE
Arkadi 23:1bcf834fb859 15 PA_5 --> led (DigitalOut)
Arkadi 23:1bcf834fb859 16
Arkadi 23:1bcf834fb859 17 PC - Serial 2
Arkadi 23:1bcf834fb859 18 PA_2 (Tx) --> STLINK
Arkadi 23:1bcf834fb859 19 PA_3 (Rx) --> STLINK
Arkadi 23:1bcf834fb859 20
Arkadi 23:1bcf834fb859 21 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 22 SPI:
Arkadi 23:1bcf834fb859 23 PA_7 (D11) --> mosi
Arkadi 23:1bcf834fb859 24 PA_9 (D12) --> miso
Arkadi 23:1bcf834fb859 25 PA_8 (D13) --> sclk
Arkadi 23:1bcf834fb859 26
Arkadi 23:1bcf834fb859 27 Motor 1
Arkadi 23:1bcf834fb859 28 PA_10(D2) --> flag_irq (DigitalOut)
Arkadi 23:1bcf834fb859 29 PA_9 (D8) --> Standby (DigitalOut)
Arkadi 23:1bcf834fb859 30 PA_8 (D7) --> MOT1Dir (DigitalOut)
Arkadi 23:1bcf834fb859 31 PC_7 (D9) --> MOT1Step (PWM)
Arkadi 23:1bcf834fb859 32 PB_6 (D10)--> ssel (DigitalOut)
Arkadi 23:1bcf834fb859 33
Arkadi 23:1bcf834fb859 34 Motor 2
Arkadi 23:1bcf834fb859 35 PA_10(D2) --> flag_irq (DigitalOut)
Arkadi 23:1bcf834fb859 36 PA_9 (D8) --> Standby (DigitalOut)
Arkadi 23:1bcf834fb859 37 PB_5 (D4) --> MOT2Dir (DigitalOut)
Arkadi 23:1bcf834fb859 38 PB_3 (D3) --> MOT2Step (PWM)
Arkadi 23:1bcf834fb859 39 PB_6 (D10)--> ssel (DigitalOut)
Arkadi 23:1bcf834fb859 40
Arkadi 23:1bcf834fb859 41 Motor 3
Arkadi 23:1bcf834fb859 42 PA_10(D2) --> flag_irq (DigitalOut)
Arkadi 23:1bcf834fb859 43 PA_9 (D8) --> Standby (DigitalOut)
Arkadi 23:1bcf834fb859 44 PB_4 (D5) --> MOT3Dir (DigitalOut)
Arkadi 23:1bcf834fb859 45 PB_10(D6) --> MOT3Step (PWM)
Arkadi 23:1bcf834fb859 46 PB_6 (D10)--> ssel (DigitalOut)
Davidroid 0:e6a49a092e2a 47
Davidroid 0:e6a49a092e2a 48
Arkadi 23:1bcf834fb859 49 */
Davidroid 0:e6a49a092e2a 50
Arkadi 23:1bcf834fb859 51 ///////////////
Arkadi 23:1bcf834fb859 52 // Libraries //
Arkadi 23:1bcf834fb859 53 ///////////////
Arkadi 23:1bcf834fb859 54 #include "mbed.h"
Arkadi 23:1bcf834fb859 55 #include "BufferedSerial.h" // solves issues of loosing data. alternative doing it yourself
Arkadi 23:1bcf834fb859 56 #include "FastPWM.h" // high frequency pwm with good resolution
Arkadi 23:1bcf834fb859 57 #include "l6474_class.h" // stepper library
Davidroid 0:e6a49a092e2a 58
Davidroid 0:e6a49a092e2a 59
Arkadi 23:1bcf834fb859 60 ///////////////
Arkadi 23:1bcf834fb859 61 // #defines //
Arkadi 23:1bcf834fb859 62 ///////////////
Arkadi 23:1bcf834fb859 63 #define DEBUG_MSG
Arkadi 23:1bcf834fb859 64 #define Motor_Control_Interval 2000 // 500Hz
Arkadi 23:1bcf834fb859 65 #define TimeoutCommand 2000 // 2 second (ms units)
Arkadi 23:1bcf834fb859 66 #define STEPS2ROTATION 3200.0f // Number of steps for rotation at 16 microstepping
Davidroid 0:e6a49a092e2a 67
Davidroid 0:e6a49a092e2a 68
Arkadi 23:1bcf834fb859 69 /////////////
Arkadi 23:1bcf834fb859 70 // Objects //
Arkadi 23:1bcf834fb859 71 /////////////
Arkadi 23:1bcf834fb859 72
Arkadi 23:1bcf834fb859 73 // X-NUCLEO-IHM01A1
Arkadi 23:1bcf834fb859 74 DigitalOut MOT1Dir(D7);
Arkadi 23:1bcf834fb859 75 DigitalOut MOT1Step(D9);
Arkadi 23:1bcf834fb859 76 DigitalOut MOT2Dir(D4);
Arkadi 23:1bcf834fb859 77 DigitalOut MOT2Step(D3);
Arkadi 23:1bcf834fb859 78 DigitalOut MOT3Dir(D5);
Arkadi 23:1bcf834fb859 79 DigitalOut MOT3Step(D6);
Davidroid 0:e6a49a092e2a 80
Davidroid 0:e6a49a092e2a 81 /* Motor Control Component. */
Davidroid 3:02d9ec4f88b2 82 L6474 *motor1;
Davidroid 3:02d9ec4f88b2 83 L6474 *motor2;
Arkadi 21:ed054abddfe4 84 L6474 *motor3;
Davidroid 0:e6a49a092e2a 85
Arkadi 23:1bcf834fb859 86 // Led
Arkadi 23:1bcf834fb859 87 DigitalOut led(LED1);
Arkadi 23:1bcf834fb859 88
Arkadi 23:1bcf834fb859 89 // serial
Arkadi 23:1bcf834fb859 90 BufferedSerial pc(USBTX, USBRX);
Arkadi 23:1bcf834fb859 91
Arkadi 23:1bcf834fb859 92 // Define Ticker for Motor Motion Control Ticker
Arkadi 23:1bcf834fb859 93 Ticker Platform_Control_Ticker;
Arkadi 23:1bcf834fb859 94
Arkadi 23:1bcf834fb859 95 // Define Ticker for Steps control
Arkadi 23:1bcf834fb859 96 Ticker Motor1_Step_Ticker;
Arkadi 23:1bcf834fb859 97 Ticker Motor2_Step_Ticker;
Arkadi 23:1bcf834fb859 98 Ticker Motor3_Step_Ticker;
Arkadi 23:1bcf834fb859 99
Arkadi 23:1bcf834fb859 100 // Timer for clock counter
Arkadi 23:1bcf834fb859 101 Timer time_timer;
Arkadi 23:1bcf834fb859 102
Arkadi 23:1bcf834fb859 103
Arkadi 23:1bcf834fb859 104 ///////////////
Arkadi 23:1bcf834fb859 105 // variables //
Arkadi 23:1bcf834fb859 106 ///////////////
Arkadi 23:1bcf834fb859 107
Arkadi 23:1bcf834fb859 108 // Driver Flag status, enabled / disabled
Arkadi 23:1bcf834fb859 109 bool EN_Stepper_Flag=0;
Arkadi 23:1bcf834fb859 110
Arkadi 23:1bcf834fb859 111 // Motor Speed control
Arkadi 23:1bcf834fb859 112 volatile float CMD_Motor_SPD[3]= {0};
Arkadi 23:1bcf834fb859 113 volatile float Motor_SPD[3]= {0};
Davidroid 0:e6a49a092e2a 114
Arkadi 23:1bcf834fb859 115 // Motor Position control
Arkadi 23:1bcf834fb859 116 volatile float CMD_Motor_Pos[3]= {0};
Arkadi 23:1bcf834fb859 117 volatile float Motor_Pos[3]= {0};
Arkadi 23:1bcf834fb859 118
Arkadi 23:1bcf834fb859 119 // timeout command time stamp
Arkadi 23:1bcf834fb859 120 volatile int CMDTimeStamp=0;
Arkadi 23:1bcf834fb859 121
Arkadi 23:1bcf834fb859 122 ///////////////
Arkadi 23:1bcf834fb859 123 // Functions //
Arkadi 23:1bcf834fb859 124 ///////////////
Arkadi 23:1bcf834fb859 125
Arkadi 23:1bcf834fb859 126 // Incoming Message parser
Arkadi 23:1bcf834fb859 127 void Parse_Message(char inbyte);
Arkadi 23:1bcf834fb859 128
Arkadi 23:1bcf834fb859 129 // Init Platform
Arkadi 23:1bcf834fb859 130 void Platform_Init();
Arkadi 23:1bcf834fb859 131
Arkadi 23:1bcf834fb859 132 // Platform Motion Set
Arkadi 23:1bcf834fb859 133 void Platform_Motion_Set(float Set_SPD_M1, float Set_SPD_M2, float Set_SPD_M3);
Arkadi 23:1bcf834fb859 134
Arkadi 23:1bcf834fb859 135 // Platform Motion Control
Arkadi 23:1bcf834fb859 136 void Platform_Motion_Control();
Arkadi 23:1bcf834fb859 137
Arkadi 23:1bcf834fb859 138 // Motor1 Step Control
Arkadi 23:1bcf834fb859 139 void Motor1_Step_Control();
Arkadi 23:1bcf834fb859 140
Arkadi 23:1bcf834fb859 141 // Motor2 Step Control
Arkadi 23:1bcf834fb859 142 void Motor2_Step_Control();
Arkadi 23:1bcf834fb859 143
Arkadi 23:1bcf834fb859 144 // Motor3 Step Control
Arkadi 23:1bcf834fb859 145 void Motor3_Step_Control();
Arkadi 23:1bcf834fb859 146
Arkadi 23:1bcf834fb859 147 ////////////////////////
Arkadi 23:1bcf834fb859 148 // Main Code Setup : //
Arkadi 23:1bcf834fb859 149 ////////////////////////
Davidroid 0:e6a49a092e2a 150 int main()
Davidroid 0:e6a49a092e2a 151 {
Arkadi 23:1bcf834fb859 152 //Initializing SPI bus.
Davidroid 0:e6a49a092e2a 153 DevSPI dev_spi(D11, D12, D13);
Arkadi 23:1bcf834fb859 154
Arkadi 23:1bcf834fb859 155 //Initializing Motor Control Components.
Davidroid 5:a0268a435bb1 156 motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
Davidroid 16:810667a9f31f 157 motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
Arkadi 23:1bcf834fb859 158 motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);
Arkadi 23:1bcf834fb859 159
Arkadi 23:1bcf834fb859 160 // Setup serial
Arkadi 23:1bcf834fb859 161 pc.baud(57600);
Arkadi 23:1bcf834fb859 162
Arkadi 23:1bcf834fb859 163 // Init shika shuka
Arkadi 23:1bcf834fb859 164 Platform_Init();
Arkadi 23:1bcf834fb859 165
Arkadi 23:1bcf834fb859 166 // initil time timer:
Arkadi 23:1bcf834fb859 167 time_timer.start();
Arkadi 23:1bcf834fb859 168
Arkadi 23:1bcf834fb859 169 ///////////////////////
Arkadi 23:1bcf834fb859 170 // Main Code Loop : //
Arkadi 23:1bcf834fb859 171 ///////////////////////
Arkadi 23:1bcf834fb859 172 while(1) {
Arkadi 23:1bcf834fb859 173 // loop time stamp
Arkadi 23:1bcf834fb859 174 int Timer_TimeStamp_ms=time_timer.read_ms();
Arkadi 23:1bcf834fb859 175 static int ADC_Read_time=0;
Arkadi 23:1bcf834fb859 176
Arkadi 23:1bcf834fb859 177 // receive Motor Command
Arkadi 23:1bcf834fb859 178 while (InMSG.readable()) {
Arkadi 23:1bcf834fb859 179 char InChar=InMSG.getc();
Arkadi 23:1bcf834fb859 180 //pc.printf("%c" ,InChar); // debug check/
Arkadi 23:1bcf834fb859 181 Parse_Message(InChar);
Arkadi 23:1bcf834fb859 182 }//end serial
Arkadi 23:1bcf834fb859 183
Arkadi 23:1bcf834fb859 184 // set time out on commad and stop motion
Arkadi 23:1bcf834fb859 185 if (abs(Timer_TimeStamp_ms-CMDTimeStamp)>TimeoutCommand){
Arkadi 23:1bcf834fb859 186 #ifdef DEBUG_MSG
Arkadi 23:1bcf834fb859 187 // pc.printf("Timer_TimeStamp_ms %d CMDTimeStamp %d\r\n", Timer_TimeStamp_ms,CMDTimeStamp);
Arkadi 23:1bcf834fb859 188 #endif /* DEBUG_MSG */
Arkadi 23:1bcf834fb859 189 CMDTimeStamp=Timer_TimeStamp_ms;
Arkadi 23:1bcf834fb859 190 PitchMotor->Disable();
Arkadi 23:1bcf834fb859 191 RollMotor->Disable();
Arkadi 23:1bcf834fb859 192 CMD_Motor_SPD[0]=0;
Arkadi 23:1bcf834fb859 193 CMD_Motor_SPD[1]=0;
Arkadi 23:1bcf834fb859 194 Motor_SPD[0]=0;
Arkadi 23:1bcf834fb859 195 Motor_SPD[1]=0;
Arkadi 23:1bcf834fb859 196 EN_Stepper_Flag=0;
Arkadi 23:1bcf834fb859 197 }
Arkadi 23:1bcf834fb859 198
Arkadi 23:1bcf834fb859 199 // read ADC value
Arkadi 23:1bcf834fb859 200 if (abs(Timer_TimeStamp_ms-ADC_Read_time)>VccReadDelay) {
Arkadi 23:1bcf834fb859 201 ADC_Read_time=Timer_TimeStamp_ms;
Arkadi 23:1bcf834fb859 202 // LPF on value
Arkadi 23:1bcf834fb859 203 float ReadVoltage=Vcc_11.read();
Arkadi 23:1bcf834fb859 204 ReadVoltage=ReadVoltage* 3.3f*11.0f; // 1/11 voltage divider
Arkadi 23:1bcf834fb859 205
Arkadi 23:1bcf834fb859 206 static float dt_VCC_Read=((float)VccReadDelay)/1000.0f;
Arkadi 23:1bcf834fb859 207 static float Alpha_LPF=dt_VCC_Read/(1.0f+dt_VCC_Read) ; // α := dt / (RC + dt)
Arkadi 23:1bcf834fb859 208
Arkadi 23:1bcf834fb859 209 // LPF: y[i] := y[i-1] + α * (x[i] - y[i-1])
Arkadi 23:1bcf834fb859 210 VCC_Voltage = VCC_Voltage + Alpha_LPF * (ReadVoltage - VCC_Voltage);
Arkadi 23:1bcf834fb859 211 // disable motion if voltage too low
Arkadi 23:1bcf834fb859 212 if (VCC_Voltage<VCC_Thresh) {
Arkadi 23:1bcf834fb859 213 PitchMotor->Disable();
Arkadi 23:1bcf834fb859 214 RollMotor->Disable();
Arkadi 23:1bcf834fb859 215 Motor_SPD[0]=0;
Arkadi 23:1bcf834fb859 216 Motor_SPD[1]=0;
Arkadi 23:1bcf834fb859 217 EN_Stepper_Flag=0;
Arkadi 23:1bcf834fb859 218 }
Arkadi 23:1bcf834fb859 219
Arkadi 23:1bcf834fb859 220 #ifdef DEBUG_MSG
Arkadi 23:1bcf834fb859 221 //pc.printf("CMD: %.3f ,%.3f ,%.3f ,%.3f \r\n" ,CMD_Values[0],CMD_Values[1],CMD_Values[2],CMD_Values[3]); // debug check/
Arkadi 23:1bcf834fb859 222 //pc.printf("VCC = %.2f LPF_V %.2f V \r\n", VCC_Voltage,ReadVoltage);
Arkadi 23:1bcf834fb859 223
Arkadi 23:1bcf834fb859 224 #endif /* DEBUG_MSG */
Arkadi 23:1bcf834fb859 225 }
Arkadi 23:1bcf834fb859 226 }// End Main Loop
Arkadi 23:1bcf834fb859 227 }// End Main
Arkadi 23:1bcf834fb859 228
Arkadi 23:1bcf834fb859 229
Arkadi 23:1bcf834fb859 230 ///////////////
Arkadi 23:1bcf834fb859 231 // Functions //
Arkadi 23:1bcf834fb859 232 ///////////////
Arkadi 23:1bcf834fb859 233 // Incoming Message parser Format: "$<SPD_M1>,<SPD_M2>,<SPD_YAW>,<PITCH_ANGLE>\r\n" // up to /r/n
Arkadi 23:1bcf834fb859 234 void Parse_Message(char inbyte)
Arkadi 23:1bcf834fb859 235 {
Arkadi 23:1bcf834fb859 236 static const uint16_t BufferCMDSize=32;
Arkadi 23:1bcf834fb859 237 static const uint16_t BufferCMD_ValuesSize=4;
Arkadi 23:1bcf834fb859 238
Arkadi 23:1bcf834fb859 239 static float CMD_Values[BufferCMD_ValuesSize]= {0};
Arkadi 23:1bcf834fb859 240 static char BufferCMD[BufferCMDSize]= {0};
Arkadi 23:1bcf834fb859 241 static uint16_t BufferIndex=0;
Arkadi 23:1bcf834fb859 242 static uint16_t Values_Index=0;
Arkadi 23:1bcf834fb859 243
Arkadi 23:1bcf834fb859 244 BufferIndex=BufferIndex%(BufferCMDSize); // simple overflow handler
Arkadi 23:1bcf834fb859 245 Values_Index=Values_Index%(BufferCMD_ValuesSize); // simple overflow handler
Arkadi 23:1bcf834fb859 246
Arkadi 23:1bcf834fb859 247 BufferCMD[BufferIndex]=inbyte;
Arkadi 23:1bcf834fb859 248 BufferIndex++;
Arkadi 23:1bcf834fb859 249
Arkadi 23:1bcf834fb859 250 // parse incoming message
Arkadi 23:1bcf834fb859 251 if (inbyte=='$') { // start of message
Arkadi 23:1bcf834fb859 252 BufferIndex=0; // initialize to start of parser
Arkadi 23:1bcf834fb859 253 Values_Index=0; // index for values position
Arkadi 23:1bcf834fb859 254 } else if (inbyte==',') { // seperator char
Arkadi 23:1bcf834fb859 255 CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values
Arkadi 23:1bcf834fb859 256 BufferIndex=0; // initialize to start of parser
Arkadi 23:1bcf834fb859 257 Values_Index++;
Arkadi 23:1bcf834fb859 258 } else if(inbyte=='\r') { // end of message // parse message
Arkadi 23:1bcf834fb859 259 // Update last value
Arkadi 23:1bcf834fb859 260 CMD_Values[Values_Index]=atof(BufferCMD); // input value to buffer values
Arkadi 23:1bcf834fb859 261
Arkadi 23:1bcf834fb859 262 BufferIndex=0; // initialize to start of parser
Arkadi 23:1bcf834fb859 263 Values_Index=0; // reset values index
Arkadi 23:1bcf834fb859 264
Arkadi 23:1bcf834fb859 265 // set time stamp on time out commad
Arkadi 23:1bcf834fb859 266 CMDTimeStamp=time_timer.read_ms();
Arkadi 23:1bcf834fb859 267 // update command
Arkadi 23:1bcf834fb859 268 ShikaShuka_Motion_Set(CMD_Values[2],CMD_Values[3]);
Arkadi 23:1bcf834fb859 269
Arkadi 23:1bcf834fb859 270 // send out the remaining message for the brushed controller
Arkadi 23:1bcf834fb859 271 OutMSG.printf("$%.3f,%.3f\r\n",CMD_Values[0],CMD_Values[1]);
Arkadi 23:1bcf834fb859 272
Arkadi 23:1bcf834fb859 273 #ifdef DEBUG_MSG
Arkadi 23:1bcf834fb859 274 pc.printf("CMD: %.3f ,%.3f ,%.3f ,%.3f \r\n" ,CMD_Values[0],CMD_Values[1],CMD_Values[2],CMD_Values[3]); // debug check/
Arkadi 23:1bcf834fb859 275 //pc.printf("CMD: %.3f ,%.3f \r\n" ,CMD_Values[0],CMD_Values[1]); // debug check/
Arkadi 23:1bcf834fb859 276 led = !led;
Arkadi 23:1bcf834fb859 277 #endif /* DEBUG_MSG */
Arkadi 23:1bcf834fb859 278
Arkadi 23:1bcf834fb859 279 }//end parser
Arkadi 23:1bcf834fb859 280 }// end parser function
Arkadi 23:1bcf834fb859 281
Arkadi 23:1bcf834fb859 282
Arkadi 23:1bcf834fb859 283 // Init shika shuka
Arkadi 23:1bcf834fb859 284 void Platform_Init()
Arkadi 23:1bcf834fb859 285 {
Arkadi 23:1bcf834fb859 286 //Initializing Motor Control Components.
Davidroid 14:fcd452b03d1a 287 if (motor1->Init() != COMPONENT_OK)
Davidroid 14:fcd452b03d1a 288 exit(EXIT_FAILURE);
Davidroid 14:fcd452b03d1a 289 if (motor2->Init() != COMPONENT_OK)
Davidroid 14:fcd452b03d1a 290 exit(EXIT_FAILURE);
Arkadi 21:ed054abddfe4 291 if (motor3->Init() != COMPONENT_OK)
Arkadi 21:ed054abddfe4 292 exit(EXIT_FAILURE);
Arkadi 23:1bcf834fb859 293
Arkadi 21:ed054abddfe4 294 /*----- Changing motor setting. -----*/
Arkadi 21:ed054abddfe4 295 /* Setting High Impedance State to update L6474's registers. */
Arkadi 21:ed054abddfe4 296 motor1->SoftHiZ();
Arkadi 21:ed054abddfe4 297 motor2->SoftHiZ();
Arkadi 21:ed054abddfe4 298 motor3->SoftHiZ();
Arkadi 21:ed054abddfe4 299 // Disabling motor
Arkadi 21:ed054abddfe4 300 motor1->Disable();
Arkadi 21:ed054abddfe4 301 motor2->Disable();
Arkadi 21:ed054abddfe4 302 motor3->Disable();
Arkadi 21:ed054abddfe4 303 /* Changing step mode. */
Arkadi 21:ed054abddfe4 304 motor1->SetStepMode(STEP_MODE_1_16);
Arkadi 21:ed054abddfe4 305 motor2->SetStepMode(STEP_MODE_1_16);
Arkadi 21:ed054abddfe4 306 motor3->SetStepMode(STEP_MODE_1_16);
Arkadi 21:ed054abddfe4 307
Arkadi 23:1bcf834fb859 308 /* Increasing the torque regulation current. */
Arkadi 23:1bcf834fb859 309 motor1->SetParameter(L6474_TVAL, 1250); // Limit 2.0A
Arkadi 23:1bcf834fb859 310 motor2->SetParameter(L6474_TVAL, 1650); // Limit 1.7A
Arkadi 23:1bcf834fb859 311 motor3->SetParameter(L6474_TVAL, 250); // Limit 0.28A
Arkadi 23:1bcf834fb859 312
Arkadi 23:1bcf834fb859 313 /* Increasing the max threshold overcurrent. */
Arkadi 23:1bcf834fb859 314 motor1->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA);
Arkadi 23:1bcf834fb859 315 motor2->SetParameter(L6474_OCD_TH, L6474_OCD_TH_2625mA);
Arkadi 23:1bcf834fb859 316 motor3->SetParameter(L6474_OCD_TH, L6474_OCD_TH_750mA);
Arkadi 21:ed054abddfe4 317
Arkadi 21:ed054abddfe4 318 // Enabling motor
Arkadi 21:ed054abddfe4 319 motor1->Enable();
Arkadi 21:ed054abddfe4 320 motor2->Enable();
Arkadi 21:ed054abddfe4 321 motor3->Enable();
Arkadi 23:1bcf834fb859 322
Arkadi 23:1bcf834fb859 323 // Initialize Control Pins
Arkadi 23:1bcf834fb859 324 MOT1Dir.write(0);
Arkadi 23:1bcf834fb859 325 MOT1Step.write(0);
Arkadi 23:1bcf834fb859 326 MOT2Dir.write(0);
Arkadi 23:1bcf834fb859 327 MOT2Step.write(0);
Arkadi 23:1bcf834fb859 328 MOT3Dir.write(0);
Arkadi 23:1bcf834fb859 329 MOT3Step.write(0);
Davidroid 0:e6a49a092e2a 330
Arkadi 23:1bcf834fb859 331 // Start Moition Control
Arkadi 23:1bcf834fb859 332 Platform_Control_Ticker.attach_us(&ShikaShuka_Motion_Control, ShikaShuka_Control_Interval);
Arkadi 21:ed054abddfe4 333
Arkadi 23:1bcf834fb859 334 }// End Init shika shuka
Davidroid 8:cec4c2c03a27 335
Arkadi 23:1bcf834fb859 336 // ShikaShuka Motion Set
Arkadi 23:1bcf834fb859 337 void ShikaShuka_Motion_Set(float Set_SPD_M1, float Set_SPD_M2)
Arkadi 23:1bcf834fb859 338 {
Arkadi 23:1bcf834fb859 339 static const float MaxSPDCMD=5.0f;
Arkadi 23:1bcf834fb859 340 static const float DeadZoneCMD=0.001f;
Arkadi 23:1bcf834fb859 341
Arkadi 23:1bcf834fb859 342 // variable limits: (-MaxSPDCMD>SPD_M>MaxSPDCMD)
Arkadi 23:1bcf834fb859 343 if (Set_SPD_M1 > MaxSPDCMD) Set_SPD_M1 = MaxSPDCMD;
Arkadi 23:1bcf834fb859 344 if (Set_SPD_M1 < -MaxSPDCMD) Set_SPD_M1 = -MaxSPDCMD;
Arkadi 23:1bcf834fb859 345 if (abs(Set_SPD_M1) < DeadZoneCMD) Set_SPD_M1 = 0;
Davidroid 8:cec4c2c03a27 346
Arkadi 23:1bcf834fb859 347 if (Set_SPD_M2 > MaxSPDCMD) Set_SPD_M2 = MaxSPDCMD;
Arkadi 23:1bcf834fb859 348 if (Set_SPD_M2 < -MaxSPDCMD) Set_SPD_M2 = -MaxSPDCMD;
Arkadi 23:1bcf834fb859 349 if (abs(Set_SPD_M2) < DeadZoneCMD) Set_SPD_M2 = 0;
Arkadi 23:1bcf834fb859 350 // verify voltage level:
Arkadi 23:1bcf834fb859 351 if (VCC_Voltage>VCC_Thresh) {
Arkadi 23:1bcf834fb859 352 // enable stepper drivers
Arkadi 23:1bcf834fb859 353 if (EN_Stepper_Flag==0){
Arkadi 23:1bcf834fb859 354 PitchMotor->Enable();
Arkadi 23:1bcf834fb859 355 RollMotor->Enable();
Arkadi 23:1bcf834fb859 356 EN_Stepper_Flag=1;
Arkadi 23:1bcf834fb859 357 }
Arkadi 23:1bcf834fb859 358 // update motor speed command
Arkadi 23:1bcf834fb859 359 CMD_Motor_SPD[0]=Set_SPD_M1;
Arkadi 23:1bcf834fb859 360 CMD_Motor_SPD[1]=Set_SPD_M2;
Arkadi 23:1bcf834fb859 361 } else {
Arkadi 23:1bcf834fb859 362 // disable motion if voltage too low
Arkadi 23:1bcf834fb859 363 if (EN_Stepper_Flag==1){
Arkadi 23:1bcf834fb859 364 PitchMotor->Disable();
Arkadi 23:1bcf834fb859 365 RollMotor->Disable();
Arkadi 23:1bcf834fb859 366 Motor_SPD[0]=0;
Arkadi 23:1bcf834fb859 367 Motor_SPD[1]=0;
Arkadi 23:1bcf834fb859 368 EN_Stepper_Flag=0;
Arkadi 23:1bcf834fb859 369 }
Arkadi 23:1bcf834fb859 370 CMD_Motor_SPD[0]=0;
Arkadi 23:1bcf834fb859 371 CMD_Motor_SPD[1]=0;
Arkadi 23:1bcf834fb859 372 }
Arkadi 23:1bcf834fb859 373 }// End ShikaShuka Motion Set
Davidroid 8:cec4c2c03a27 374
Arkadi 23:1bcf834fb859 375 // ShikaShuka Motion Control
Arkadi 23:1bcf834fb859 376 void ShikaShuka_Motion_Control()
Arkadi 23:1bcf834fb859 377 {
Arkadi 23:1bcf834fb859 378 // variable limits: (-100>SPD_M>100)
Arkadi 23:1bcf834fb859 379 static const float MaxSPD=5.0f;
Arkadi 23:1bcf834fb859 380 static const float DeadZone=0.001f;
Arkadi 23:1bcf834fb859 381 static const float MaxACC=0.5f/(1000000/ShikaShuka_Control_Interval); //acceleration set as val/sec here for open loop it is %/sec
Davidroid 8:cec4c2c03a27 382
Arkadi 23:1bcf834fb859 383 static float SetMotorSPDPWM[2]= {0}; // the actual command set frequency in Hz
Arkadi 23:1bcf834fb859 384
Arkadi 23:1bcf834fb859 385 // implement control loop here: (basic control with max acceleration limit)
Arkadi 23:1bcf834fb859 386 if(1) {
Arkadi 23:1bcf834fb859 387 if (abs(CMD_Motor_SPD[0]-Motor_SPD[0])> MaxACC) {
Arkadi 23:1bcf834fb859 388 CMD_Motor_SPD[0]>Motor_SPD[0] ? Motor_SPD[0]+=MaxACC : Motor_SPD[0]-=MaxACC;
Arkadi 23:1bcf834fb859 389 } else {
Arkadi 23:1bcf834fb859 390 Motor_SPD[0]=CMD_Motor_SPD[0];
Arkadi 23:1bcf834fb859 391 }
Arkadi 23:1bcf834fb859 392 if (abs(CMD_Motor_SPD[1]-Motor_SPD[1])> MaxACC) {
Arkadi 23:1bcf834fb859 393 CMD_Motor_SPD[1]>Motor_SPD[1] ? Motor_SPD[1]+=MaxACC : Motor_SPD[1]-=MaxACC;
Arkadi 23:1bcf834fb859 394 } else {
Arkadi 23:1bcf834fb859 395 Motor_SPD[1]=CMD_Motor_SPD[1];
Arkadi 23:1bcf834fb859 396 }
Arkadi 23:1bcf834fb859 397 }
Arkadi 23:1bcf834fb859 398 // update driver PWM frequency
Arkadi 23:1bcf834fb859 399 if (1) {
Arkadi 23:1bcf834fb859 400 // motor 1
Arkadi 23:1bcf834fb859 401 if (abs(Motor_SPD[0])<DeadZone) {
Arkadi 23:1bcf834fb859 402 SetMotorSPDPWM[0]=1;
Arkadi 23:1bcf834fb859 403 PitchStep.write(0); // disable pulsing, euqal to stop
Arkadi 23:1bcf834fb859 404 } else {
Arkadi 23:1bcf834fb859 405 // Set Pulse rate based on pulses per second
Arkadi 23:1bcf834fb859 406 SetMotorSPDPWM[0]=(abs(Motor_SPD[0])*STEPS2ROTATION); // rotation per second, to pulses with duty cycle of 0.5
Arkadi 23:1bcf834fb859 407 PitchStep.write(0.5f); // enable pulsing
Arkadi 23:1bcf834fb859 408 }
Arkadi 23:1bcf834fb859 409 // update driver PWM based on direction
Arkadi 23:1bcf834fb859 410 if (SetMotorSPDPWM[0]>STEPS2ROTATION*MaxSPD) SetMotorSPDPWM[0]=STEPS2ROTATION*MaxSPD; // make sure pwm command is trimmed
Arkadi 23:1bcf834fb859 411 if (SetMotorSPDPWM[0]<1) SetMotorSPDPWM[0]=1; // make sure pwm command is trimmed
Arkadi 23:1bcf834fb859 412
Arkadi 23:1bcf834fb859 413 if (Motor_SPD[0]>0) {
Arkadi 23:1bcf834fb859 414 PitchDir.write(1);
Arkadi 23:1bcf834fb859 415 } else {
Arkadi 23:1bcf834fb859 416 PitchDir.write(0);
Arkadi 23:1bcf834fb859 417 }
Arkadi 23:1bcf834fb859 418 PitchStep.period(1.0f/(SetMotorSPDPWM[0]));
Arkadi 23:1bcf834fb859 419
Arkadi 23:1bcf834fb859 420 // motor 2
Arkadi 23:1bcf834fb859 421 if (abs(Motor_SPD[1])<DeadZone) {
Arkadi 23:1bcf834fb859 422 SetMotorSPDPWM[1]=1;
Arkadi 23:1bcf834fb859 423 RollStep.write(0); // disable pulsing, euqal to stop
Arkadi 23:1bcf834fb859 424 } else {
Arkadi 23:1bcf834fb859 425 // Set Pulse rate based on pulses per second
Arkadi 23:1bcf834fb859 426 SetMotorSPDPWM[1]=(abs(Motor_SPD[1])*STEPS2ROTATION); // rotation per second, to pulses with duty cycle of 0.5
Arkadi 23:1bcf834fb859 427 RollStep.write(0.5f); // enable pulsing
Arkadi 23:1bcf834fb859 428 }
Arkadi 23:1bcf834fb859 429 // update driver PWM based on direction
Arkadi 23:1bcf834fb859 430 if (SetMotorSPDPWM[1]>STEPS2ROTATION*MaxSPD) SetMotorSPDPWM[1]=STEPS2ROTATION*MaxSPD; // make sure pwm command is trimmed
Arkadi 23:1bcf834fb859 431 if (SetMotorSPDPWM[1]<1) SetMotorSPDPWM[1]=1; // make sure pwm command is trimmed
Arkadi 23:1bcf834fb859 432
Arkadi 23:1bcf834fb859 433 if (Motor_SPD[1]>0) {
Arkadi 23:1bcf834fb859 434 RollDir.write(1);
Arkadi 23:1bcf834fb859 435 } else {
Arkadi 23:1bcf834fb859 436 RollDir.write(0);
Arkadi 23:1bcf834fb859 437 }
Arkadi 23:1bcf834fb859 438 RollStep.period(1.0f/(SetMotorSPDPWM[1]));
Davidroid 0:e6a49a092e2a 439 }
Arkadi 23:1bcf834fb859 440 //pc.printf("CMD: %.3f ,%.3f \r\n" ,SetMotorSPDPWM[0],SetMotorSPDPWM[1]); // debug check/
Arkadi 23:1bcf834fb859 441 }// End ShikaShuka Motion Control