2015-2016_Mouserat / Lab1_Test

hello

Dependencies:   AVEncoder QEI mbed-src-AV

main.cpp@9:e2feaadf504c, 2015-11-20 (annotated)

Committer:
intgsull
Date:
Fri Nov 20 04:18:00 2015 +0000
Revision:
9:e2feaadf504c
Parent:
8:a254346f20aa 
IR stuff added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
intgsull 4:453d534b1c1d 1 //Micromouse code
intgsull 0:fa523db3f4f5 2 #include "mbed.h"
intgsull 4:453d534b1c1d 3 #include "AVEncoder.h"
intgsull 6:32d9b855b90f 4 #include "analogin_api.h"
intgsull 0:fa523db3f4f5 5
intgsull 4:453d534b1c1d 6 // set things
intgsull 4:453d534b1c1d 7 Serial pc(SERIAL_TX, SERIAL_RX);
intgsull 4:453d534b1c1d 8 Ticker Systicker;
intgsull 4:453d534b1c1d 9 Timer timer;
intgsull 4:453d534b1c1d 10
intgsull 4:453d534b1c1d 11 PwmOut motor1_forward(PB_10);
intgsull 2:5f9b78950a17 12 PwmOut motor1_reverse(PA_6);
intgsull 0:fa523db3f4f5 13 PwmOut motor2_forward(PA_7);
intgsull 0:fa523db3f4f5 14 PwmOut motor2_reverse(PB_6);
intgsull 0:fa523db3f4f5 15
intgsull 4:453d534b1c1d 16 // TODO: change our encoder pins from AnalogIn into:
intgsull 4:453d534b1c1d 17 // otherwise, we can also use the AVEncoder thing as well.
intgsull 4:453d534b1c1d 18 AVEncoder l_enco(PA_15, PB_3);
intgsull 4:453d534b1c1d 19 AVEncoder r_enco(PA_1, PA_10);
intgsull 2:5f9b78950a17 20
intgsull 4:453d534b1c1d 21 // gyro
intgsull 2:5f9b78950a17 22 AnalogIn _gyro(PA_0);
intgsull 4:453d534b1c1d 23 // AnalogIn gyro_cal(PC_1) ?? currently this isn't connected.
intgsull 1:5b9fa1823663 24
intgsull 2:5f9b78950a17 25 //Left Front IR
intgsull 2:5f9b78950a17 26 DigitalOut eLF(PC_3);
intgsull 2:5f9b78950a17 27 AnalogIn rLF(PC_0);
intgsull 2:5f9b78950a17 28 //PC_4 is an ADC
intgsull 2:5f9b78950a17 29 //Left Side IR
intgsull 2:5f9b78950a17 30 DigitalOut eLS(PC_2);
intgsull 2:5f9b78950a17 31 AnalogIn rLS(PC_1);
intgsull 2:5f9b78950a17 32
intgsull 2:5f9b78950a17 33 //Right Front IR
intgsull 2:5f9b78950a17 34 DigitalOut eRF(PC_12);
intgsull 2:5f9b78950a17 35 AnalogIn rRF(PA_4);
intgsull 2:5f9b78950a17 36
intgsull 2:5f9b78950a17 37 //Right Side IR
intgsull 2:5f9b78950a17 38 DigitalOut eRS(PC_15);
intgsull 2:5f9b78950a17 39 AnalogIn rRS(PB_0);
intgsull 1:5b9fa1823663 40
intgsull 0:fa523db3f4f5 41 DigitalOut myled(LED1);
intgsull 0:fa523db3f4f5 42
intgsull 4:453d534b1c1d 43 // global variables.
intgsull 4:453d534b1c1d 44 volatile float gyro_offset = 0;
intgsull 4:453d534b1c1d 45
intgsull 4:453d534b1c1d 46 volatile float line_speed = 1; // currently is in terms of encoder pulses / ms.
intgsull 4:453d534b1c1d 47 volatile float angl_speed = 0; // should not turn while moving forward.
intgsull 4:453d534b1c1d 48
intgsull 4:453d534b1c1d 49 volatile float line_prevError = 0;
intgsull 4:453d534b1c1d 50 volatile int enco_prevError = 0;
intgsull 4:453d534b1c1d 51 volatile float gyro_prevError = 0;
intgsull 1:5b9fa1823663 52
intgsull 4:453d534b1c1d 53 volatile float line_accumulator = 0;
intgsull 6:32d9b855b90f 54 volatile float line_decayFactor = 1;
intgsull 4:453d534b1c1d 55 volatile float enco_accumulator = 0;
intgsull 6:32d9b855b90f 56 volatile float enco_decayFactor = 1;
intgsull 4:453d534b1c1d 57 volatile float gyro_accumulator = 0;
intgsull 6:32d9b855b90f 58 volatile float gyro_decayFactor = 1;
intgsull 4:453d534b1c1d 59
intgsull 4:453d534b1c1d 60 volatile float left_speed = 0;
intgsull 4:453d534b1c1d 61 volatile float right_speed = 0;
intgsull 1:5b9fa1823663 62
intgsull 4:453d534b1c1d 63 // pid constants. these need to be tuned. but i set them as a default val for now.
intgsull 4:453d534b1c1d 64 // line refers to the translational speed.
intgsull 4:453d534b1c1d 65 // enco and gyro will be used primarily for angular speed.
intgsull 4:453d534b1c1d 66 // (we can change the names later,
intgsull 4:453d534b1c1d 67 // i added line in after i realized that i already had the angular code)
intgsull 6:32d9b855b90f 68 const int max_speed = 6; // max speed is 6 encoder pulses per ms.
intgsull 6:32d9b855b90f 69
intgsull 6:32d9b855b90f 70 const float line_propo = 0;
aduriseti 5:f9837617817b 71 const float line_integ = 0;
aduriseti 5:f9837617817b 72 const float line_deriv = 0;
intgsull 2:5f9b78950a17 73
intgsull 4:453d534b1c1d 74 const float gyro_propo = 1;
aduriseti 5:f9837617817b 75 const float gyro_integ = 0;
aduriseti 5:f9837617817b 76 const float gyro_deriv = 0;
intgsull 4:453d534b1c1d 77
intgsull 4:453d534b1c1d 78 const float enco_propo = 1;
aduriseti 5:f9837617817b 79 const float enco_integ = 0;
aduriseti 5:f9837617817b 80 const float enco_deriv = 0;
intgsull 4:453d534b1c1d 81
intgsull 4:453d534b1c1d 82 const float spin_enco_weight = 0.5;
intgsull 4:453d534b1c1d 83 const float spin_gyro_weight = 1 - spin_enco_weight;
intgsull 1:5b9fa1823663 84
intgsull 4:453d534b1c1d 85 // this is just so that we can maintain what state our mouse is in.
intgsull 4:453d534b1c1d 86 // currently this has no real use, but it may in the future.
intgsull 4:453d534b1c1d 87 // or we could just remove this entirely.
intgsull 4:453d534b1c1d 88 typedef enum
intgsull 4:453d534b1c1d 89 {
intgsull 4:453d534b1c1d 90 STOPPED,
intgsull 4:453d534b1c1d 91 FORWARD,
intgsull 4:453d534b1c1d 92 TURNING,
intgsull 4:453d534b1c1d 93 UNKNOWN
intgsull 4:453d534b1c1d 94 } STATE;
intgsull 6:32d9b855b90f 95 volatile STATE mouse_state;
intgsull 4:453d534b1c1d 96
intgsull 4:453d534b1c1d 97 // helper functions
intgsull 4:453d534b1c1d 98 void reset();
intgsull 4:453d534b1c1d 99 void offsetCalc();
intgsull 4:453d534b1c1d 100 void stop();
intgsull 4:453d534b1c1d 101 void moveForward();
intgsull 4:453d534b1c1d 102 void turn();
intgsull 9:e2feaadf504c 103 void irReset();
intgsull 6:32d9b855b90f 104
intgsull 6:32d9b855b90f 105 //irPID function to not crash into stuff
intgsull 6:32d9b855b90f 106
intgsull 6:32d9b855b90f 107 const float leftWall= 0;
intgsull 6:32d9b855b90f 108 const float rightWall= 0; //we need to find the threshold value for when
intgsull 6:32d9b855b90f 109 //left or right walls are present
intgsull 6:32d9b855b90f 110 const float irOffset= 0; // difference between right and left ir sensors when
intgsull 6:32d9b855b90f 111 //mouse is in the middle
intgsull 6:32d9b855b90f 112 const float lirOffset= 0; //middle value for when there is only a wall on one
intgsull 6:32d9b855b90f 113 const float rirOffset= 0; //side of the mouse (left and right)
intgsull 6:32d9b855b90f 114 volatile float irError = 0;
intgsull 6:32d9b855b90f 115 volatile float irErrorD = 0;
intgsull 6:32d9b855b90f 116 volatile float irErrorI = 0;
intgsull 6:32d9b855b90f 117 volatile float oldirError = 0;
intgsull 6:32d9b855b90f 118 volatile float totalirError= 0; //errors
intgsull 6:32d9b855b90f 119 const float irKp = 0;
intgsull 6:32d9b855b90f 120 const float irKd = 0;
intgsull 6:32d9b855b90f 121 const float irKi = 0; //constants
intgsull 6:32d9b855b90f 122 volatile float leftDistance = 0;
intgsull 6:32d9b855b90f 123 volatile float rightDistance= 0;
intgsull 6:32d9b855b90f 124 void irPID()
intgsull 6:32d9b855b90f 125 {
intgsull 6:32d9b855b90f 126 eLS = 1;
intgsull 6:32d9b855b90f 127 eRS = 1;
intgsull 6:32d9b855b90f 128 if(rLS > leftWall && rRS > rightWall)//walls on both sides
intgsull 6:32d9b855b90f 129 {
intgsull 9:e2feaadf504c 130 //leftDistance = rLS;
intgsull 9:e2feaadf504c 131 // rightDistance = rRS;
intgsull 9:e2feaadf504c 132 irError = rRS-rLS-irOffset; //– leftDistance – irOffset;
intgsull 6:32d9b855b90f 133
intgsull 9:e2feaadf504c 134 irErrorD = irError-oldirError;
intgsull 9:e2feaadf504c 135 //irErrorD -= oldirError;//(irError – oldirError);
intgsull 6:32d9b855b90f 136
intgsull 6:32d9b855b90f 137 irErrorI += irError;
intgsull 6:32d9b855b90f 138 }
intgsull 6:32d9b855b90f 139 else if(rLS > leftWall) //just left wall
intgsull 6:32d9b855b90f 140 {
intgsull 6:32d9b855b90f 141 leftDistance = rLS;
intgsull 7:f1bceb2bab70 142 irError = 2*(lirOffset-leftDistance);//(2 * (lirOffset – leftDistance));
intgsull 6:32d9b855b90f 143
intgsull 7:f1bceb2bab70 144 irErrorD=irError-oldirError;
intgsull 7:f1bceb2bab70 145
intgsull 7:f1bceb2bab70 146 irErrorI += irError;
intgsull 6:32d9b855b90f 147 }
intgsull 6:32d9b855b90f 148 else if(rRS > rightWall)//just right wall
intgsull 6:32d9b855b90f 149 {
intgsull 7:f1bceb2bab70 150 rightDistance = rRS;
intgsull 8:a254346f20aa 151 irError=2*(rightDistance-rirOffset);
intgsull 8:a254346f20aa 152 irError = irError-oldirError;
intgsull 6:32d9b855b90f 153 irErrorI += irError;
intgsull 6:32d9b855b90f 154 }
intgsull 6:32d9b855b90f 155 else if(rLS < leftWall && rRS < rightWall)//no walls!! Use encoder PID
intgsull 6:32d9b855b90f 156 {
intgsull 6:32d9b855b90f 157 irError = 0;
intgsull 6:32d9b855b90f 158 irErrorD = 0;
intgsull 6:32d9b855b90f 159 irErrorI += irError;
intgsull 6:32d9b855b90f 160 }
intgsull 6:32d9b855b90f 161 totalirError = irError*irKp + irErrorD*irKd + irErrorI*irKi;
intgsull 6:32d9b855b90f 162 oldirError = irError;
intgsull 6:32d9b855b90f 163 left_speed -= totalirError;
intgsull 6:32d9b855b90f 164 right_speed += totalirError;
intgsull 6:32d9b855b90f 165 eLS = 0;
intgsull 6:32d9b855b90f 166 eRS = 0;
intgsull 4:453d534b1c1d 167 }
intgsull 4:453d534b1c1d 168
intgsull 6:32d9b855b90f 169 const float frontWall = 0; //need to calibrate this threshold to a value where mouse can stop in time
intgsull 6:32d9b855b90f 170 //something like this may be useful for the very temporal present
intgsull 6:32d9b855b90f 171 //doesn't account for current speed/trajectory atm
intgsull 6:32d9b855b90f 172 void dontDriveStraightIntoAWall()
intgsull 4:453d534b1c1d 173 {
intgsull 6:32d9b855b90f 174 eRF = 1;
intgsull 6:32d9b855b90f 175 eLF = 1;
intgsull 6:32d9b855b90f 176 if(rRF > frontWall || rLF > frontWall)
intgsull 6:32d9b855b90f 177 {
intgsull 6:32d9b855b90f 178 eRF = 0;
intgsull 6:32d9b855b90f 179 eLF = 0;
intgsull 6:32d9b855b90f 180 mouse_state = STOPPED;
intgsull 6:32d9b855b90f 181 stop();
intgsull 6:32d9b855b90f 182 return;
intgsull 6:32d9b855b90f 183 }
intgsull 4:453d534b1c1d 184 }
intgsull 2:5f9b78950a17 185
intgsull 4:453d534b1c1d 186 // PID Control
intgsull 4:453d534b1c1d 187 // this contains the simplistic PID control for the most part.
intgsull 4:453d534b1c1d 188 // we do have to calibrate constants though.
intgsull 4:453d534b1c1d 189 void systick()
intgsull 4:453d534b1c1d 190 {
intgsull 6:32d9b855b90f 191 //pc.printf("systick ran\r\n");
intgsull 4:453d534b1c1d 192 if ( mouse_state == STOPPED || mouse_state == UNKNOWN )
intgsull 4:453d534b1c1d 193 {
intgsull 4:453d534b1c1d 194 // do nothing?
intgsull 4:453d534b1c1d 195 // reset?
intgsull 6:32d9b855b90f 196 //reset();
intgsull 4:453d534b1c1d 197 offsetCalc();
intgsull 4:453d534b1c1d 198 return;
intgsull 2:5f9b78950a17 199 }
intgsull 6:32d9b855b90f 200 //pc.printf("systick ran while state is FORWARD \r\n");
intgsull 2:5f9b78950a17 201
intgsull 4:453d534b1c1d 202 int dt = timer.read_us(); // should be around 1 ms.
intgsull 4:453d534b1c1d 203 timer.reset();
intgsull 4:453d534b1c1d 204
intgsull 6:32d9b855b90f 205 //printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
intgsull 6:32d9b855b90f 206
intgsull 6:32d9b855b90f 207 float line_error = line_speed - 0.5 * ( l_enco.getPulses() + r_enco.getPulses());
intgsull 4:453d534b1c1d 208 int enco_error = l_enco.getPulses() - r_enco.getPulses();
intgsull 4:453d534b1c1d 209 float gyro_error = _gyro.read() - gyro_offset;
intgsull 4:453d534b1c1d 210
intgsull 4:453d534b1c1d 211 line_accumulator += line_error;
intgsull 4:453d534b1c1d 212 enco_accumulator += enco_error;
intgsull 4:453d534b1c1d 213 gyro_accumulator += gyro_error;
intgsull 4:453d534b1c1d 214
intgsull 4:453d534b1c1d 215 float line_pid = 0;
intgsull 4:453d534b1c1d 216 line_pid += line_propo * line_error;
intgsull 4:453d534b1c1d 217 line_pid += line_integ * line_accumulator;
intgsull 4:453d534b1c1d 218 line_pid += line_deriv * (line_error - line_prevError)/dt;
intgsull 4:453d534b1c1d 219
intgsull 4:453d534b1c1d 220 float enco_pid = 0;
intgsull 4:453d534b1c1d 221 enco_pid += enco_propo * enco_error;
intgsull 4:453d534b1c1d 222 enco_pid += enco_integ * enco_accumulator;
intgsull 4:453d534b1c1d 223 enco_pid += enco_deriv * (enco_error - enco_prevError)/dt;
intgsull 4:453d534b1c1d 224
intgsull 4:453d534b1c1d 225 float gyro_pid = 0;
intgsull 4:453d534b1c1d 226 gyro_pid += gyro_propo * gyro_error;
intgsull 4:453d534b1c1d 227 gyro_pid += gyro_integ * gyro_accumulator;
intgsull 6:32d9b855b90f 228 gyro_pid += gyro_deriv *(gyro_error - gyro_prevError)/dt;
intgsull 4:453d534b1c1d 229
intgsull 4:453d534b1c1d 230 // forward moving pid control.
intgsull 4:453d534b1c1d 231 if ( mouse_state == FORWARD )
intgsull 4:453d534b1c1d 232 {
intgsull 4:453d534b1c1d 233 float x_error = line_pid;
intgsull 6:32d9b855b90f 234 float w_error = 0; //spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
intgsull 6:32d9b855b90f 235 left_speed += (x_error - w_error);
intgsull 6:32d9b855b90f 236 right_speed += (x_error + w_error); //swapped
aduriseti 5:f9837617817b 237
intgsull 6:32d9b855b90f 238 //pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
aduriseti 5:f9837617817b 239
aduriseti 5:f9837617817b 240 moveForward();
intgsull 4:453d534b1c1d 241 // offsetCalc();
intgsull 4:453d534b1c1d 242 }
intgsull 4:453d534b1c1d 243 if ( mouse_state == TURNING )
intgsull 4:453d534b1c1d 244 {
intgsull 4:453d534b1c1d 245 // nothing for now. if we turn in place, we assume no pid control.
intgsull 4:453d534b1c1d 246 // this may have to change when we try curve turns.
intgsull 4:453d534b1c1d 247 }
intgsull 4:453d534b1c1d 248
intgsull 4:453d534b1c1d 249 line_prevError = line_error;
intgsull 4:453d534b1c1d 250 enco_prevError = enco_error;
intgsull 4:453d534b1c1d 251 gyro_prevError = gyro_error;
intgsull 4:453d534b1c1d 252
intgsull 4:453d534b1c1d 253 line_accumulator /= line_decayFactor;
intgsull 4:453d534b1c1d 254 enco_accumulator /= enco_decayFactor;
intgsull 4:453d534b1c1d 255 gyro_accumulator /= gyro_decayFactor;
aduriseti 5:f9837617817b 256
aduriseti 5:f9837617817b 257 reset();
intgsull 4:453d534b1c1d 258 }
intgsull 4:453d534b1c1d 259
intgsull 4:453d534b1c1d 260 // setup stuff.
intgsull 4:453d534b1c1d 261 void setup()
intgsull 1:5b9fa1823663 262 {
aduriseti 5:f9837617817b 263 pc.printf("Hello World\r\n");
intgsull 6:32d9b855b90f 264 pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
intgsull 6:32d9b855b90f 265 pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
intgsull 4:453d534b1c1d 266 mouse_state = STOPPED;
intgsull 4:453d534b1c1d 267
intgsull 6:32d9b855b90f 268 reset();
intgsull 6:32d9b855b90f 269
intgsull 4:453d534b1c1d 270 eRS = 0;
intgsull 4:453d534b1c1d 271 eRF = 0;
intgsull 4:453d534b1c1d 272 eLS = 0;
intgsull 4:453d534b1c1d 273 eLF = 0;
intgsull 4:453d534b1c1d 274
intgsull 4:453d534b1c1d 275 myled = 1;
intgsull 1:5b9fa1823663 276
intgsull 4:453d534b1c1d 277 // repeat this for some time frame.
aduriseti 5:f9837617817b 278 for ( int i = 0; i < 200; i++ )
aduriseti 5:f9837617817b 279 offsetCalc();
intgsull 4:453d534b1c1d 280 Systicker.attach_us(&systick, 1000);
intgsull 4:453d534b1c1d 281 }
intgsull 4:453d534b1c1d 282
intgsull 4:453d534b1c1d 283 void reset()
intgsull 4:453d534b1c1d 284 {
aduriseti 5:f9837617817b 285 l_enco.reset();
aduriseti 5:f9837617817b 286 r_enco.reset();
intgsull 1:5b9fa1823663 287 }
intgsull 1:5b9fa1823663 288
intgsull 1:5b9fa1823663 289
intgsull 4:453d534b1c1d 290 // computes gyro_offset
intgsull 4:453d534b1c1d 291 // uses a "weighted" average.
intgsull 4:453d534b1c1d 292 // idea is that the current gyro offset is weighted more than previous ones.
intgsull 4:453d534b1c1d 293 // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n).
intgsull 4:453d534b1c1d 294 // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.)
intgsull 4:453d534b1c1d 295 // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight.
intgsull 4:453d534b1c1d 296 // currently this is only in the setup function. we can run this when the mouse is running in a line
intgsull 4:453d534b1c1d 297 // when we figure out good line running pid.
intgsull 4:453d534b1c1d 298 void offsetCalc()
intgsull 4:453d534b1c1d 299 {
intgsull 4:453d534b1c1d 300 gyro_offset = gyro_offset / 2 + _gyro.read() / 2;
intgsull 4:453d534b1c1d 301 }
intgsull 1:5b9fa1823663 302
intgsull 4:453d534b1c1d 303
intgsull 0:fa523db3f4f5 304
intgsull 4:453d534b1c1d 305 int main()
intgsull 4:453d534b1c1d 306 {
intgsull 4:453d534b1c1d 307 setup();
intgsull 6:32d9b855b90f 308 pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
intgsull 6:32d9b855b90f 309
intgsull 6:32d9b855b90f 310 wait(2);
aduriseti 5:f9837617817b 311
intgsull 6:32d9b855b90f 312 mouse_state = FORWARD;
intgsull 6:32d9b855b90f 313 for ( int i = 0; i < 10; i++ )
intgsull 6:32d9b855b90f 314 {
intgsull 6:32d9b855b90f 315 pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
intgsull 6:32d9b855b90f 316 pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
intgsull 6:32d9b855b90f 317 }
aduriseti 5:f9837617817b 318 stop();
aduriseti 5:f9837617817b 319
intgsull 6:32d9b855b90f 320 //pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
aduriseti 5:f9837617817b 321
aduriseti 5:f9837617817b 322 //while(1)
aduriseti 5:f9837617817b 323 // {
aduriseti 5:f9837617817b 324 // pc.printf("The left motor is going at speed: %d\r\n", left_speed);
aduriseti 5:f9837617817b 325 // pc.printf("The left motor is going at speed: %d\r\n", right_speed);
aduriseti 5:f9837617817b 326 // wait(1);
aduriseti 5:f9837617817b 327 // }
intgsull 0:fa523db3f4f5 328 }
intgsull 0:fa523db3f4f5 329
intgsull 4:453d534b1c1d 330
intgsull 4:453d534b1c1d 331 // movement functions.
intgsull 4:453d534b1c1d 332 void moveForward()
intgsull 4:453d534b1c1d 333 {
intgsull 6:32d9b855b90f 334 //mouse_state = FORWARD;
intgsull 4:453d534b1c1d 335
intgsull 4:453d534b1c1d 336 if ( left_speed > 0 ) // which should be always.
intgsull 4:453d534b1c1d 337 {
intgsull 6:32d9b855b90f 338 motor1_forward = left_speed / max_speed;
intgsull 4:453d534b1c1d 339 motor1_reverse = 0;
intgsull 4:453d534b1c1d 340 }
intgsull 4:453d534b1c1d 341 else
intgsull 4:453d534b1c1d 342 {
intgsull 4:453d534b1c1d 343 motor1_forward = 0;
intgsull 6:32d9b855b90f 344 motor1_reverse = -left_speed / max_speed;
intgsull 0:fa523db3f4f5 345 }
intgsull 0:fa523db3f4f5 346
intgsull 4:453d534b1c1d 347 if ( right_speed > 0 ) // which again should be always.
intgsull 4:453d534b1c1d 348 {
intgsull 6:32d9b855b90f 349 motor2_forward = right_speed / max_speed;
intgsull 0:fa523db3f4f5 350 motor2_reverse = 0;
intgsull 0:fa523db3f4f5 351 }
intgsull 4:453d534b1c1d 352 else
intgsull 4:453d534b1c1d 353 {
intgsull 0:fa523db3f4f5 354 motor2_forward = 0;
intgsull 6:32d9b855b90f 355 motor2_reverse = -right_speed / max_speed;
intgsull 0:fa523db3f4f5 356 }
intgsull 4:453d534b1c1d 357 }
intgsull 4:453d534b1c1d 358
intgsull 4:453d534b1c1d 359 void stop()
intgsull 4:453d534b1c1d 360 {
intgsull 4:453d534b1c1d 361 mouse_state = STOPPED;
intgsull 4:453d534b1c1d 362
aduriseti 5:f9837617817b 363
intgsull 4:453d534b1c1d 364 motor1_forward = 1.0;
intgsull 4:453d534b1c1d 365 motor1_reverse = 1.0;
intgsull 4:453d534b1c1d 366 motor2_forward = 1.0;
intgsull 4:453d534b1c1d 367 motor2_reverse = 1.0;
aduriseti 5:f9837617817b 368
aduriseti 5:f9837617817b 369
intgsull 4:453d534b1c1d 370 }
intgsull 4:453d534b1c1d 371
aduriseti 5:f9837617817b 372 void turn()// maybe split this into two functions?
aduriseti 5:f9837617817b 373 {
aduriseti 5:f9837617817b 374 mouse_state = TURNING;
aduriseti 5:f9837617817b 375 float angle = 0;
intgsull 9:e2feaadf504c 376 while (angle < 0.90)
aduriseti 5:f9837617817b 377 {
aduriseti 5:f9837617817b 378 float gyro_val = _gyro.read() - gyro_offset;
aduriseti 5:f9837617817b 379 angle += gyro_val;
aduriseti 5:f9837617817b 380
aduriseti 5:f9837617817b 381 pc.printf("%f\r\n", angle);
aduriseti 5:f9837617817b 382
aduriseti 5:f9837617817b 383 motor1_forward = 0.5;
aduriseti 5:f9837617817b 384 motor1_reverse = 0;
aduriseti 5:f9837617817b 385 motor2_forward = 0;
aduriseti 5:f9837617817b 386 motor2_reverse = 0.5;
aduriseti 5:f9837617817b 387 }
intgsull 9:e2feaadf504c 388 }
intgsull 9:e2feaadf504c 389 void irReset() //maybe split resets
intgsull 9:e2feaadf504c 390 {
intgsull 9:e2feaadf504c 391 eRF = 0;
intgsull 9:e2feaadf504c 392 eRS = 0;
intgsull 9:e2feaadf504c 393 eLF = 0;
intgsull 9:e2feaadf504c 394 eRF = 0;
intgsull 9:e2feaadf504c 395 }