Thesis Rotating Platform, Uart Control
Dependencies: BufferedSerial X_NUCLEO_IHM01A1_Disabled_Control mbed
Fork of Demo_IHM01A1_3-Motors by
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?
User | Revision | Line number | New 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 |