Configurable Robots / Mbed 2 deprecated Configurable_Robots

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 Christopher Eubanks

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?

UserRevisionLine numberNew 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 }