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@8:826ec74d53c9, 2019-02-28 (annotated)

Committer:
Stevie
Date:
Thu Feb 28 21:45:11 2019 +0000
Revision:
8:826ec74d53c9
Parent:
7:948651ca4c0e
Child:
9:0d274fc2e6df
Partially working acceleration - forward only

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stevie 0:d665d69947ab 1 #include "mbed.h"
Stevie 8:826ec74d53c9 2
Stevie 4:abf0070897ff 3 //Serial pc(SERIAL_TX, SERIAL_RX);
Stevie 8:826ec74d53c9 4
Stevie 8:826ec74d53c9 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 8:826ec74d53c9 34
Stevie 3:497ac7d026b5 35 bool control_loop_flag = false;
Stevie 8:826ec74d53c9 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 6:a44d6f7626f2 47 int distance_scale = 14;
Stevie 6:a44d6f7626f2 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 8:826ec74d53c9 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 8:826ec74d53c9 54 bool m_direction [2] = {true, true};
Stevie 8:826ec74d53c9 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 8:826ec74d53c9 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 8:826ec74d53c9 68 float calcSpeed(float currentPosition, float start, float end, float accel, float topSpeed, bool direction) ;
Stevie 8:826ec74d53c9 69
Stevie 8:826ec74d53c9 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 8:826ec74d53c9 75
Stevie 5:600c5c9cbb19 76 float Kp = 0.01; //proportional constant
Stevie 5:600c5c9cbb19 77 float Ki = 0.01; //integral constant
Stevie 5:600c5c9cbb19 78 int i = 0;
Stevie 5:600c5c9cbb19 79
Stevie 6:a44d6f7626f2 80 float distance_error = 0;
Stevie 6:a44d6f7626f2 81 float Kp_d = 0.01;
Stevie 6:a44d6f7626f2 82 float max_duty = 0;
Stevie 6:a44d6f7626f2 83
Stevie 5:600c5c9cbb19 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 3:497ac7d026b5 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 4:abf0070897ff 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 5:600c5c9cbb19 99
Stevie 8:826ec74d53c9 100
Stevie 3:497ac7d026b5 101
Stevie 0:d665d69947ab 102 while (1) {
Stevie 4:abf0070897ff 103 // pc.printf("Motor count %i\r\n", m_count[0]);
Stevie 8:826ec74d53c9 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 6:a44d6f7626f2 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 6:a44d6f7626f2 110 speed_error = pos_change - 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 6:a44d6f7626f2 118 }
Stevie 6:a44d6f7626f2 119 else if(control_mode == false) { //distance x and max speed y control
Stevie 8:826ec74d53c9 120 int targetSpeed=0;
Stevie 7:948651ca4c0e 121 for(i=0;i<=1;i++){ //do this for right and left in turn
Stevie 8:826ec74d53c9 122 pos_change = float(m_count[i]-m_last_count[i]); //calculate change in position
Stevie 8:826ec74d53c9 123 m_last_count[i] = m_count[i]; //store count for next cycle
Stevie 8:826ec74d53c9 124 targetSpeed=(int)calcSpeed((float)m_count[i], 0.0f, (float)m_distance_ref[i], (float)max_accel, (float)m_top_speed[i], m_direction[i]);//m_speed_ref is used as top speed
Stevie 8:826ec74d53c9 125 speed_error = pos_change - targetSpeed; //calculate different between current speed and reference speed
Stevie 8:826ec74d53c9 126 integral[i] = integral[i] + speed_error;
Stevie 8:826ec74d53c9 127 m_duty[i] = Kp*speed_error + Ki*integral[i]; //speed proportional control
Stevie 8:826ec74d53c9 128
Stevie 8:826ec74d53c9 129
Stevie 8:826ec74d53c9 130
Stevie 8:826ec74d53c9 131 //distance_error = float(m_count[i]-m_distance_ref[i]); //calculate difference between current speed and reference speed
Stevie 8:826ec74d53c9 132 //m_duty[i] = Kp_d*distance_error; //speed proportional control
Stevie 8:826ec74d53c9 133 //max_duty = float(m_top_speed[i])/100;
Stevie 8:826ec74d53c9 134 //if(m_duty[i] > max_duty){ //check the max duty hasn't been exceeded
Stevie 8:826ec74d53c9 135 // m_duty[i] = max_duty;
Stevie 8:826ec74d53c9 136 //}
Stevie 8:826ec74d53c9 137 //else if(m_duty[i] < -1*max_duty){
Stevie 8:826ec74d53c9 138 // m_duty[i] = -1*max_duty;
Stevie 8:826ec74d53c9 139 //}
Stevie 8:826ec74d53c9 140 }
Stevie 8:826ec74d53c9 141 /* if(m_speed_ref[0] == 0) //stop the control loop from doing weird things at standstill
Stevie 8:826ec74d53c9 142 m_duty[0] = 0;
Stevie 8:826ec74d53c9 143 if(m_speed_ref[1] == 0)
Stevie 8:826ec74d53c9 144 m_duty[1] = 0;*/
Stevie 5:600c5c9cbb19 145 }
Stevie 5:600c5c9cbb19 146
Stevie 6:a44d6f7626f2 147
Stevie 5:600c5c9cbb19 148
Stevie 5:600c5c9cbb19 149 if(m_duty[0] < 0){
Stevie 5:600c5c9cbb19 150 setRMotorDir(0); //set the motors backwards
Stevie 5:600c5c9cbb19 151 m_duty[0] = -1*m_duty[0]; //make the negative value into positive
Stevie 5:600c5c9cbb19 152 } else {
Stevie 5:600c5c9cbb19 153 setRMotorDir(1); //set the motors forwards
Stevie 5:600c5c9cbb19 154 }
Stevie 5:600c5c9cbb19 155 if(m_duty[0] > 100){
Stevie 5:600c5c9cbb19 156 m_duty[0] = 100; //make sure the speed isn't greater than 100%
Stevie 5:600c5c9cbb19 157 }
Stevie 5:600c5c9cbb19 158
Stevie 5:600c5c9cbb19 159 if(m_duty[1] < 0){
Stevie 5:600c5c9cbb19 160 setLMotorDir(0); //set the motors backwards
Stevie 5:600c5c9cbb19 161 m_duty[1] = -1*m_duty[1]; //make the negative value into positive
Stevie 5:600c5c9cbb19 162 } else {
Stevie 5:600c5c9cbb19 163 setLMotorDir(1); //set the motors forwards
Stevie 5:600c5c9cbb19 164 }
Stevie 5:600c5c9cbb19 165 if(m_duty[1] > 100){
Stevie 5:600c5c9cbb19 166 m_duty[1] = 100; //make sure the speed isn't greater than 100%
Stevie 5:600c5c9cbb19 167 }
Stevie 5:600c5c9cbb19 168 setDuty(); //set all the duty cycles to the motor drivers
Stevie 5:600c5c9cbb19 169 control_loop_flag = false;
Stevie 4:abf0070897ff 170 }
Stevie 8:826ec74d53c9 171 for(i=0;i<1;i++){
Stevie 8:826ec74d53c9 172 if(m_distance_ref[i]<0)
Stevie 8:826ec74d53c9 173 m_direction[i] = false;
Stevie 8:826ec74d53c9 174 else
Stevie 8:826ec74d53c9 175 m_direction[i] = true;
Stevie 8:826ec74d53c9 176 }
Stevie 3:497ac7d026b5 177 readADC(); //read all the ADC values when not doing something else
Stevie 3:497ac7d026b5 178 spiComms(); //do SPI communication stuff
Stevie 2:00535b62c344 179 }
Stevie 2:00535b62c344 180 }
Stevie 8:826ec74d53c9 181
Stevie 2:00535b62c344 182 //function to read all 6 ADC channels
Stevie 2:00535b62c344 183 void readADC(){
Stevie 2:00535b62c344 184 adc_values[0] = int(ANALOG0.read()*255);
Stevie 2:00535b62c344 185 adc_values[1] = int(ANALOG1.read()*255);
Stevie 2:00535b62c344 186 adc_values[2] = int(ANALOG2.read()*255);
Stevie 2:00535b62c344 187 adc_values[3] = int(ANALOG3.read()*255);
Stevie 2:00535b62c344 188 adc_values[4] = int(ANALOG4.read()*255);
Stevie 2:00535b62c344 189 adc_values[5] = int(ANALOG5.read()*255);
Stevie 2:00535b62c344 190 }
Stevie 8:826ec74d53c9 191
Stevie 2:00535b62c344 192 //function to set all 4 motor PWM duty cycles
Stevie 2:00535b62c344 193 void setDuty(){
Stevie 2:00535b62c344 194 MRB.write(m_duty[0]);
Stevie 4:abf0070897ff 195 MRF.write(m_duty[0]);
Stevie 4:abf0070897ff 196 MLB.write(m_duty[1]);
Stevie 4:abf0070897ff 197 MLF.write(m_duty[1]);
Stevie 2:00535b62c344 198 }
Stevie 2:00535b62c344 199 //set left motor direction. 1 is forward, 0 is backwards.
Stevie 2:00535b62c344 200 void setLMotorDir(bool direction){
Stevie 2:00535b62c344 201 LSTANDBY = 1;
Stevie 2:00535b62c344 202 if(direction == true){
Stevie 2:00535b62c344 203 LAIN1 = 1;
Stevie 2:00535b62c344 204 LAIN2 = 0;
Stevie 2:00535b62c344 205 }
Stevie 2:00535b62c344 206 else if(direction == false){
Stevie 2:00535b62c344 207 LAIN1 = 0;
Stevie 2:00535b62c344 208 LAIN2 = 1;
Stevie 2:00535b62c344 209 }
Stevie 2:00535b62c344 210 }
Stevie 2:00535b62c344 211 //set right motor direction. 1 is forward, 0 is backwards.
Stevie 2:00535b62c344 212 void setRMotorDir(bool direction){
Stevie 2:00535b62c344 213 RSTANDBY = 1;
Stevie 2:00535b62c344 214 if(direction == true){
Stevie 2:00535b62c344 215 RAIN1 = 0;
Stevie 2:00535b62c344 216 RAIN2 = 1;
Stevie 2:00535b62c344 217 }
Stevie 2:00535b62c344 218 else if(direction == false){
Stevie 2:00535b62c344 219 RAIN1 = 1;
Stevie 2:00535b62c344 220 RAIN2 = 0;
Stevie 2:00535b62c344 221 }
Stevie 2:00535b62c344 222 }
Stevie 8:826ec74d53c9 223
Stevie 2:00535b62c344 224 //initialisation function to set motor PWM period and set to 0 duty
Stevie 2:00535b62c344 225 void setPeriod(int period){
Stevie 2:00535b62c344 226 MRB.period_us(period);
Stevie 2:00535b62c344 227 MRB.write(0.0);
Stevie 2:00535b62c344 228 MRF.period_us(period);
Stevie 2:00535b62c344 229 MRF.write(0.0);
Stevie 2:00535b62c344 230 MLB.period_us(period);
Stevie 2:00535b62c344 231 MLB.write(0.0);
Stevie 2:00535b62c344 232 MLF.period_us(period);
Stevie 2:00535b62c344 233 MLF.write(0.0);
Stevie 2:00535b62c344 234 }
Stevie 8:826ec74d53c9 235
Stevie 2:00535b62c344 236 void spiComms(){
Stevie 2:00535b62c344 237 //do SPI communication stuff
Stevie 2:00535b62c344 238 int i = 0; //temp counter variable
Stevie 2:00535b62c344 239 int v = 0; //temp SPI variable
Stevie 2:00535b62c344 240 if(rpi.receive()) {
Stevie 2:00535b62c344 241 v = rpi.read(); // Read byte from master
Stevie 2:00535b62c344 242 if(v == char('S')){
Stevie 2:00535b62c344 243 rpi.reply(0x01);
Stevie 4:abf0070897ff 244 for (i=0;i<=1;i++){
Stevie 2:00535b62c344 245 m_speed_ref[i] = rpi.read() - 128;
Stevie 2:00535b62c344 246 rpi.reply(m_speed_ref[i]);
Stevie 0:d665d69947ab 247 }
Stevie 2:00535b62c344 248 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 249 rpi.reply(0x00);
Stevie 6:a44d6f7626f2 250 control_mode = true; //set the controller go do distance control
Stevie 6:a44d6f7626f2 251 m_count[0] = 0; //reset encoder counts to avoid overflow
Stevie 6:a44d6f7626f2 252 m_count[1] = 0;
Stevie 7:948651ca4c0e 253 integral[0] = 0; //reset integrals to avoid odd behaviour
Stevie 7:948651ca4c0e 254 integral[1] = 0;
Stevie 2:00535b62c344 255 }
Stevie 2:00535b62c344 256 else if(v == char('D')){
Stevie 2:00535b62c344 257 rpi.reply(0x02);
Stevie 4:abf0070897ff 258 for (i=0;i<=1;i++){
Stevie 6:a44d6f7626f2 259 m_distance_ref[i] = rpi.read() - 128;
Stevie 6:a44d6f7626f2 260 rpi.reply(m_distance_ref[i]);
Stevie 6:a44d6f7626f2 261 m_distance_ref[i] = m_distance_ref[i] * distance_scale;
Stevie 2:00535b62c344 262 }
Stevie 4:abf0070897ff 263 for (i=0;i<=1;i++){
Stevie 8:826ec74d53c9 264 m_top_speed[i] = rpi.read() - 128;
Stevie 8:826ec74d53c9 265 rpi.reply(m_top_speed[i]);
Stevie 0:d665d69947ab 266 }
Stevie 2:00535b62c344 267 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 268 rpi.reply(0x00);
Stevie 6:a44d6f7626f2 269 control_mode = false; //set the controller go do distance control
Stevie 6:a44d6f7626f2 270 m_count[0] = 0; //reset encoder counts to avoid overflow
Stevie 6:a44d6f7626f2 271 m_count[1] = 0;
Stevie 8:826ec74d53c9 272 m_last_count[0] = 0;
Stevie 8:826ec74d53c9 273 m_last_count[1] = 0;
Stevie 2:00535b62c344 274 }
Stevie 2:00535b62c344 275 else if(v == char('A')){
Stevie 2:00535b62c344 276 rpi.reply(0x03);
Stevie 2:00535b62c344 277 max_accel = rpi.read();
Stevie 2:00535b62c344 278 rpi.reply(max_accel);
Stevie 2:00535b62c344 279 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 280 rpi.reply(0x00);
Stevie 2:00535b62c344 281 }
Stevie 2:00535b62c344 282 else if(v == char('V')){
Stevie 2:00535b62c344 283 rpi.reply(0x04);
Stevie 2:00535b62c344 284 v = rpi.read();
Stevie 2:00535b62c344 285 if(v <= 6){ //check the ADC to be addressed exists
Stevie 2:00535b62c344 286 rpi.reply(adc_values[v]);
Stevie 0:d665d69947ab 287 }
Stevie 2:00535b62c344 288 v = rpi.read(); //last bit setup a blank reply
Stevie 2:00535b62c344 289 rpi.reply(0x00);
Stevie 0:d665d69947ab 290 }
Stevie 0:d665d69947ab 291 }
Stevie 3:497ac7d026b5 292 }
Stevie 4:abf0070897ff 293 void mrfEncoderIsr1(){
Stevie 4:abf0070897ff 294 if(MRF_ENC2 == 0) {
Stevie 3:497ac7d026b5 295 m_count[0] ++;
Stevie 3:497ac7d026b5 296 } else {
Stevie 3:497ac7d026b5 297 m_count[0] --;
Stevie 3:497ac7d026b5 298 }
Stevie 3:497ac7d026b5 299 }
Stevie 4:abf0070897ff 300 void mrfEncoderIsr2(){
Stevie 4:abf0070897ff 301 if(MRF_ENC1 == 1) {
Stevie 4:abf0070897ff 302 m_count[0] ++;
Stevie 3:497ac7d026b5 303 } else {
Stevie 4:abf0070897ff 304 m_count[0] --;
Stevie 3:497ac7d026b5 305 }
Stevie 3:497ac7d026b5 306 }
Stevie 4:abf0070897ff 307 void mlfEncoderIsr1(){
Stevie 4:abf0070897ff 308 if(MLF_ENC2 == 0) {
Stevie 3:497ac7d026b5 309 m_count[1] ++;
Stevie 3:497ac7d026b5 310 } else {
Stevie 3:497ac7d026b5 311 m_count[1] --;
Stevie 3:497ac7d026b5 312 }
Stevie 3:497ac7d026b5 313 }
Stevie 4:abf0070897ff 314 void mlfEncoderIsr2(){
Stevie 4:abf0070897ff 315 if(MLF_ENC1 == 1) {
Stevie 4:abf0070897ff 316 m_count[1] ++;
Stevie 3:497ac7d026b5 317 } else {
Stevie 4:abf0070897ff 318 m_count[1] --;
Stevie 3:497ac7d026b5 319 }
Stevie 3:497ac7d026b5 320 }
Stevie 8:826ec74d53c9 321
Stevie 5:600c5c9cbb19 322 void controlTick()
Stevie 5:600c5c9cbb19 323 {
Stevie 5:600c5c9cbb19 324 control_loop_flag = true;
Stevie 5:600c5c9cbb19 325 }
Stevie 8:826ec74d53c9 326
Stevie 8:826ec74d53c9 327
Stevie 8:826ec74d53c9 328 float calcSpeed(float currentPosition, float start, float end, float accel, float topSpeed, bool direction) {
Stevie 8:826ec74d53c9 329 //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 330 //note to self: check direction
Stevie 8:826ec74d53c9 331
Stevie 8:826ec74d53c9 332 // end = end/float(distance_scale);
Stevie 8:826ec74d53c9 333 float minSpeed=1;//deg per sec - to prevent motor speed=0 and therefore jam by delay=infinity.
Stevie 8:826ec74d53c9 334 float halfWay = 0.0;
Stevie 8:826ec74d53c9 335 float v = 0.0;
Stevie 8:826ec74d53c9 336 float midPointVsqrd = 0.0;
Stevie 8:826ec74d53c9 337 halfWay=start+(end-start)/2;
Stevie 8:826ec74d53c9 338
Stevie 8:826ec74d53c9 339 // if (direction == true) {
Stevie 8:826ec74d53c9 340 if (abs(currentPosition)>abs(halfWay)) {
Stevie 8:826ec74d53c9 341 //deaccelarate
Stevie 8:826ec74d53c9 342 midPointVsqrd=minSpeed*minSpeed+2*accel*(halfWay-start);
Stevie 8:826ec74d53c9 343 v=sqrt(midPointVsqrd-2*accel*(currentPosition-halfWay));
Stevie 8:826ec74d53c9 344 } else {
Stevie 8:826ec74d53c9 345 //accelerate
Stevie 8:826ec74d53c9 346 //v^2=u^2+2as
Stevie 8:826ec74d53c9 347 v=sqrt(minSpeed*minSpeed+2*accel*(currentPosition-start));
Stevie 8:826ec74d53c9 348 }
Stevie 8:826ec74d53c9 349 /* } else {
Stevie 8:826ec74d53c9 350 if (currentPosition<halfWay) {
Stevie 8:826ec74d53c9 351 //deaccelarate
Stevie 8:826ec74d53c9 352 midPointVsqrd=minSpeed*minSpeed+2*accel*(start-halfWay);
Stevie 8:826ec74d53c9 353 v=sqrt(midPointVsqrd-2*accel*(halfWay-currentPosition));
Stevie 8:826ec74d53c9 354 } else {
Stevie 8:826ec74d53c9 355 //accelerate
Stevie 8:826ec74d53c9 356 //calc velocity
Stevie 8:826ec74d53c9 357 //v^2=u^2+2as
Stevie 8:826ec74d53c9 358 v=sqrt(minSpeed*minSpeed+2*accel*(start-currentPosition));
Stevie 8:826ec74d53c9 359 }
Stevie 8:826ec74d53c9 360 } */
Stevie 8:826ec74d53c9 361
Stevie 8:826ec74d53c9 362 if (v<minSpeed && v > 0) {
Stevie 8:826ec74d53c9 363 v=minSpeed;
Stevie 8:826ec74d53c9 364 }
Stevie 8:826ec74d53c9 365 else if (abs(v) < minSpeed && v < 0){
Stevie 8:826ec74d53c9 366 v = -1*minSpeed;
Stevie 8:826ec74d53c9 367 }
Stevie 8:826ec74d53c9 368
Stevie 8:826ec74d53c9 369 if (v>topSpeed && v > 0) {
Stevie 8:826ec74d53c9 370 v=topSpeed;
Stevie 8:826ec74d53c9 371 }
Stevie 8:826ec74d53c9 372 else if (abs(v) > topSpeed && v < 0){
Stevie 8:826ec74d53c9 373 v = -1*topSpeed;
Stevie 8:826ec74d53c9 374 }
Stevie 8:826ec74d53c9 375
Stevie 8:826ec74d53c9 376 return v;
Stevie 8:826ec74d53c9 377 //return topSpeed;
Stevie 8:826ec74d53c9 378 }