Dr. Davis and Dr. Dyer special studies robotics project
Dependencies: BSP_DISCO_F469NI LCD_DISCO_F469NI TS_DISCO_F469NI mbed Motordriver
Fork of Configurable_Robots by
Classes/RobotMVC/RobotModel.cpp@20:4173e6b7adde, 2017-05-11 (annotated)
- Committer:
- blu12758
- Date:
- Thu May 11 15:40:01 2017 +0000
- Revision:
- 20:4173e6b7adde
- Parent:
- 19:c44dc6716201
- Child:
- 21:ee2b617cc0e6
Object Avoidance Working
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
blu12758 | 7:0f8c3dfbbb86 | 1 | //OU Configurable Robot Project |
blu12758 | 7:0f8c3dfbbb86 | 2 | //Spring 2017 |
blu12758 | 7:0f8c3dfbbb86 | 3 | //William Bonner |
blu12758 | 7:0f8c3dfbbb86 | 4 | |
blu12758 | 7:0f8c3dfbbb86 | 5 | #include "RobotModel.h" |
blu12758 | 7:0f8c3dfbbb86 | 6 | |
blu12758 | 10:4dd8b18e07d0 | 7 | //Pin definitions |
blu12758 | 10:4dd8b18e07d0 | 8 | //Onboard LEDs |
blu12758 | 10:4dd8b18e07d0 | 9 | DigitalOut led_green(LED1); |
blu12758 | 10:4dd8b18e07d0 | 10 | DigitalOut led_orange(LED2); |
blu12758 | 10:4dd8b18e07d0 | 11 | DigitalOut led_red(LED3); |
blu12758 | 10:4dd8b18e07d0 | 12 | DigitalOut led_blue(LED4); |
blu12758 | 10:4dd8b18e07d0 | 13 | |
blu12758 | 10:4dd8b18e07d0 | 14 | //Input Pins |
blu12758 | 10:4dd8b18e07d0 | 15 | AnalogIn ain(A0); |
blu12758 | 10:4dd8b18e07d0 | 16 | |
blu12758 | 18:f2a516ad24eb | 17 | //Sonar |
blu12758 | 18:f2a516ad24eb | 18 | DigitalOut trig(A1); |
blu12758 | 20:4173e6b7adde | 19 | AnalogIn echo(A2); |
blu12758 | 18:f2a516ad24eb | 20 | |
blu12758 | 11:10a7bb4bc714 | 21 | //Serial |
blu12758 | 11:10a7bb4bc714 | 22 | //Line Detector |
blu12758 | 11:10a7bb4bc714 | 23 | SPI spi(D11,D12,D13);//SPI interface |
blu12758 | 11:10a7bb4bc714 | 24 | DigitalOut cs(D10);// chip select |
blu12758 | 7:0f8c3dfbbb86 | 25 | |
blu12758 | 18:f2a516ad24eb | 26 | //Motors |
chris1996 | 19:c44dc6716201 | 27 | Motor RightMotor(D6, D5, D4, 1); // pwm, fwd, rev, can break |
chris1996 | 19:c44dc6716201 | 28 | Motor LeftMotor(D3, D7, D2, 1); // pwm, fwd, rev, can break |
blu12758 | 18:f2a516ad24eb | 29 | |
blu12758 | 7:0f8c3dfbbb86 | 30 | //Constructors/Destructors |
blu12758 | 7:0f8c3dfbbb86 | 31 | RobotModel::~RobotModel() |
blu12758 | 7:0f8c3dfbbb86 | 32 | { |
blu12758 | 7:0f8c3dfbbb86 | 33 | //#TODO |
blu12758 | 7:0f8c3dfbbb86 | 34 | } |
blu12758 | 7:0f8c3dfbbb86 | 35 | RobotModel::RobotModel() |
blu12758 | 7:0f8c3dfbbb86 | 36 | { |
blu12758 | 7:0f8c3dfbbb86 | 37 | _mode = 0; |
blu12758 | 10:4dd8b18e07d0 | 38 | } |
blu12758 | 10:4dd8b18e07d0 | 39 | |
blu12758 | 10:4dd8b18e07d0 | 40 | //initialize the robot's hardware |
blu12758 | 10:4dd8b18e07d0 | 41 | void RobotModel::init() |
blu12758 | 10:4dd8b18e07d0 | 42 | { |
blu12758 | 10:4dd8b18e07d0 | 43 | led_green = 0; |
blu12758 | 10:4dd8b18e07d0 | 44 | led_orange = 0; |
blu12758 | 10:4dd8b18e07d0 | 45 | led_red = 0; |
blu12758 | 10:4dd8b18e07d0 | 46 | led_blue = 0; |
chris1996 | 15:e7fc3271bf53 | 47 | |
chris1996 | 15:e7fc3271bf53 | 48 | //Line Detector Setup |
blu12758 | 11:10a7bb4bc714 | 49 | // Chip must be deselected |
blu12758 | 11:10a7bb4bc714 | 50 | cs = 1; |
blu12758 | 11:10a7bb4bc714 | 51 | // Setup the spi for 8 bit data, high steady state clock, |
blu12758 | 11:10a7bb4bc714 | 52 | // second edge capture, with a 1MHz clock rate |
blu12758 | 11:10a7bb4bc714 | 53 | spi.format(8,0); |
blu12758 | 11:10a7bb4bc714 | 54 | spi.frequency(1000000); |
blu12758 | 11:10a7bb4bc714 | 55 | //set line array to 0 |
blu12758 | 11:10a7bb4bc714 | 56 | for(int i=0; i<8; i++) |
blu12758 | 11:10a7bb4bc714 | 57 | _larray[i] = 0; |
blu12758 | 18:f2a516ad24eb | 58 | _threshold = 200; |
blu12758 | 11:10a7bb4bc714 | 59 | //set cds to 0 |
blu12758 | 11:10a7bb4bc714 | 60 | for(int i=0; i<8; i++) |
blu12758 | 11:10a7bb4bc714 | 61 | _cds[i] = 0; |
blu12758 | 18:f2a516ad24eb | 62 | trig.write(0); |
blu12758 | 18:f2a516ad24eb | 63 | speedLeft = 0; |
blu12758 | 18:f2a516ad24eb | 64 | speedRight = 0; |
blu12758 | 18:f2a516ad24eb | 65 | |
blu12758 | 18:f2a516ad24eb | 66 | wait_ms(100); |
blu12758 | 10:4dd8b18e07d0 | 67 | led_orange = 1; |
blu12758 | 10:4dd8b18e07d0 | 68 | led_red = 1; |
blu12758 | 10:4dd8b18e07d0 | 69 | led_blue = 1; |
blu12758 | 10:4dd8b18e07d0 | 70 | } |
blu12758 | 10:4dd8b18e07d0 | 71 | |
blu12758 | 11:10a7bb4bc714 | 72 | //read the indicated light sensor |
chris1996 | 15:e7fc3271bf53 | 73 | int RobotModel::checkLight(int x) |
chris1996 | 15:e7fc3271bf53 | 74 | { |
blu12758 | 11:10a7bb4bc714 | 75 | return _cds[x]; |
blu12758 | 11:10a7bb4bc714 | 76 | } |
blu12758 | 11:10a7bb4bc714 | 77 | |
blu12758 | 11:10a7bb4bc714 | 78 | //read bit b of the line array |
chris1996 | 15:e7fc3271bf53 | 79 | int RobotModel::checkLine(int b) |
chris1996 | 15:e7fc3271bf53 | 80 | { |
blu12758 | 18:f2a516ad24eb | 81 | return _larray[b]>_threshold ? 1 : 0; |
blu12758 | 11:10a7bb4bc714 | 82 | } |
blu12758 | 11:10a7bb4bc714 | 83 | |
blu12758 | 11:10a7bb4bc714 | 84 | //scan the ADC on channel n |
chris1996 | 15:e7fc3271bf53 | 85 | int RobotModel::scan(int n) |
chris1996 | 15:e7fc3271bf53 | 86 | { |
blu12758 | 11:10a7bb4bc714 | 87 | // Select the device by seting chip select low |
blu12758 | 11:10a7bb4bc714 | 88 | cs = 0; |
blu12758 | 11:10a7bb4bc714 | 89 | spi.write(0x18); |
blu12758 | 11:10a7bb4bc714 | 90 | //Scan channel N |
blu12758 | 11:10a7bb4bc714 | 91 | //Command bits -- 1 CHS3 CHS2 CHS1 CHS0 1 1 0 |
blu12758 | 11:10a7bb4bc714 | 92 | //CHSX is the binary for N |
chris1996 | 15:e7fc3271bf53 | 93 | switch(n) { |
blu12758 | 11:10a7bb4bc714 | 94 | default: |
blu12758 | 11:10a7bb4bc714 | 95 | case 0: |
blu12758 | 11:10a7bb4bc714 | 96 | spi.write(0x84); |
blu12758 | 11:10a7bb4bc714 | 97 | break; |
blu12758 | 11:10a7bb4bc714 | 98 | case 1: |
blu12758 | 11:10a7bb4bc714 | 99 | spi.write(0x8c); |
blu12758 | 11:10a7bb4bc714 | 100 | break; |
blu12758 | 11:10a7bb4bc714 | 101 | case 2: |
blu12758 | 11:10a7bb4bc714 | 102 | spi.write(0x94); |
blu12758 | 11:10a7bb4bc714 | 103 | break; |
blu12758 | 11:10a7bb4bc714 | 104 | case 3: |
blu12758 | 11:10a7bb4bc714 | 105 | spi.write(0x9C); |
blu12758 | 11:10a7bb4bc714 | 106 | break; |
blu12758 | 11:10a7bb4bc714 | 107 | case 4: |
blu12758 | 11:10a7bb4bc714 | 108 | spi.write(0xA4); |
blu12758 | 11:10a7bb4bc714 | 109 | break; |
blu12758 | 11:10a7bb4bc714 | 110 | case 5: |
blu12758 | 11:10a7bb4bc714 | 111 | spi.write(0xAC); |
blu12758 | 11:10a7bb4bc714 | 112 | break; |
blu12758 | 11:10a7bb4bc714 | 113 | case 6: |
blu12758 | 11:10a7bb4bc714 | 114 | spi.write(0xB4); |
blu12758 | 11:10a7bb4bc714 | 115 | break; |
blu12758 | 11:10a7bb4bc714 | 116 | case 7: |
blu12758 | 11:10a7bb4bc714 | 117 | spi.write(0xBC); |
blu12758 | 11:10a7bb4bc714 | 118 | break; |
blu12758 | 11:10a7bb4bc714 | 119 | case 8: |
blu12758 | 11:10a7bb4bc714 | 120 | spi.write(0xC4); |
blu12758 | 11:10a7bb4bc714 | 121 | break; |
blu12758 | 11:10a7bb4bc714 | 122 | case 9: |
blu12758 | 11:10a7bb4bc714 | 123 | spi.write(0xCC); |
blu12758 | 11:10a7bb4bc714 | 124 | break; |
blu12758 | 11:10a7bb4bc714 | 125 | case 10: |
blu12758 | 11:10a7bb4bc714 | 126 | spi.write(0xD4); |
blu12758 | 11:10a7bb4bc714 | 127 | break; |
blu12758 | 11:10a7bb4bc714 | 128 | } |
blu12758 | 11:10a7bb4bc714 | 129 | int temp = spi.write(0x00) << 4; |
blu12758 | 11:10a7bb4bc714 | 130 | temp |= spi.write(0x00) >> 4; |
blu12758 | 11:10a7bb4bc714 | 131 | spi.write(0x18); |
blu12758 | 11:10a7bb4bc714 | 132 | // Deselect the device |
blu12758 | 11:10a7bb4bc714 | 133 | cs = 1; |
blu12758 | 11:10a7bb4bc714 | 134 | return temp; |
blu12758 | 11:10a7bb4bc714 | 135 | } |
blu12758 | 11:10a7bb4bc714 | 136 | |
blu12758 | 18:f2a516ad24eb | 137 | void RobotModel::StopMotors(void) |
chris1996 | 17:df86b0e2bb40 | 138 | { |
chris1996 | 19:c44dc6716201 | 139 | RightMotor.stop(1); |
chris1996 | 19:c44dc6716201 | 140 | LeftMotor.stop(1); |
chris1996 | 17:df86b0e2bb40 | 141 | } |
chris1996 | 15:e7fc3271bf53 | 142 | |
blu12758 | 18:f2a516ad24eb | 143 | void RobotModel::TurnRight() |
chris1996 | 17:df86b0e2bb40 | 144 | { |
chris1996 | 19:c44dc6716201 | 145 | RightMotor.speed(1); |
chris1996 | 19:c44dc6716201 | 146 | LeftMotor.stop(1); |
chris1996 | 19:c44dc6716201 | 147 | wait(.3875); |
chris1996 | 17:df86b0e2bb40 | 148 | } |
blu12758 | 18:f2a516ad24eb | 149 | void RobotModel::TurnLeft() |
chris1996 | 17:df86b0e2bb40 | 150 | { |
chris1996 | 19:c44dc6716201 | 151 | LeftMotor.speed(1); |
chris1996 | 19:c44dc6716201 | 152 | RightMotor.stop(1); |
chris1996 | 19:c44dc6716201 | 153 | wait(.3875); |
chris1996 | 17:df86b0e2bb40 | 154 | } |
blu12758 | 18:f2a516ad24eb | 155 | void RobotModel::DriveStraight() |
chris1996 | 17:df86b0e2bb40 | 156 | { |
chris1996 | 19:c44dc6716201 | 157 | RightMotor.speed(.5); //Right |
chris1996 | 19:c44dc6716201 | 158 | LeftMotor.speed(.572); //Left |
chris1996 | 19:c44dc6716201 | 159 | } |
chris1996 | 19:c44dc6716201 | 160 | void RobotModel::DriveStraightMode(int ModeSetting) |
chris1996 | 19:c44dc6716201 | 161 | { |
chris1996 | 19:c44dc6716201 | 162 | switch(ModeSetting) { |
chris1996 | 19:c44dc6716201 | 163 | default: |
chris1996 | 19:c44dc6716201 | 164 | //Medium is default |
chris1996 | 19:c44dc6716201 | 165 | case 0: |
chris1996 | 19:c44dc6716201 | 166 | RightMotor.speed(.5); //Right |
chris1996 | 19:c44dc6716201 | 167 | LeftMotor.speed(.572); //Left |
chris1996 | 19:c44dc6716201 | 168 | //Slow |
chris1996 | 19:c44dc6716201 | 169 | case 1: |
chris1996 | 19:c44dc6716201 | 170 | RightMotor.speed(.25); //Right |
chris1996 | 19:c44dc6716201 | 171 | LeftMotor.speed(.2672); //Left |
chris1996 | 19:c44dc6716201 | 172 | //Fast |
chris1996 | 19:c44dc6716201 | 173 | case 2: |
chris1996 | 19:c44dc6716201 | 174 | RightMotor.speed(.75); //Right |
chris1996 | 19:c44dc6716201 | 175 | LeftMotor.speed(.8172); //Left |
chris1996 | 19:c44dc6716201 | 176 | //Max |
chris1996 | 19:c44dc6716201 | 177 | case 3: |
chris1996 | 19:c44dc6716201 | 178 | RightMotor.speed(1); //Right |
chris1996 | 19:c44dc6716201 | 179 | LeftMotor.speed(1); //Left |
chris1996 | 19:c44dc6716201 | 180 | } |
chris1996 | 19:c44dc6716201 | 181 | } |
chris1996 | 19:c44dc6716201 | 182 | |
chris1996 | 19:c44dc6716201 | 183 | void TurnDegrees(int Degrees,int TurnDir) //Degrees to turn, TurnDir = 1 for left, 0 for right |
chris1996 | 19:c44dc6716201 | 184 | { |
chris1996 | 19:c44dc6716201 | 185 | //Roughly .004 seconds turns 1 degree |
chris1996 | 19:c44dc6716201 | 186 | float DegreesFactor = .0043055556; //in seconds |
chris1996 | 19:c44dc6716201 | 187 | float TurnTime = (float)DegreesFactor*Degrees; |
chris1996 | 19:c44dc6716201 | 188 | if(TurnDir == 0) { |
chris1996 | 19:c44dc6716201 | 189 | RightMotor.stop(1); |
chris1996 | 19:c44dc6716201 | 190 | LeftMotor.speed(1); |
chris1996 | 19:c44dc6716201 | 191 | wait(TurnTime); |
chris1996 | 19:c44dc6716201 | 192 | } else { |
chris1996 | 19:c44dc6716201 | 193 | LeftMotor.stop(1); |
chris1996 | 19:c44dc6716201 | 194 | RightMotor.speed(1); |
chris1996 | 19:c44dc6716201 | 195 | wait(TurnTime); |
chris1996 | 19:c44dc6716201 | 196 | } |
chris1996 | 17:df86b0e2bb40 | 197 | } |
blu12758 | 18:f2a516ad24eb | 198 | void RobotModel::DriveSquare() |
chris1996 | 17:df86b0e2bb40 | 199 | { |
chris1996 | 15:e7fc3271bf53 | 200 | DriveStraight(); |
chris1996 | 15:e7fc3271bf53 | 201 | wait(1); |
chris1996 | 15:e7fc3271bf53 | 202 | TurnLeft(); |
chris1996 | 15:e7fc3271bf53 | 203 | DriveStraight(); |
chris1996 | 15:e7fc3271bf53 | 204 | wait(1); |
chris1996 | 15:e7fc3271bf53 | 205 | TurnLeft(); |
chris1996 | 15:e7fc3271bf53 | 206 | DriveStraight(); |
chris1996 | 15:e7fc3271bf53 | 207 | wait(1); |
chris1996 | 15:e7fc3271bf53 | 208 | TurnLeft(); |
chris1996 | 15:e7fc3271bf53 | 209 | DriveStraight(); |
chris1996 | 15:e7fc3271bf53 | 210 | wait(1); |
chris1996 | 15:e7fc3271bf53 | 211 | TurnLeft(); |
chris1996 | 17:df86b0e2bb40 | 212 | } |
chris1996 | 15:e7fc3271bf53 | 213 | |
blu12758 | 18:f2a516ad24eb | 214 | float RobotModel::LeftSpeed(){ |
blu12758 | 18:f2a516ad24eb | 215 | return speedLeft; |
blu12758 | 18:f2a516ad24eb | 216 | } |
blu12758 | 18:f2a516ad24eb | 217 | |
blu12758 | 18:f2a516ad24eb | 218 | float RobotModel::RightSpeed(){ |
blu12758 | 18:f2a516ad24eb | 219 | return speedRight; |
blu12758 | 18:f2a516ad24eb | 220 | } |
blu12758 | 18:f2a516ad24eb | 221 | |
blu12758 | 10:4dd8b18e07d0 | 222 | //update the model based on the mode |
blu12758 | 10:4dd8b18e07d0 | 223 | int RobotModel::update() |
blu12758 | 10:4dd8b18e07d0 | 224 | { |
blu12758 | 10:4dd8b18e07d0 | 225 | if(_mode == 0) |
blu12758 | 10:4dd8b18e07d0 | 226 | led_green = 0; |
blu12758 | 10:4dd8b18e07d0 | 227 | else |
blu12758 | 10:4dd8b18e07d0 | 228 | led_green = !led_green; |
chris1996 | 17:df86b0e2bb40 | 229 | |
chris1996 | 15:e7fc3271bf53 | 230 | switch(_mode) { |
chris1996 | 15:e7fc3271bf53 | 231 | default: |
chris1996 | 15:e7fc3271bf53 | 232 | //Reset |
chris1996 | 15:e7fc3271bf53 | 233 | case 0: |
chris1996 | 15:e7fc3271bf53 | 234 | StopMotors(); |
chris1996 | 15:e7fc3271bf53 | 235 | break; |
chris1996 | 15:e7fc3271bf53 | 236 | //LF |
chris1996 | 15:e7fc3271bf53 | 237 | case 2: |
blu12758 | 18:f2a516ad24eb | 238 | //Read line sensors |
blu12758 | 18:f2a516ad24eb | 239 | for(int i=0; i<8; i++) |
blu12758 | 18:f2a516ad24eb | 240 | { |
chris1996 | 15:e7fc3271bf53 | 241 | scan(i); |
chris1996 | 15:e7fc3271bf53 | 242 | _larray[i] = scan(i); |
blu12758 | 18:f2a516ad24eb | 243 | } |
blu12758 | 18:f2a516ad24eb | 244 | |
blu12758 | 18:f2a516ad24eb | 245 | //Calculate Line Location |
blu12758 | 18:f2a516ad24eb | 246 | float p = 0; |
blu12758 | 20:4173e6b7adde | 247 | float Gp = 0.5f; |
blu12758 | 18:f2a516ad24eb | 248 | int width = 0; |
blu12758 | 18:f2a516ad24eb | 249 | for(int i=0; i<8; i++) |
blu12758 | 18:f2a516ad24eb | 250 | { |
blu12758 | 18:f2a516ad24eb | 251 | int temp = checkLine(i); |
blu12758 | 18:f2a516ad24eb | 252 | p += temp*i; |
blu12758 | 18:f2a516ad24eb | 253 | width += temp>0 ? 1 : 0; |
chris1996 | 15:e7fc3271bf53 | 254 | } |
blu12758 | 18:f2a516ad24eb | 255 | p = Gp*(p/width)/7; |
blu12758 | 18:f2a516ad24eb | 256 | float speedLeft = 0; |
blu12758 | 18:f2a516ad24eb | 257 | float speedRight = 0; |
blu12758 | 18:f2a516ad24eb | 258 | if((width == 8) || (width == 7)){ |
blu12758 | 18:f2a516ad24eb | 259 | StopMotors(); |
blu12758 | 18:f2a516ad24eb | 260 | return -1; |
blu12758 | 18:f2a516ad24eb | 261 | }else if(width == 0){ |
blu12758 | 20:4173e6b7adde | 262 | LeftMotor.speed(Gp*.5172); |
blu12758 | 20:4173e6b7adde | 263 | RightMotor.speed(Gp*.5); |
blu12758 | 18:f2a516ad24eb | 264 | return 0; |
blu12758 | 18:f2a516ad24eb | 265 | }else{ |
blu12758 | 18:f2a516ad24eb | 266 | speedLeft = Gp-p; |
blu12758 | 18:f2a516ad24eb | 267 | speedRight = p*Gp; |
blu12758 | 18:f2a516ad24eb | 268 | } |
chris1996 | 19:c44dc6716201 | 269 | LeftMotor.speed(speedRight); |
chris1996 | 19:c44dc6716201 | 270 | RightMotor.speed(speedLeft*0.5f); |
blu12758 | 18:f2a516ad24eb | 271 | return (int)(p*100); |
chris1996 | 15:e7fc3271bf53 | 272 | //OA |
chris1996 | 15:e7fc3271bf53 | 273 | case 3: |
blu12758 | 20:4173e6b7adde | 274 | //sonar_t.reset(); |
blu12758 | 20:4173e6b7adde | 275 | // //ping sensor |
blu12758 | 20:4173e6b7adde | 276 | // trig.write(1); |
blu12758 | 20:4173e6b7adde | 277 | // wait_us(10); |
blu12758 | 20:4173e6b7adde | 278 | // trig.write(0); |
blu12758 | 20:4173e6b7adde | 279 | // while(echo){} |
blu12758 | 20:4173e6b7adde | 280 | // sonar_t.start(); |
blu12758 | 20:4173e6b7adde | 281 | // while(!echo){} |
blu12758 | 20:4173e6b7adde | 282 | // sonar_t.stop(); |
blu12758 | 20:4173e6b7adde | 283 | // float time_us = sonar_t.read_us(); |
blu12758 | 20:4173e6b7adde | 284 | float volts = echo.read(); |
blu12758 | 20:4173e6b7adde | 285 | if(volts >= .7){ |
blu12758 | 20:4173e6b7adde | 286 | TurnRight(); |
blu12758 | 20:4173e6b7adde | 287 | }else{ |
blu12758 | 20:4173e6b7adde | 288 | DriveStraight(); |
blu12758 | 20:4173e6b7adde | 289 | } |
blu12758 | 20:4173e6b7adde | 290 | return (int)(volts * 100); |
chris1996 | 15:e7fc3271bf53 | 291 | //LA |
chris1996 | 15:e7fc3271bf53 | 292 | case 4: |
blu12758 | 18:f2a516ad24eb | 293 | //Scan sensors |
chris1996 | 15:e7fc3271bf53 | 294 | scan(10); |
chris1996 | 15:e7fc3271bf53 | 295 | _cds[0] = scan(10); |
chris1996 | 15:e7fc3271bf53 | 296 | scan(8); |
chris1996 | 15:e7fc3271bf53 | 297 | _cds[1] = scan(8); |
blu12758 | 18:f2a516ad24eb | 298 | |
blu12758 | 18:f2a516ad24eb | 299 | //Convert readings |
blu12758 | 18:f2a516ad24eb | 300 | speedLeft = (float)(_cds[0]-200)/200; |
blu12758 | 18:f2a516ad24eb | 301 | speedRight = (float)(_cds[1]-200)/200; |
blu12758 | 18:f2a516ad24eb | 302 | |
chris1996 | 19:c44dc6716201 | 303 | //Check light levels |
blu12758 | 18:f2a516ad24eb | 304 | if ((speedLeft < 0) && (speedRight < 0)) { |
chris1996 | 19:c44dc6716201 | 305 | LeftMotor.speed(speedLeft); |
chris1996 | 19:c44dc6716201 | 306 | RightMotor.speed(speedRight); |
blu12758 | 18:f2a516ad24eb | 307 | }else if(speedRight < 0){ |
chris1996 | 19:c44dc6716201 | 308 | LeftMotor.stop(1); |
chris1996 | 19:c44dc6716201 | 309 | RightMotor.speed(speedRight); |
blu12758 | 18:f2a516ad24eb | 310 | }else if (speedLeft < 0) { |
chris1996 | 19:c44dc6716201 | 311 | LeftMotor.speed(speedLeft); |
chris1996 | 19:c44dc6716201 | 312 | RightMotor.stop(1); |
blu12758 | 18:f2a516ad24eb | 313 | }else{ |
chris1996 | 19:c44dc6716201 | 314 | StopMotors(); |
chris1996 | 17:df86b0e2bb40 | 315 | } |
blu12758 | 18:f2a516ad24eb | 316 | return 4; |
chris1996 | 15:e7fc3271bf53 | 317 | //TRC |
chris1996 | 15:e7fc3271bf53 | 318 | case 5: |
chris1996 | 17:df86b0e2bb40 | 319 | DriveStraight(); |
chris1996 | 15:e7fc3271bf53 | 320 | break; |
chris1996 | 15:e7fc3271bf53 | 321 | //WiiC |
chris1996 | 15:e7fc3271bf53 | 322 | case 6: |
chris1996 | 15:e7fc3271bf53 | 323 | DriveSquare(); |
chris1996 | 15:e7fc3271bf53 | 324 | break; |
blu12758 | 11:10a7bb4bc714 | 325 | } |
chris1996 | 15:e7fc3271bf53 | 326 | return (int)(ain.read()*100); |
blu12758 | 7:0f8c3dfbbb86 | 327 | } |