2015-2016_Mouserat / Lab1_Test

hello

Dependencies:   AVEncoder QEI mbed-src-AV

main.cpp@8:a254346f20aa, 2015-11-18 (annotated)

Committer:
intgsull
Date:
Wed Nov 18 07:55:13 2015 +0000
Revision:
8:a254346f20aa
Parent:
7:f1bceb2bab70
Child:
9:e2feaadf504c
yay it's fixed

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