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

Committer:
blu12758
Date:
Wed May 10 14:10:43 2017 +0000
Revision:
18:f2a516ad24eb
Parent:
17:df86b0e2bb40
Child:
19:c44dc6716201
Line Following Tweaks

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
blu12758 18:f2a516ad24eb 27 Motor A(D6, D5, D4, 1); // pwm, fwd, rev, can break
blu12758 18:f2a516ad24eb 28 Motor B(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 15:e7fc3271bf53 139 A.stop(1);
chris1996 15:e7fc3271bf53 140 B.stop(1);
chris1996 17:df86b0e2bb40 141 }
chris1996 15:e7fc3271bf53 142
blu12758 18:f2a516ad24eb 143 void RobotModel::TurnRight()
chris1996 17:df86b0e2bb40 144 {
chris1996 15:e7fc3271bf53 145 A.speed(1);
chris1996 15:e7fc3271bf53 146 B.stop(1);
chris1996 15:e7fc3271bf53 147 wait(.3);
chris1996 17:df86b0e2bb40 148 }
blu12758 18:f2a516ad24eb 149 void RobotModel::TurnLeft()
chris1996 17:df86b0e2bb40 150 {
chris1996 15:e7fc3271bf53 151 B.speed(1);
chris1996 15:e7fc3271bf53 152 A.stop(1);
chris1996 15:e7fc3271bf53 153 wait(.3);
chris1996 17:df86b0e2bb40 154 }
blu12758 18:f2a516ad24eb 155 void RobotModel::DriveStraight()
chris1996 17:df86b0e2bb40 156 {
blu12758 18:f2a516ad24eb 157 A.speed(.5); //Right
blu12758 18:f2a516ad24eb 158 B.speed(.5); //Left
chris1996 17:df86b0e2bb40 159 }
blu12758 18:f2a516ad24eb 160 void RobotModel::DriveSquare()
chris1996 17:df86b0e2bb40 161 {
chris1996 15:e7fc3271bf53 162 DriveStraight();
chris1996 15:e7fc3271bf53 163 wait(1);
chris1996 15:e7fc3271bf53 164 TurnLeft();
chris1996 15:e7fc3271bf53 165 DriveStraight();
chris1996 15:e7fc3271bf53 166 wait(1);
chris1996 15:e7fc3271bf53 167 TurnLeft();
chris1996 15:e7fc3271bf53 168 DriveStraight();
chris1996 15:e7fc3271bf53 169 wait(1);
chris1996 15:e7fc3271bf53 170 TurnLeft();
chris1996 15:e7fc3271bf53 171 DriveStraight();
chris1996 15:e7fc3271bf53 172 wait(1);
chris1996 15:e7fc3271bf53 173 TurnLeft();
chris1996 17:df86b0e2bb40 174 }
chris1996 15:e7fc3271bf53 175
blu12758 18:f2a516ad24eb 176 float RobotModel::LeftSpeed(){
blu12758 18:f2a516ad24eb 177 return speedLeft;
blu12758 18:f2a516ad24eb 178 }
blu12758 18:f2a516ad24eb 179
blu12758 18:f2a516ad24eb 180 float RobotModel::RightSpeed(){
blu12758 18:f2a516ad24eb 181 return speedRight;
blu12758 18:f2a516ad24eb 182 }
blu12758 18:f2a516ad24eb 183
blu12758 10:4dd8b18e07d0 184 //update the model based on the mode
blu12758 10:4dd8b18e07d0 185 int RobotModel::update()
blu12758 10:4dd8b18e07d0 186 {
blu12758 10:4dd8b18e07d0 187 if(_mode == 0)
blu12758 10:4dd8b18e07d0 188 led_green = 0;
blu12758 10:4dd8b18e07d0 189 else
blu12758 10:4dd8b18e07d0 190 led_green = !led_green;
chris1996 17:df86b0e2bb40 191
chris1996 15:e7fc3271bf53 192 switch(_mode) {
chris1996 15:e7fc3271bf53 193 default:
chris1996 15:e7fc3271bf53 194 //Reset
chris1996 15:e7fc3271bf53 195 case 0:
chris1996 15:e7fc3271bf53 196 StopMotors();
chris1996 15:e7fc3271bf53 197 break;
chris1996 15:e7fc3271bf53 198 //LF
chris1996 15:e7fc3271bf53 199 case 2:
blu12758 18:f2a516ad24eb 200 //Read line sensors
blu12758 18:f2a516ad24eb 201 for(int i=0; i<8; i++)
blu12758 18:f2a516ad24eb 202 {
chris1996 15:e7fc3271bf53 203 scan(i);
chris1996 15:e7fc3271bf53 204 _larray[i] = scan(i);
blu12758 18:f2a516ad24eb 205 }
blu12758 18:f2a516ad24eb 206
blu12758 18:f2a516ad24eb 207 //Calculate Line Location
blu12758 18:f2a516ad24eb 208 float p = 0;
blu12758 18:f2a516ad24eb 209 float Gp = 0.6f;
blu12758 18:f2a516ad24eb 210 int width = 0;
blu12758 18:f2a516ad24eb 211 for(int i=0; i<8; i++)
blu12758 18:f2a516ad24eb 212 {
blu12758 18:f2a516ad24eb 213 int temp = checkLine(i);
blu12758 18:f2a516ad24eb 214 p += temp*i;
blu12758 18:f2a516ad24eb 215 width += temp>0 ? 1 : 0;
chris1996 15:e7fc3271bf53 216 }
blu12758 18:f2a516ad24eb 217 p = Gp*(p/width)/7;
blu12758 18:f2a516ad24eb 218 float speedLeft = 0;
blu12758 18:f2a516ad24eb 219 float speedRight = 0;
blu12758 18:f2a516ad24eb 220 if((width == 8) || (width == 7)){
blu12758 18:f2a516ad24eb 221 StopMotors();
blu12758 18:f2a516ad24eb 222 return -1;
blu12758 18:f2a516ad24eb 223 }else if(width == 0){
blu12758 18:f2a516ad24eb 224 DriveStraight();
blu12758 18:f2a516ad24eb 225 return 0;
blu12758 18:f2a516ad24eb 226 }else{
blu12758 18:f2a516ad24eb 227 speedLeft = Gp-p;
blu12758 18:f2a516ad24eb 228 speedRight = p*Gp;
blu12758 18:f2a516ad24eb 229 }
blu12758 18:f2a516ad24eb 230 B.speed(speedRight);
blu12758 18:f2a516ad24eb 231 A.speed(speedLeft*0.5f);
blu12758 18:f2a516ad24eb 232 return (int)(p*100);
chris1996 15:e7fc3271bf53 233 //OA
chris1996 15:e7fc3271bf53 234 case 3:
blu12758 18:f2a516ad24eb 235 sonar_t.reset();
blu12758 18:f2a516ad24eb 236 //ping sensor
blu12758 18:f2a516ad24eb 237 trig.write(1);
blu12758 18:f2a516ad24eb 238 wait_us(10);
blu12758 18:f2a516ad24eb 239 trig.write(0);
blu12758 18:f2a516ad24eb 240 while(echo){}
blu12758 18:f2a516ad24eb 241 sonar_t.start();
blu12758 18:f2a516ad24eb 242 while(!echo){}
blu12758 18:f2a516ad24eb 243 sonar_t.stop();
blu12758 18:f2a516ad24eb 244 float time_us = sonar_t.read_us();
blu12758 18:f2a516ad24eb 245 return (int)(time_us * 10);
chris1996 15:e7fc3271bf53 246 //LA
chris1996 15:e7fc3271bf53 247 case 4:
blu12758 18:f2a516ad24eb 248 //Scan sensors
chris1996 15:e7fc3271bf53 249 scan(10);
chris1996 15:e7fc3271bf53 250 _cds[0] = scan(10);
chris1996 15:e7fc3271bf53 251 scan(8);
chris1996 15:e7fc3271bf53 252 _cds[1] = scan(8);
blu12758 18:f2a516ad24eb 253
blu12758 18:f2a516ad24eb 254 //Convert readings
blu12758 18:f2a516ad24eb 255 speedLeft = (float)(_cds[0]-200)/200;
blu12758 18:f2a516ad24eb 256 speedRight = (float)(_cds[1]-200)/200;
blu12758 18:f2a516ad24eb 257
blu12758 18:f2a516ad24eb 258 //Check light levels
blu12758 18:f2a516ad24eb 259 if ((speedLeft < 0) && (speedRight < 0)) {
chris1996 17:df86b0e2bb40 260 B.speed(speedLeft);
blu12758 18:f2a516ad24eb 261 A.speed(speedRight);
blu12758 18:f2a516ad24eb 262 }else if(speedRight < 0){
blu12758 18:f2a516ad24eb 263 B.stop(1);
blu12758 18:f2a516ad24eb 264 A.speed(speedRight);
blu12758 18:f2a516ad24eb 265 }else if (speedLeft < 0) {
blu12758 18:f2a516ad24eb 266 B.speed(speedLeft);
blu12758 18:f2a516ad24eb 267 A.stop(1);
blu12758 18:f2a516ad24eb 268 }else{
blu12758 18:f2a516ad24eb 269 B.stop(1);
blu12758 18:f2a516ad24eb 270 A.stop(1);
chris1996 17:df86b0e2bb40 271 }
blu12758 18:f2a516ad24eb 272 return 4;
chris1996 15:e7fc3271bf53 273 //TRC
chris1996 15:e7fc3271bf53 274 case 5:
chris1996 17:df86b0e2bb40 275 DriveStraight();
chris1996 15:e7fc3271bf53 276 break;
chris1996 15:e7fc3271bf53 277 //WiiC
chris1996 15:e7fc3271bf53 278 case 6:
chris1996 15:e7fc3271bf53 279 DriveSquare();
chris1996 15:e7fc3271bf53 280 break;
blu12758 11:10a7bb4bc714 281 }
chris1996 15:e7fc3271bf53 282 return (int)(ain.read()*100);
blu12758 7:0f8c3dfbbb86 283 }