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@19:c44dc6716201, 2017-05-10 (annotated)

Committer:
chris1996
Date:
Wed May 10 19:14:25 2017 +0000
Revision:
19:c44dc6716201
Parent:
18:f2a516ad24eb
Child:
20:4173e6b7adde
Updated. Quality of LIfe. More Motors Capabailties

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 18:f2a516ad24eb 19 DigitalIn 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 18:f2a516ad24eb 247 float Gp = 0.6f;
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 18:f2a516ad24eb 262 DriveStraight();
blu12758 18:f2a516ad24eb 263 return 0;
blu12758 18:f2a516ad24eb 264 }else{
blu12758 18:f2a516ad24eb 265 speedLeft = Gp-p;
blu12758 18:f2a516ad24eb 266 speedRight = p*Gp;
blu12758 18:f2a516ad24eb 267 }
chris1996 19:c44dc6716201 268 LeftMotor.speed(speedRight);
chris1996 19:c44dc6716201 269 RightMotor.speed(speedLeft*0.5f);
blu12758 18:f2a516ad24eb 270 return (int)(p*100);
chris1996 15:e7fc3271bf53 271 //OA
chris1996 15:e7fc3271bf53 272 case 3:
blu12758 18:f2a516ad24eb 273 sonar_t.reset();
blu12758 18:f2a516ad24eb 274 //ping sensor
blu12758 18:f2a516ad24eb 275 trig.write(1);
blu12758 18:f2a516ad24eb 276 wait_us(10);
blu12758 18:f2a516ad24eb 277 trig.write(0);
blu12758 18:f2a516ad24eb 278 while(echo){}
blu12758 18:f2a516ad24eb 279 sonar_t.start();
blu12758 18:f2a516ad24eb 280 while(!echo){}
blu12758 18:f2a516ad24eb 281 sonar_t.stop();
blu12758 18:f2a516ad24eb 282 float time_us = sonar_t.read_us();
blu12758 18:f2a516ad24eb 283 return (int)(time_us * 10);
chris1996 15:e7fc3271bf53 284 //LA
chris1996 15:e7fc3271bf53 285 case 4:
blu12758 18:f2a516ad24eb 286 //Scan sensors
chris1996 15:e7fc3271bf53 287 scan(10);
chris1996 15:e7fc3271bf53 288 _cds[0] = scan(10);
chris1996 15:e7fc3271bf53 289 scan(8);
chris1996 15:e7fc3271bf53 290 _cds[1] = scan(8);
blu12758 18:f2a516ad24eb 291
blu12758 18:f2a516ad24eb 292 //Convert readings
blu12758 18:f2a516ad24eb 293 speedLeft = (float)(_cds[0]-200)/200;
blu12758 18:f2a516ad24eb 294 speedRight = (float)(_cds[1]-200)/200;
blu12758 18:f2a516ad24eb 295
chris1996 19:c44dc6716201 296 //Check light levels
blu12758 18:f2a516ad24eb 297 if ((speedLeft < 0) && (speedRight < 0)) {
chris1996 19:c44dc6716201 298 LeftMotor.speed(speedLeft);
chris1996 19:c44dc6716201 299 RightMotor.speed(speedRight);
blu12758 18:f2a516ad24eb 300 }else if(speedRight < 0){
chris1996 19:c44dc6716201 301 LeftMotor.stop(1);
chris1996 19:c44dc6716201 302 RightMotor.speed(speedRight);
blu12758 18:f2a516ad24eb 303 }else if (speedLeft < 0) {
chris1996 19:c44dc6716201 304 LeftMotor.speed(speedLeft);
chris1996 19:c44dc6716201 305 RightMotor.stop(1);
blu12758 18:f2a516ad24eb 306 }else{
chris1996 19:c44dc6716201 307 StopMotors();
chris1996 17:df86b0e2bb40 308 }
blu12758 18:f2a516ad24eb 309 return 4;
chris1996 15:e7fc3271bf53 310 //TRC
chris1996 15:e7fc3271bf53 311 case 5:
chris1996 17:df86b0e2bb40 312 DriveStraight();
chris1996 15:e7fc3271bf53 313 break;
chris1996 15:e7fc3271bf53 314 //WiiC
chris1996 15:e7fc3271bf53 315 case 6:
chris1996 15:e7fc3271bf53 316 DriveSquare();
chris1996 15:e7fc3271bf53 317 break;
blu12758 11:10a7bb4bc714 318 }
chris1996 15:e7fc3271bf53 319 return (int)(ain.read()*100);
blu12758 7:0f8c3dfbbb86 320 }