Stevie Wray / Mbed 2 deprecated SkyFawkes

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@10:63381713610c, 2019-03-03 (annotated)

Committer:
Stevie
Date:
Sun Mar 03 11:22:24 2019 +0000
Revision:
10:63381713610c
Parent:
9:0d274fc2e6df 
Initial tuning complete.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stevie 0:d665d69947ab 1 #include "mbed.h"
Stevie 9:0d274fc2e6df 2
Stevie 4:abf0070897ff 3 //Serial pc(SERIAL_TX, SERIAL_RX);
Stevie 9:0d274fc2e6df 4
Stevie 9:0d274fc2e6df 5
Stevie 1:08315c315df0 6 //Analog inputs A0-A5 addressable as such. A6 to A9 do not work.
Stevie 1:08315c315df0 7 AnalogIn ANALOG0(A0);
Stevie 1:08315c315df0 8 AnalogIn ANALOG1(A1);
Stevie 1:08315c315df0 9 AnalogIn ANALOG2(A2);
Stevie 1:08315c315df0 10 AnalogIn ANALOG3(A3);
Stevie 1:08315c315df0 11 AnalogIn ANALOG4(A4);
Stevie 1:08315c315df0 12 AnalogIn ANALOG5(A5);
Stevie 0:d665d69947ab 13 SPISlave rpi(PB_5, PB_4, PB_3, PA_15); //setup SPI pins to talk to the raspberry pi
Stevie 0:d665d69947ab 14 //PA_9 - MLB //PA_8 - MLF //PA_10 - MRF //PA_11 - MRB
Stevie 0:d665d69947ab 15 PwmOut MRB(PA_11); //back right motor
Stevie 0:d665d69947ab 16 PwmOut MRF(PA_10); //front right motor
Stevie 0:d665d69947ab 17 PwmOut MLB(PA_9); //back left motor
Stevie 0:d665d69947ab 18 PwmOut MLF(PA_8); //front left motor
Stevie 0:d665d69947ab 19 DigitalOut RAIN1(PC_6); //PC_9 - Left //PC_6 - Right
Stevie 0:d665d69947ab 20 DigitalOut RAIN2(PB_9); // PC_8 - Left //PB_9 - Right
Stevie 0:d665d69947ab 21 DigitalOut RSTANDBY(PC_5); //PB_8 - Left //PC_5 - Right
Stevie 0:d665d69947ab 22 DigitalOut LAIN1(PC_9);
Stevie 0:d665d69947ab 23 DigitalOut LAIN2(PC_8);
Stevie 0:d665d69947ab 24 DigitalOut LSTANDBY(PB_8);
Stevie 3:497ac7d026b5 25 //InterruptIn MRB_ENC1(PB_6);
Stevie 3:497ac7d026b5 26 //InterruptIn MRB_ENC2(PC_7);
Stevie 3:497ac7d026b5 27 InterruptIn MRF_ENC1(PA_13);
Stevie 3:497ac7d026b5 28 InterruptIn MRF_ENC2(PB_7);
Stevie 3:497ac7d026b5 29 //InterruptIn MLB_ENC1(PD_2);
Stevie 3:497ac7d026b5 30 //InterruptIn MLB_ENC2(PC_12);
Stevie 3:497ac7d026b5 31 InterruptIn MLF_ENC1(PC_11); //PC_12 - MLB //PC_11 - MLF //PB_7 - MRF //PB_6 - MRB
Stevie 3:497ac7d026b5 32 InterruptIn MLF_ENC2(PC_10); //PD_2 - MLB //PC_10 - MLF //PA_13 - MRF //PC_7 - MRB
Stevie 3:497ac7d026b5 33 Ticker control_timer;
Stevie 9:0d274fc2e6df 34
Stevie 3:497ac7d026b5 35 bool control_loop_flag = false;
Stevie 9:0d274fc2e6df 36
Stevie 3:497ac7d026b5 37 //motor associations within arrays
Stevie 4:abf0070897ff 38 //Right = 0; Left = 1
Stevie 5:600c5c9cbb19 39 int m_count [2] = {0,0}; //setup array for 2 encoder counts
Stevie 5:600c5c9cbb19 40 int m_last_count [2] = {0,0}; //setup array for storing previous encoder counts
Stevie 5:600c5c9cbb19 41 int m_speed_ref [2] = {0,0}; //setup array for 2 motor speeds
Stevie 8:826ec74d53c9 42 int m_top_speed [2] = {0,0};
Stevie 4:abf0070897ff 43 float m_duty [2] = {0.0, 0.0}; //setup array for motor pwm duty
Stevie 5:600c5c9cbb19 44 int m_distance_ref [2] = {0,0}; //setup array for 2 motor distances
Stevie 8:826ec74d53c9 45 int max_accel = 5; //for storing the maximum acceleration
Stevie 1:08315c315df0 46 int adc_values[6] = {0,0,0,0,0,0}; //setup array for ADC values
Stevie 9:0d274fc2e6df 47 int distance_scale = 14;
Stevie 9:0d274fc2e6df 48 //max number over SPI is 100. Scale to make this equivalent to 1m
Stevie 6:a44d6f7626f2 49 //with 48mm diameter wheels and 210 counts per revolution. Circumference = pi*D = 0.1508m per rev.
Stevie 6:a44d6f7626f2 50 //1m/0.1508m = 6.63. 6.63*210 = 1392 counts. So scale to make 100 = 1400 counts
Stevie 9:0d274fc2e6df 51
Stevie 7:948651ca4c0e 52 float integral [2] = {0,0};
Stevie 6:a44d6f7626f2 53 bool control_mode = true; //control mode flag. True is speed control and False is distance control
Stevie 9:0d274fc2e6df 54 //bool m_direction [2] = {true, true};
Stevie 9:0d274fc2e6df 55
Stevie 2:00535b62c344 56 void readADC(); //read 6 channels of ADC
Stevie 2:00535b62c344 57 void setDuty(); //set the 4 motor PWM duty cycles
Stevie 2:00535b62c344 58 void setPeriod(int period); //set PWM period for motors in microseconds
Stevie 2:00535b62c344 59 void setLMotorDir(bool direction); //set left motor direction. 1 for forward, 0 for back.
Stevie 2:00535b62c344 60 void setRMotorDir(bool direction); //set right motor direction. 1 for forward, 0 for back.
Stevie 2:00535b62c344 61 void spiComms(); //do spi communications with raspberry pi
Stevie 9:0d274fc2e6df 62
Stevie 3:497ac7d026b5 63 void mrfEncoderIsr1();
Stevie 3:497ac7d026b5 64 void mrfEncoderIsr2();
Stevie 3:497ac7d026b5 65 void mlfEncoderIsr1();
Stevie 3:497ac7d026b5 66 void mlfEncoderIsr2();
Stevie 5:600c5c9cbb19 67 void controlTick();
Stevie 9:0d274fc2e6df 68 float calcSpeed(float currentPosition, float start, float end, float accel, float topSpeed) ;
Stevie 9:0d274fc2e6df 69
Stevie 9:0d274fc2e6df 70
Stevie 0:d665d69947ab 71 int main()
Stevie 0:d665d69947ab 72 {
Stevie 5:600c5c9cbb19 73 float pos_change = 0; //temp variable for speed control
Stevie 5:600c5c9cbb19 74 float speed_error = 0; //temp variable for speed control
Stevie 9:0d274fc2e6df 75
Stevie 10:63381713610c 76 float Kp = 0.02; //proportional constant
Stevie 10:63381713610c 77 float Ki = 0.05; //integral constant
Stevie 5:600c5c9cbb19 78 int i = 0;
Stevie 9:0d274fc2e6df 79
Stevie 9:0d274fc2e6df 80 // float distance_error = 0;
Stevie 9:0d274fc2e6df 81 // float Kp_d = 0.01;
Stevie 9:0d274fc2e6df 82 // float max_duty = 0;
Stevie 9:0d274fc2e6df 83
Stevie 9:0d274fc2e6df 84
Stevie 3:497ac7d026b5 85 //setup interrupts for encoders
Stevie 3:497ac7d026b5 86 MRF_ENC1.fall(&mrfEncoderIsr1);
Stevie 3:497ac7d026b5 87 MRF_ENC2.fall(&mrfEncoderIsr2);
Stevie 3:497ac7d026b5 88 MLF_ENC1.fall(&mlfEncoderIsr1);
Stevie 3:497ac7d026b5 89 MLF_ENC2.fall(&mlfEncoderIsr2);
Stevie 9:0d274fc2e6df 90
Stevie 0:d665d69947ab 91 //setup driver pins
Stevie 2:00535b62c344 92 setLMotorDir(1);
Stevie 2:00535b62c344 93 setRMotorDir(1);
Stevie 2:00535b62c344 94 // Set PWM period in us
Stevie 2:00535b62c344 95 setPeriod(100);
Stevie 9:0d274fc2e6df 96 // pc.printf("Starting up");
Stevie 5:600c5c9cbb19 97 //setup control loop
Stevie 5:600c5c9cbb19 98 control_timer.attach(&controlTick, 0.05); //attach the flag setting interrupt for control timing every 50ms
Stevie 9:0d274fc2e6df 99
Stevie 9:0d274fc2e6df 100
Stevie 9:0d274fc2e6df 101
Stevie 0:d665d69947ab 102 while (1) {
Stevie 9:0d274fc2e6df 103 // pc.printf("Motor count %i\r\n", m_count[0]);
Stevie 9:0d274fc2e6df 104
Stevie 5:600c5c9cbb19 105 if(control_loop_flag == true) { //flag set true by ticker timer every 50ms
Stevie 6:a44d6f7626f2 106 if(control_mode == true) { //speed control
Stevie 9:0d274fc2e6df 107 for(i=0; i<=1; i++) { //do this for right and left in turn
Stevie 6:a44d6f7626f2 108 pos_change = float(m_count[i]-m_last_count[i]); //calculate change in position
Stevie 6:a44d6f7626f2 109 m_last_count[i] = m_count[i]; //store count for next cycle
Stevie 9:0d274fc2e6df 110 speed_error = pos_change - float(m_speed_ref[i]); //calculate different between current speed and reference speed
Stevie 6:a44d6f7626f2 111 integral[i] = integral[i] + speed_error;
Stevie 6:a44d6f7626f2 112 m_duty[i] = Kp*speed_error + Ki*integral[i]; //speed proportional control
Stevie 6:a44d6f7626f2 113 }
Stevie 6:a44d6f7626f2 114 if(m_speed_ref[0] == 0) //stop the control loop from doing weird things at standstill
Stevie 6:a44d6f7626f2 115 m_duty[0] = 0;
Stevie 6:a44d6f7626f2 116 if(m_speed_ref[1] == 0)
Stevie 6:a44d6f7626f2 117 m_duty[1] = 0;
Stevie 9:0d274fc2e6df 118 } else if(control_mode == false) { //distance x and max speed y control
Stevie 8:826ec74d53c9 119 int targetSpeed=0;
Stevie 9:0d274fc2e6df 120 for(i=0; i<=1; i++) { //do this for right and left in turn
Stevie 8:826ec74d53c9 121 pos_change = float(m_count[i]-m_last_count[i]); //calculate change in position
Stevie 8:826ec74d53c9 122 m_last_count[i] = m_count[i]; //store count for next cycle
Stevie 9:0d274fc2e6df 123 targetSpeed=(int)calcSpeed((float)m_count[i], 0.0f, (float)m_distance_ref[i], (float)max_accel, (float)m_top_speed[i]);//m_speed_ref is used as top speed
Stevie 8:826ec74d53c9 124 speed_error = pos_change - targetSpeed; //calculate different between current speed and reference speed
Stevie 8:826ec74d53c9 125 integral[i] = integral[i] + speed_error;
Stevie 10:63381713610c 126 m_duty[i] = Kp*0.1*speed_error + Ki*0.1*integral[i]; //speed proportional control
Stevie 8:826ec74d53c9 127 }
Stevie 5:600c5c9cbb19 128 }
Stevie 6:a44d6f7626f2 129
Stevie 9:0d274fc2e6df 130 if(m_duty[0] < 0) {
Stevie 5:600c5c9cbb19 131 setRMotorDir(0); //set the motors backwards
Stevie 5:600c5c9cbb19 132 m_duty[0] = -1*m_duty[0]; //make the negative value into positive
Stevie 5:600c5c9cbb19 133 } else {
Stevie 5:600c5c9cbb19 134 setRMotorDir(1); //set the motors forwards
Stevie 5:600c5c9cbb19 135 }
Stevie 9:0d274fc2e6df 136 if(m_duty[0] > 100) {
Stevie 5:600c5c9cbb19 137 m_duty[0] = 100; //make sure the speed isn't greater than 100%
Stevie 5:600c5c9cbb19 138 }
Stevie 9:0d274fc2e6df 139
Stevie 9:0d274fc2e6df 140 if(m_duty[1] < 0) {
Stevie 5:600c5c9cbb19 141 setLMotorDir(0); //set the motors backwards
Stevie 5:600c5c9cbb19 142 m_duty[1] = -1*m_duty[1]; //make the negative value into positive
Stevie 5:600c5c9cbb19 143 } else {
Stevie 5:600c5c9cbb19 144 setLMotorDir(1); //set the motors forwards
Stevie 5:600c5c9cbb19 145 }
Stevie 9:0d274fc2e6df 146 if(m_duty[1] > 100) {
Stevie 5:600c5c9cbb19 147 m_duty[1] = 100; //make sure the speed isn't greater than 100%
Stevie 9:0d274fc2e6df 148 }
Stevie 5:600c5c9cbb19 149 setDuty(); //set all the duty cycles to the motor drivers
Stevie 5:600c5c9cbb19 150 control_loop_flag = false;
Stevie 4:abf0070897ff 151 }
Stevie 3:497ac7d026b5 152 readADC(); //read all the ADC values when not doing something else
Stevie 3:497ac7d026b5 153 spiComms(); //do SPI communication stuff
Stevie 2:00535b62c344 154 }
Stevie 2:00535b62c344 155 }
Stevie 9:0d274fc2e6df 156
Stevie 9:0d274fc2e6df 157
Stevie 2:00535b62c344 158 //function to read all 6 ADC channels
Stevie 9:0d274fc2e6df 159 void readADC()
Stevie 9:0d274fc2e6df 160 {
Stevie 2:00535b62c344 161 adc_values[0] = int(ANALOG0.read()*255);
Stevie 2:00535b62c344 162 adc_values[1] = int(ANALOG1.read()*255);
Stevie 2:00535b62c344 163 adc_values[2] = int(ANALOG2.read()*255);
Stevie 2:00535b62c344 164 adc_values[3] = int(ANALOG3.read()*255);
Stevie 2:00535b62c344 165 adc_values[4] = int(ANALOG4.read()*255);
Stevie 2:00535b62c344 166 adc_values[5] = int(ANALOG5.read()*255);
Stevie 2:00535b62c344 167 }
Stevie 9:0d274fc2e6df 168
Stevie 2:00535b62c344 169 //function to set all 4 motor PWM duty cycles
Stevie 9:0d274fc2e6df 170 void setDuty()
Stevie 9:0d274fc2e6df 171 {
Stevie 2:00535b62c344 172 MRB.write(m_duty[0]);
Stevie 4:abf0070897ff 173 MRF.write(m_duty[0]);
Stevie 4:abf0070897ff 174 MLB.write(m_duty[1]);
Stevie 4:abf0070897ff 175 MLF.write(m_duty[1]);
Stevie 2:00535b62c344 176 }
Stevie 2:00535b62c344 177 //set left motor direction. 1 is forward, 0 is backwards.
Stevie 9:0d274fc2e6df 178 void setLMotorDir(bool direction)
Stevie 9:0d274fc2e6df 179 {
Stevie 2:00535b62c344 180 LSTANDBY = 1;
Stevie 9:0d274fc2e6df 181 if(direction == true) {
Stevie 2:00535b62c344 182 LAIN1 = 1;
Stevie 2:00535b62c344 183 LAIN2 = 0;
Stevie 9:0d274fc2e6df 184 } else if(direction == false) {
Stevie 2:00535b62c344 185 LAIN1 = 0;
Stevie 2:00535b62c344 186 LAIN2 = 1;
Stevie 2:00535b62c344 187 }
Stevie 2:00535b62c344 188 }
Stevie 2:00535b62c344 189 //set right motor direction. 1 is forward, 0 is backwards.
Stevie 9:0d274fc2e6df 190 void setRMotorDir(bool direction)
Stevie 9:0d274fc2e6df 191 {
Stevie 2:00535b62c344 192 RSTANDBY = 1;
Stevie 9:0d274fc2e6df 193 if(direction == true) {
Stevie 2:00535b62c344 194 RAIN1 = 0;
Stevie 2:00535b62c344 195 RAIN2 = 1;
Stevie 9:0d274fc2e6df 196 } else if(direction == false) {
Stevie 2:00535b62c344 197 RAIN1 = 1;
Stevie 2:00535b62c344 198 RAIN2 = 0;
Stevie 2:00535b62c344 199 }
Stevie 2:00535b62c344 200 }
Stevie 9:0d274fc2e6df 201
Stevie 2:00535b62c344 202 //initialisation function to set motor PWM period and set to 0 duty
Stevie 9:0d274fc2e6df 203 void setPeriod(int period)
Stevie 9:0d274fc2e6df 204 {
Stevie 2:00535b62c344 205 MRB.period_us(period);
Stevie 2:00535b62c344 206 MRB.write(0.0);
Stevie 2:00535b62c344 207 MRF.period_us(period);
Stevie 2:00535b62c344 208 MRF.write(0.0);
Stevie 2:00535b62c344 209 MLB.period_us(period);
Stevie 2:00535b62c344 210 MLB.write(0.0);
Stevie 2:00535b62c344 211 MLF.period_us(period);
Stevie 2:00535b62c344 212 MLF.write(0.0);
Stevie 2:00535b62c344 213 }
Stevie 9:0d274fc2e6df 214
Stevie 9:0d274fc2e6df 215 void spiComms()
Stevie 9:0d274fc2e6df 216 {
Stevie 9:0d274fc2e6df 217 //do SPI communication stuff
Stevie 2:00535b62c344 218 int i = 0; //temp counter variable
Stevie 2:00535b62c344 219 int v = 0; //temp SPI variable
Stevie 2:00535b62c344 220 if(rpi.receive()) {
Stevie 2:00535b62c344 221 v = rpi.read(); // Read byte from master
Stevie 9:0d274fc2e6df 222 if(v == char('S')) {
Stevie 2:00535b62c344 223 rpi.reply(0x01);
Stevie 9:0d274fc2e6df 224 for (i=0; i<=1; i++) {
Stevie 2:00535b62c344 225 m_speed_ref[i] = rpi.read() - 128;
Stevie 2:00535b62c344 226 rpi.reply(m_speed_ref[i]);
Stevie 0:d665d69947ab 227 }
Stevie 2:00535b62c344 228 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 229 rpi.reply(0x00);
Stevie 6:a44d6f7626f2 230 control_mode = true; //set the controller go do distance control
Stevie 6:a44d6f7626f2 231 m_count[0] = 0; //reset encoder counts to avoid overflow
Stevie 6:a44d6f7626f2 232 m_count[1] = 0;
Stevie 7:948651ca4c0e 233 integral[0] = 0; //reset integrals to avoid odd behaviour
Stevie 7:948651ca4c0e 234 integral[1] = 0;
Stevie 9:0d274fc2e6df 235 } else if(v == char('D')) {
Stevie 2:00535b62c344 236 rpi.reply(0x02);
Stevie 9:0d274fc2e6df 237 for (i=0; i<=1; i++) {
Stevie 6:a44d6f7626f2 238 m_distance_ref[i] = rpi.read() - 128;
Stevie 6:a44d6f7626f2 239 rpi.reply(m_distance_ref[i]);
Stevie 6:a44d6f7626f2 240 m_distance_ref[i] = m_distance_ref[i] * distance_scale;
Stevie 2:00535b62c344 241 }
Stevie 9:0d274fc2e6df 242 for (i=0; i<=1; i++) {
Stevie 8:826ec74d53c9 243 m_top_speed[i] = rpi.read() - 128;
Stevie 8:826ec74d53c9 244 rpi.reply(m_top_speed[i]);
Stevie 0:d665d69947ab 245 }
Stevie 2:00535b62c344 246 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 247 rpi.reply(0x00);
Stevie 6:a44d6f7626f2 248 control_mode = false; //set the controller go do distance control
Stevie 6:a44d6f7626f2 249 m_count[0] = 0; //reset encoder counts to avoid overflow
Stevie 6:a44d6f7626f2 250 m_count[1] = 0;
Stevie 8:826ec74d53c9 251 m_last_count[0] = 0;
Stevie 8:826ec74d53c9 252 m_last_count[1] = 0;
Stevie 9:0d274fc2e6df 253 } else if(v == char('A')) {
Stevie 9:0d274fc2e6df 254 rpi.reply(0x03);
Stevie 2:00535b62c344 255 max_accel = rpi.read();
Stevie 2:00535b62c344 256 rpi.reply(max_accel);
Stevie 2:00535b62c344 257 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 258 rpi.reply(0x00);
Stevie 9:0d274fc2e6df 259 } else if(v == char('V')) {
Stevie 9:0d274fc2e6df 260 rpi.reply(0x04);
Stevie 2:00535b62c344 261 v = rpi.read();
Stevie 9:0d274fc2e6df 262 if(v <= 6) { //check the ADC to be addressed exists
Stevie 2:00535b62c344 263 rpi.reply(adc_values[v]);
Stevie 0:d665d69947ab 264 }
Stevie 2:00535b62c344 265 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 266 rpi.reply(0x00);
Stevie 0:d665d69947ab 267 }
Stevie 0:d665d69947ab 268 }
Stevie 3:497ac7d026b5 269 }
Stevie 9:0d274fc2e6df 270 void mrfEncoderIsr1()
Stevie 9:0d274fc2e6df 271 {
Stevie 9:0d274fc2e6df 272 if(MRF_ENC2 == 0) {
Stevie 3:497ac7d026b5 273 m_count[0] ++;
Stevie 3:497ac7d026b5 274 } else {
Stevie 3:497ac7d026b5 275 m_count[0] --;
Stevie 3:497ac7d026b5 276 }
Stevie 3:497ac7d026b5 277 }
Stevie 9:0d274fc2e6df 278 void mrfEncoderIsr2()
Stevie 9:0d274fc2e6df 279 {
Stevie 4:abf0070897ff 280 if(MRF_ENC1 == 1) {
Stevie 4:abf0070897ff 281 m_count[0] ++;
Stevie 3:497ac7d026b5 282 } else {
Stevie 4:abf0070897ff 283 m_count[0] --;
Stevie 3:497ac7d026b5 284 }
Stevie 3:497ac7d026b5 285 }
Stevie 9:0d274fc2e6df 286 void mlfEncoderIsr1()
Stevie 9:0d274fc2e6df 287 {
Stevie 4:abf0070897ff 288 if(MLF_ENC2 == 0) {
Stevie 3:497ac7d026b5 289 m_count[1] ++;
Stevie 3:497ac7d026b5 290 } else {
Stevie 3:497ac7d026b5 291 m_count[1] --;
Stevie 3:497ac7d026b5 292 }
Stevie 3:497ac7d026b5 293 }
Stevie 9:0d274fc2e6df 294 void mlfEncoderIsr2()
Stevie 9:0d274fc2e6df 295 {
Stevie 4:abf0070897ff 296 if(MLF_ENC1 == 1) {
Stevie 4:abf0070897ff 297 m_count[1] ++;
Stevie 3:497ac7d026b5 298 } else {
Stevie 4:abf0070897ff 299 m_count[1] --;
Stevie 3:497ac7d026b5 300 }
Stevie 3:497ac7d026b5 301 }
Stevie 9:0d274fc2e6df 302
Stevie 5:600c5c9cbb19 303 void controlTick()
Stevie 5:600c5c9cbb19 304 {
Stevie 5:600c5c9cbb19 305 control_loop_flag = true;
Stevie 5:600c5c9cbb19 306 }
Stevie 8:826ec74d53c9 307
Stevie 9:0d274fc2e6df 308
Stevie 9:0d274fc2e6df 309 float calcSpeed(float currentPosition, float start, float end, float accel, float topSpeed)
Stevie 9:0d274fc2e6df 310 {
Stevie 8:826ec74d53c9 311 //targetSpeed=(int)calcSpeed((float)m_count[i], 0.0f, (float)m_distance_ref[i], (float)max_accel, (float)m_top_speed[i], true);//m_speed_ref is used as top speed
Stevie 8:826ec74d53c9 312 //note to self: check direction
Stevie 9:0d274fc2e6df 313 bool direction;
Stevie 9:0d274fc2e6df 314
Stevie 9:0d274fc2e6df 315 if(end<0)
Stevie 9:0d274fc2e6df 316 direction = false;
Stevie 9:0d274fc2e6df 317 else
Stevie 9:0d274fc2e6df 318 direction = true;
Stevie 9:0d274fc2e6df 319
Stevie 8:826ec74d53c9 320 float minSpeed=1;//deg per sec - to prevent motor speed=0 and therefore jam by delay=infinity.
Stevie 8:826ec74d53c9 321 float halfWay = 0.0;
Stevie 8:826ec74d53c9 322 float v = 0.0;
Stevie 8:826ec74d53c9 323 float midPointVsqrd = 0.0;
Stevie 9:0d274fc2e6df 324 halfWay=abs(start)+(abs(end)-abs(start))/2;
Stevie 9:0d274fc2e6df 325
Stevie 8:826ec74d53c9 326
Stevie 9:0d274fc2e6df 327 if (abs(currentPosition)>halfWay) {
Stevie 9:0d274fc2e6df 328 //deaccelarate
Stevie 9:0d274fc2e6df 329 midPointVsqrd=minSpeed*minSpeed+2*accel*(halfWay-abs(start));
Stevie 9:0d274fc2e6df 330 v=sqrt(midPointVsqrd-2*accel*(abs(currentPosition)-halfWay));
Stevie 9:0d274fc2e6df 331 } else {
Stevie 9:0d274fc2e6df 332 //accelerate
Stevie 9:0d274fc2e6df 333 //v^2=u^2+2as
Stevie 9:0d274fc2e6df 334 v=sqrt(minSpeed*minSpeed+2*accel*(abs(currentPosition)-abs(start)));
Stevie 8:826ec74d53c9 335 }
Stevie 9:0d274fc2e6df 336
Stevie 8:826ec74d53c9 337
Stevie 9:0d274fc2e6df 338 if (v<minSpeed) {
Stevie 9:0d274fc2e6df 339 v=minSpeed;
Stevie 9:0d274fc2e6df 340 } else if (v>topSpeed) {
Stevie 9:0d274fc2e6df 341 v = topSpeed;
Stevie 8:826ec74d53c9 342 }
Stevie 9:0d274fc2e6df 343
Stevie 9:0d274fc2e6df 344 if (direction == false) {
Stevie 9:0d274fc2e6df 345 v = -1*v;
Stevie 8:826ec74d53c9 346 }
Stevie 8:826ec74d53c9 347
Stevie 8:826ec74d53c9 348 return v;
Stevie 8:826ec74d53c9 349 }