2015-2016_Mouserat / Lab1_Test

hello

Dependencies:   AVEncoder QEI mbed-src-AV

main.cpp@6:32d9b855b90f, 2015-11-18 (annotated)

Committer:
intgsull
Date:
Wed Nov 18 06:49:06 2015 +0000
Revision:
6:32d9b855b90f
Parent:
5:f9837617817b
Child:
7:f1bceb2bab70
tryna fix irPID

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 1:5b9fa1823663 103
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 6:32d9b855b90f 130 leftDistance = rLS;
intgsull 6:32d9b855b90f 131 rightDistance = rRS;
intgsull 6:32d9b855b90f 132 irError = rightDistance; //– leftDistance – irOffset;
intgsull 6:32d9b855b90f 133 irError -= leftDistance;
intgsull 6:32d9b855b90f 134 irError -= irOffset;
intgsull 6:32d9b855b90f 135
intgsull 6:32d9b855b90f 136 irErrorD = irError;
intgsull 6:32d9b855b90f 137 irErrorD -= oldirError;//(irError – oldirError);
intgsull 6:32d9b855b90f 138
intgsull 6:32d9b855b90f 139 irErrorI += irError;
intgsull 6:32d9b855b90f 140 }
intgsull 6:32d9b855b90f 141 else if(rLS > leftWall) //just left wall
intgsull 6:32d9b855b90f 142 {
intgsull 6:32d9b855b90f 143 leftDistance = rLS;
intgsull 6:32d9b855b90f 144 irError = lirOffset;//(2 * (lirOffset – leftDistance));
intgsull 6:32d9b855b90f 145
intgsull 6:32d9b855b90f 146 irErrorD = (irError – oldirError);
intgsull 6:32d9b855b90f 147 irErrorI += irError;
intgsull 6:32d9b855b90f 148 }
intgsull 6:32d9b855b90f 149 else if(rRS > rightWall)//just right wall
intgsull 6:32d9b855b90f 150 {
intgsull 6:32d9b855b90f 151 rightDistance = rRS();
intgsull 6:32d9b855b90f 152 irError = (2 * (rightDistance – rirOffset));
intgsull 6:32d9b855b90f 153 irErrorD = (irError – oldirError);
intgsull 6:32d9b855b90f 154 irErrorI += irError;
intgsull 6:32d9b855b90f 155 }
intgsull 6:32d9b855b90f 156 else if(rLS < leftWall && rRS < rightWall)//no walls!! Use encoder PID
intgsull 6:32d9b855b90f 157 {
intgsull 6:32d9b855b90f 158 irError = 0;
intgsull 6:32d9b855b90f 159 irErrorD = 0;
intgsull 6:32d9b855b90f 160 irErrorI += irError;
intgsull 6:32d9b855b90f 161 }
intgsull 6:32d9b855b90f 162 totalirError = irError*irKp + irErrorD*irKd + irErrorI*irKi;
intgsull 6:32d9b855b90f 163 oldirError = irError;
intgsull 6:32d9b855b90f 164 left_speed -= totalirError;
intgsull 6:32d9b855b90f 165 right_speed += totalirError;
intgsull 6:32d9b855b90f 166 eLS = 0;
intgsull 6:32d9b855b90f 167 eRS = 0;
intgsull 4:453d534b1c1d 168 }
intgsull 4:453d534b1c1d 169
intgsull 6:32d9b855b90f 170 const float frontWall = 0; //need to calibrate this threshold to a value where mouse can stop in time
intgsull 6:32d9b855b90f 171 //something like this may be useful for the very temporal present
intgsull 6:32d9b855b90f 172 //doesn't account for current speed/trajectory atm
intgsull 6:32d9b855b90f 173 void dontDriveStraightIntoAWall()
intgsull 4:453d534b1c1d 174 {
intgsull 6:32d9b855b90f 175 eRF = 1;
intgsull 6:32d9b855b90f 176 eLF = 1;
intgsull 6:32d9b855b90f 177 if(rRF > frontWall || rLF > frontWall)
intgsull 6:32d9b855b90f 178 {
intgsull 6:32d9b855b90f 179 eRF = 0;
intgsull 6:32d9b855b90f 180 eLF = 0;
intgsull 6:32d9b855b90f 181 mouse_state = STOPPED;
intgsull 6:32d9b855b90f 182 stop();
intgsull 6:32d9b855b90f 183 return;
intgsull 6:32d9b855b90f 184 }
intgsull 4:453d534b1c1d 185 }
intgsull 2:5f9b78950a17 186
intgsull 4:453d534b1c1d 187 // PID Control
intgsull 4:453d534b1c1d 188 // this contains the simplistic PID control for the most part.
intgsull 4:453d534b1c1d 189 // we do have to calibrate constants though.
intgsull 4:453d534b1c1d 190 void systick()
intgsull 4:453d534b1c1d 191 {
intgsull 6:32d9b855b90f 192 //pc.printf("systick ran\r\n");
intgsull 4:453d534b1c1d 193 if ( mouse_state == STOPPED || mouse_state == UNKNOWN )
intgsull 4:453d534b1c1d 194 {
intgsull 4:453d534b1c1d 195 // do nothing?
intgsull 4:453d534b1c1d 196 // reset?
intgsull 6:32d9b855b90f 197 //reset();
intgsull 4:453d534b1c1d 198 offsetCalc();
intgsull 4:453d534b1c1d 199 return;
intgsull 2:5f9b78950a17 200 }
intgsull 6:32d9b855b90f 201 //pc.printf("systick ran while state is FORWARD \r\n");
intgsull 2:5f9b78950a17 202
intgsull 4:453d534b1c1d 203 int dt = timer.read_us(); // should be around 1 ms.
intgsull 4:453d534b1c1d 204 timer.reset();
intgsull 4:453d534b1c1d 205
intgsull 6:32d9b855b90f 206 //printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
intgsull 6:32d9b855b90f 207
intgsull 6:32d9b855b90f 208 float line_error = line_speed - 0.5 * ( l_enco.getPulses() + r_enco.getPulses());
intgsull 4:453d534b1c1d 209 int enco_error = l_enco.getPulses() - r_enco.getPulses();
intgsull 4:453d534b1c1d 210 float gyro_error = _gyro.read() - gyro_offset;
intgsull 4:453d534b1c1d 211
intgsull 4:453d534b1c1d 212 line_accumulator += line_error;
intgsull 4:453d534b1c1d 213 enco_accumulator += enco_error;
intgsull 4:453d534b1c1d 214 gyro_accumulator += gyro_error;
intgsull 4:453d534b1c1d 215
intgsull 4:453d534b1c1d 216 float line_pid = 0;
intgsull 4:453d534b1c1d 217 line_pid += line_propo * line_error;
intgsull 4:453d534b1c1d 218 line_pid += line_integ * line_accumulator;
intgsull 4:453d534b1c1d 219 line_pid += line_deriv * (line_error - line_prevError)/dt;
intgsull 4:453d534b1c1d 220
intgsull 4:453d534b1c1d 221 float enco_pid = 0;
intgsull 4:453d534b1c1d 222 enco_pid += enco_propo * enco_error;
intgsull 4:453d534b1c1d 223 enco_pid += enco_integ * enco_accumulator;
intgsull 4:453d534b1c1d 224 enco_pid += enco_deriv * (enco_error - enco_prevError)/dt;
intgsull 4:453d534b1c1d 225
intgsull 4:453d534b1c1d 226 float gyro_pid = 0;
intgsull 4:453d534b1c1d 227 gyro_pid += gyro_propo * gyro_error;
intgsull 4:453d534b1c1d 228 gyro_pid += gyro_integ * gyro_accumulator;
intgsull 6:32d9b855b90f 229 gyro_pid += gyro_deriv *(gyro_error - gyro_prevError)/dt;
intgsull 4:453d534b1c1d 230
intgsull 4:453d534b1c1d 231 // forward moving pid control.
intgsull 4:453d534b1c1d 232 if ( mouse_state == FORWARD )
intgsull 4:453d534b1c1d 233 {
intgsull 4:453d534b1c1d 234 float x_error = line_pid;
intgsull 6:32d9b855b90f 235 float w_error = 0; //spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
intgsull 6:32d9b855b90f 236 left_speed += (x_error - w_error);
intgsull 6:32d9b855b90f 237 right_speed += (x_error + w_error); //swapped
aduriseti 5:f9837617817b 238
intgsull 6:32d9b855b90f 239 //pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
aduriseti 5:f9837617817b 240
aduriseti 5:f9837617817b 241 moveForward();
intgsull 4:453d534b1c1d 242 // offsetCalc();
intgsull 4:453d534b1c1d 243 }
intgsull 4:453d534b1c1d 244 if ( mouse_state == TURNING )
intgsull 4:453d534b1c1d 245 {
intgsull 4:453d534b1c1d 246 // nothing for now. if we turn in place, we assume no pid control.
intgsull 4:453d534b1c1d 247 // this may have to change when we try curve turns.
intgsull 4:453d534b1c1d 248 }
intgsull 4:453d534b1c1d 249
intgsull 4:453d534b1c1d 250 line_prevError = line_error;
intgsull 4:453d534b1c1d 251 enco_prevError = enco_error;
intgsull 4:453d534b1c1d 252 gyro_prevError = gyro_error;
intgsull 4:453d534b1c1d 253
intgsull 4:453d534b1c1d 254 line_accumulator /= line_decayFactor;
intgsull 4:453d534b1c1d 255 enco_accumulator /= enco_decayFactor;
intgsull 4:453d534b1c1d 256 gyro_accumulator /= gyro_decayFactor;
aduriseti 5:f9837617817b 257
aduriseti 5:f9837617817b 258 reset();
intgsull 4:453d534b1c1d 259 }
intgsull 4:453d534b1c1d 260
intgsull 4:453d534b1c1d 261 // setup stuff.
intgsull 4:453d534b1c1d 262 void setup()
intgsull 1:5b9fa1823663 263 {
aduriseti 5:f9837617817b 264 pc.printf("Hello World\r\n");
intgsull 6:32d9b855b90f 265 pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
intgsull 6:32d9b855b90f 266 pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
intgsull 4:453d534b1c1d 267 mouse_state = STOPPED;
intgsull 4:453d534b1c1d 268
intgsull 6:32d9b855b90f 269 reset();
intgsull 6:32d9b855b90f 270
intgsull 4:453d534b1c1d 271 eRS = 0;
intgsull 4:453d534b1c1d 272 eRF = 0;
intgsull 4:453d534b1c1d 273 eLS = 0;
intgsull 4:453d534b1c1d 274 eLF = 0;
intgsull 4:453d534b1c1d 275
intgsull 4:453d534b1c1d 276 myled = 1;
intgsull 1:5b9fa1823663 277
intgsull 4:453d534b1c1d 278 // repeat this for some time frame.
aduriseti 5:f9837617817b 279 for ( int i = 0; i < 200; i++ )
aduriseti 5:f9837617817b 280 offsetCalc();
intgsull 4:453d534b1c1d 281 Systicker.attach_us(&systick, 1000);
intgsull 4:453d534b1c1d 282 }
intgsull 4:453d534b1c1d 283
intgsull 4:453d534b1c1d 284 void reset()
intgsull 4:453d534b1c1d 285 {
aduriseti 5:f9837617817b 286 l_enco.reset();
aduriseti 5:f9837617817b 287 r_enco.reset();
intgsull 1:5b9fa1823663 288 }
intgsull 1:5b9fa1823663 289
intgsull 1:5b9fa1823663 290
intgsull 4:453d534b1c1d 291 // computes gyro_offset
intgsull 4:453d534b1c1d 292 // uses a "weighted" average.
intgsull 4:453d534b1c1d 293 // idea is that the current gyro offset is weighted more than previous ones.
intgsull 4:453d534b1c1d 294 // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n).
intgsull 4:453d534b1c1d 295 // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.)
intgsull 4:453d534b1c1d 296 // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight.
intgsull 4:453d534b1c1d 297 // currently this is only in the setup function. we can run this when the mouse is running in a line
intgsull 4:453d534b1c1d 298 // when we figure out good line running pid.
intgsull 4:453d534b1c1d 299 void offsetCalc()
intgsull 4:453d534b1c1d 300 {
intgsull 4:453d534b1c1d 301 gyro_offset = gyro_offset / 2 + _gyro.read() / 2;
intgsull 4:453d534b1c1d 302 }
intgsull 1:5b9fa1823663 303
intgsull 4:453d534b1c1d 304
intgsull 0:fa523db3f4f5 305
intgsull 4:453d534b1c1d 306 int main()
intgsull 4:453d534b1c1d 307 {
intgsull 4:453d534b1c1d 308 setup();
intgsull 6:32d9b855b90f 309 pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
intgsull 6:32d9b855b90f 310
intgsull 6:32d9b855b90f 311 wait(2);
aduriseti 5:f9837617817b 312
intgsull 6:32d9b855b90f 313 mouse_state = FORWARD;
intgsull 6:32d9b855b90f 314 for ( int i = 0; i < 10; i++ )
intgsull 6:32d9b855b90f 315 {
intgsull 6:32d9b855b90f 316 pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
intgsull 6:32d9b855b90f 317 pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
intgsull 6:32d9b855b90f 318 }
aduriseti 5:f9837617817b 319 stop();
aduriseti 5:f9837617817b 320
intgsull 6:32d9b855b90f 321 //pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
aduriseti 5:f9837617817b 322
aduriseti 5:f9837617817b 323 //while(1)
aduriseti 5:f9837617817b 324 // {
aduriseti 5:f9837617817b 325 // pc.printf("The left motor is going at speed: %d\r\n", left_speed);
aduriseti 5:f9837617817b 326 // pc.printf("The left motor is going at speed: %d\r\n", right_speed);
aduriseti 5:f9837617817b 327 // wait(1);
aduriseti 5:f9837617817b 328 // }
intgsull 0:fa523db3f4f5 329 }
intgsull 0:fa523db3f4f5 330
intgsull 4:453d534b1c1d 331
intgsull 4:453d534b1c1d 332 // movement functions.
intgsull 4:453d534b1c1d 333 void moveForward()
intgsull 4:453d534b1c1d 334 {
intgsull 6:32d9b855b90f 335 //mouse_state = FORWARD;
intgsull 4:453d534b1c1d 336
intgsull 4:453d534b1c1d 337 if ( left_speed > 0 ) // which should be always.
intgsull 4:453d534b1c1d 338 {
intgsull 6:32d9b855b90f 339 motor1_forward = left_speed / max_speed;
intgsull 4:453d534b1c1d 340 motor1_reverse = 0;
intgsull 4:453d534b1c1d 341 }
intgsull 4:453d534b1c1d 342 else
intgsull 4:453d534b1c1d 343 {
intgsull 4:453d534b1c1d 344 motor1_forward = 0;
intgsull 6:32d9b855b90f 345 motor1_reverse = -left_speed / max_speed;
intgsull 0:fa523db3f4f5 346 }
intgsull 0:fa523db3f4f5 347
intgsull 4:453d534b1c1d 348 if ( right_speed > 0 ) // which again should be always.
intgsull 4:453d534b1c1d 349 {
intgsull 6:32d9b855b90f 350 motor2_forward = right_speed / max_speed;
intgsull 0:fa523db3f4f5 351 motor2_reverse = 0;
intgsull 0:fa523db3f4f5 352 }
intgsull 4:453d534b1c1d 353 else
intgsull 4:453d534b1c1d 354 {
intgsull 0:fa523db3f4f5 355 motor2_forward = 0;
intgsull 6:32d9b855b90f 356 motor2_reverse = -right_speed / max_speed;
intgsull 0:fa523db3f4f5 357 }
intgsull 4:453d534b1c1d 358 }
intgsull 4:453d534b1c1d 359
intgsull 4:453d534b1c1d 360 void stop()
intgsull 4:453d534b1c1d 361 {
intgsull 4:453d534b1c1d 362 mouse_state = STOPPED;
intgsull 4:453d534b1c1d 363
aduriseti 5:f9837617817b 364
intgsull 4:453d534b1c1d 365 motor1_forward = 1.0;
intgsull 4:453d534b1c1d 366 motor1_reverse = 1.0;
intgsull 4:453d534b1c1d 367 motor2_forward = 1.0;
intgsull 4:453d534b1c1d 368 motor2_reverse = 1.0;
aduriseti 5:f9837617817b 369
aduriseti 5:f9837617817b 370
intgsull 4:453d534b1c1d 371 }
intgsull 4:453d534b1c1d 372
aduriseti 5:f9837617817b 373 void turn()// maybe split this into two functions?
aduriseti 5:f9837617817b 374 {
aduriseti 5:f9837617817b 375 mouse_state = TURNING;
aduriseti 5:f9837617817b 376 float angle = 0;
aduriseti 5:f9837617817b 377 while (angle < 0.9)
aduriseti 5:f9837617817b 378 {
aduriseti 5:f9837617817b 379 float gyro_val = _gyro.read() - gyro_offset;
aduriseti 5:f9837617817b 380 angle += gyro_val;
aduriseti 5:f9837617817b 381
aduriseti 5:f9837617817b 382 pc.printf("%f\r\n", angle);
aduriseti 5:f9837617817b 383
aduriseti 5:f9837617817b 384 motor1_forward = 0.5;
aduriseti 5:f9837617817b 385 motor1_reverse = 0;
aduriseti 5:f9837617817b 386 motor2_forward = 0;
aduriseti 5:f9837617817b 387 motor2_reverse = 0.5;
aduriseti 5:f9837617817b 388 }
aduriseti 5:f9837617817b 389 }