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

Committer:
chris1996
Date:
Wed May 10 04:09:45 2017 +0000
Revision:
17:df86b0e2bb40
Parent:
15:e7fc3271bf53
Child:
18:f2a516ad24eb
LF and LA Updated

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 11:10a7bb4bc714 17 //Serial
blu12758 11:10a7bb4bc714 18 //Line Detector
blu12758 11:10a7bb4bc714 19 SPI spi(D11,D12,D13);//SPI interface
blu12758 11:10a7bb4bc714 20 DigitalOut cs(D10);// chip select
blu12758 7:0f8c3dfbbb86 21
blu12758 7:0f8c3dfbbb86 22 //Constructors/Destructors
blu12758 7:0f8c3dfbbb86 23 RobotModel::~RobotModel()
blu12758 7:0f8c3dfbbb86 24 {
blu12758 7:0f8c3dfbbb86 25 //#TODO
blu12758 7:0f8c3dfbbb86 26 }
blu12758 7:0f8c3dfbbb86 27 RobotModel::RobotModel()
blu12758 7:0f8c3dfbbb86 28 {
blu12758 7:0f8c3dfbbb86 29 _mode = 0;
blu12758 10:4dd8b18e07d0 30 }
blu12758 10:4dd8b18e07d0 31
blu12758 10:4dd8b18e07d0 32 //initialize the robot's hardware
blu12758 10:4dd8b18e07d0 33 void RobotModel::init()
blu12758 10:4dd8b18e07d0 34 {
blu12758 10:4dd8b18e07d0 35 led_green = 0;
blu12758 10:4dd8b18e07d0 36 led_orange = 0;
blu12758 10:4dd8b18e07d0 37 led_red = 0;
blu12758 10:4dd8b18e07d0 38 led_blue = 0;
chris1996 15:e7fc3271bf53 39
chris1996 15:e7fc3271bf53 40 //Line Detector Setup
blu12758 11:10a7bb4bc714 41 // Chip must be deselected
blu12758 11:10a7bb4bc714 42 cs = 1;
blu12758 11:10a7bb4bc714 43 // Setup the spi for 8 bit data, high steady state clock,
blu12758 11:10a7bb4bc714 44 // second edge capture, with a 1MHz clock rate
blu12758 11:10a7bb4bc714 45 spi.format(8,0);
blu12758 11:10a7bb4bc714 46 spi.frequency(1000000);
blu12758 11:10a7bb4bc714 47 //set line array to 0
blu12758 11:10a7bb4bc714 48 for(int i=0; i<8; i++)
blu12758 11:10a7bb4bc714 49 _larray[i] = 0;
blu12758 11:10a7bb4bc714 50 _threshold = 120;
blu12758 11:10a7bb4bc714 51 //set cds to 0
blu12758 11:10a7bb4bc714 52 for(int i=0; i<8; i++)
blu12758 11:10a7bb4bc714 53 _cds[i] = 0;
blu12758 11:10a7bb4bc714 54 //cs = 0;
blu12758 11:10a7bb4bc714 55 // //Averaging
blu12758 11:10a7bb4bc714 56 // spi.write(0x3C);
blu12758 11:10a7bb4bc714 57 // cs = 1;
chris1996 15:e7fc3271bf53 58
blu12758 10:4dd8b18e07d0 59 wait_ms(200);
blu12758 10:4dd8b18e07d0 60 led_orange = 1;
blu12758 10:4dd8b18e07d0 61 led_red = 1;
blu12758 10:4dd8b18e07d0 62 led_blue = 1;
blu12758 10:4dd8b18e07d0 63 }
blu12758 10:4dd8b18e07d0 64
blu12758 11:10a7bb4bc714 65 //read the indicated light sensor
chris1996 15:e7fc3271bf53 66 int RobotModel::checkLight(int x)
chris1996 15:e7fc3271bf53 67 {
blu12758 11:10a7bb4bc714 68 return _cds[x];
blu12758 11:10a7bb4bc714 69 }
blu12758 11:10a7bb4bc714 70
blu12758 11:10a7bb4bc714 71 //read bit b of the line array
chris1996 15:e7fc3271bf53 72 int RobotModel::checkLine(int b)
chris1996 15:e7fc3271bf53 73 {
blu12758 11:10a7bb4bc714 74 return _larray[b];
blu12758 11:10a7bb4bc714 75 }
blu12758 11:10a7bb4bc714 76
blu12758 11:10a7bb4bc714 77 //scan the ADC on channel n
chris1996 15:e7fc3271bf53 78 int RobotModel::scan(int n)
chris1996 15:e7fc3271bf53 79 {
blu12758 11:10a7bb4bc714 80 // Select the device by seting chip select low
blu12758 11:10a7bb4bc714 81 cs = 0;
blu12758 11:10a7bb4bc714 82 spi.write(0x18);
blu12758 11:10a7bb4bc714 83 //Scan channel N
blu12758 11:10a7bb4bc714 84 //Command bits -- 1 CHS3 CHS2 CHS1 CHS0 1 1 0
blu12758 11:10a7bb4bc714 85 //CHSX is the binary for N
chris1996 15:e7fc3271bf53 86 switch(n) {
blu12758 11:10a7bb4bc714 87 default:
blu12758 11:10a7bb4bc714 88 case 0:
blu12758 11:10a7bb4bc714 89 spi.write(0x84);
blu12758 11:10a7bb4bc714 90 break;
blu12758 11:10a7bb4bc714 91 case 1:
blu12758 11:10a7bb4bc714 92 spi.write(0x8c);
blu12758 11:10a7bb4bc714 93 break;
blu12758 11:10a7bb4bc714 94 case 2:
blu12758 11:10a7bb4bc714 95 spi.write(0x94);
blu12758 11:10a7bb4bc714 96 break;
blu12758 11:10a7bb4bc714 97 case 3:
blu12758 11:10a7bb4bc714 98 spi.write(0x9C);
blu12758 11:10a7bb4bc714 99 break;
blu12758 11:10a7bb4bc714 100 case 4:
blu12758 11:10a7bb4bc714 101 spi.write(0xA4);
blu12758 11:10a7bb4bc714 102 break;
blu12758 11:10a7bb4bc714 103 case 5:
blu12758 11:10a7bb4bc714 104 spi.write(0xAC);
blu12758 11:10a7bb4bc714 105 break;
blu12758 11:10a7bb4bc714 106 case 6:
blu12758 11:10a7bb4bc714 107 spi.write(0xB4);
blu12758 11:10a7bb4bc714 108 break;
blu12758 11:10a7bb4bc714 109 case 7:
blu12758 11:10a7bb4bc714 110 spi.write(0xBC);
blu12758 11:10a7bb4bc714 111 break;
blu12758 11:10a7bb4bc714 112 case 8:
blu12758 11:10a7bb4bc714 113 spi.write(0xC4);
blu12758 11:10a7bb4bc714 114 break;
blu12758 11:10a7bb4bc714 115 case 9:
blu12758 11:10a7bb4bc714 116 spi.write(0xCC);
blu12758 11:10a7bb4bc714 117 break;
blu12758 11:10a7bb4bc714 118 case 10:
blu12758 11:10a7bb4bc714 119 spi.write(0xD4);
blu12758 11:10a7bb4bc714 120 break;
blu12758 11:10a7bb4bc714 121 }
blu12758 11:10a7bb4bc714 122 int temp = spi.write(0x00) << 4;
blu12758 11:10a7bb4bc714 123 temp |= spi.write(0x00) >> 4;
blu12758 11:10a7bb4bc714 124 spi.write(0x18);
blu12758 11:10a7bb4bc714 125 // Deselect the device
blu12758 11:10a7bb4bc714 126 cs = 1;
blu12758 11:10a7bb4bc714 127 return temp;
blu12758 11:10a7bb4bc714 128 }
blu12758 11:10a7bb4bc714 129
chris1996 15:e7fc3271bf53 130 //Motor Control
chris1996 15:e7fc3271bf53 131 #include "motordriver.h"
chris1996 15:e7fc3271bf53 132
chris1996 15:e7fc3271bf53 133
chris1996 15:e7fc3271bf53 134 Motor A(D6, D5, D4, 1); // pwm, fwd, rev, can break
chris1996 15:e7fc3271bf53 135 Motor B(D3, D7, D2, 1); // pwm, fwd, rev, can break
chris1996 15:e7fc3271bf53 136
chris1996 17:df86b0e2bb40 137 void 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
chris1996 17:df86b0e2bb40 143 void 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 }
chris1996 17:df86b0e2bb40 149 void 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 }
chris1996 17:df86b0e2bb40 155 void DriveStraight()
chris1996 17:df86b0e2bb40 156 {
chris1996 15:e7fc3271bf53 157 A.speed(1); //Right
chris1996 15:e7fc3271bf53 158 B.speed(1); //Left
chris1996 17:df86b0e2bb40 159 }
chris1996 17:df86b0e2bb40 160 void 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 10:4dd8b18e07d0 176 //update the model based on the mode
blu12758 10:4dd8b18e07d0 177 int RobotModel::update()
blu12758 10:4dd8b18e07d0 178 {
blu12758 10:4dd8b18e07d0 179 if(_mode == 0)
blu12758 10:4dd8b18e07d0 180 led_green = 0;
blu12758 10:4dd8b18e07d0 181 else
blu12758 10:4dd8b18e07d0 182 led_green = !led_green;
chris1996 17:df86b0e2bb40 183
chris1996 15:e7fc3271bf53 184 switch(_mode) {
chris1996 15:e7fc3271bf53 185 default:
chris1996 15:e7fc3271bf53 186 //Reset
chris1996 15:e7fc3271bf53 187 case 0:
chris1996 15:e7fc3271bf53 188 StopMotors();
chris1996 15:e7fc3271bf53 189 break;
chris1996 15:e7fc3271bf53 190 //LF
chris1996 15:e7fc3271bf53 191 case 2:
chris1996 15:e7fc3271bf53 192 for(int i=0; i<8; i++) {
chris1996 15:e7fc3271bf53 193 scan(i);
chris1996 15:e7fc3271bf53 194 _larray[i] = scan(i);
chris1996 17:df86b0e2bb40 195 //LF track using m and n
chris1996 17:df86b0e2bb40 196 //Forget which ones work exactly
chris1996 17:df86b0e2bb40 197 int SpeedOnLine = .75;
chris1996 17:df86b0e2bb40 198 int SpeedOffLine = .5;
chris1996 17:df86b0e2bb40 199 int m = 1;
chris1996 17:df86b0e2bb40 200 int n = 2;
chris1996 17:df86b0e2bb40 201 int OnLineLeft = _larray[m];
chris1996 17:df86b0e2bb40 202 int OnLineRight = _larray[n];
chris1996 17:df86b0e2bb40 203 if((OnLineLeft && OnLineRight) > 200){
chris1996 17:df86b0e2bb40 204 A.speed(SpeedOnLine);
chris1996 17:df86b0e2bb40 205 B.speed(SpeedOnLine);
chris1996 17:df86b0e2bb40 206 }
chris1996 17:df86b0e2bb40 207 if((OnLineLeft > 200) && (OnLineRight < 200)){
chris1996 17:df86b0e2bb40 208 A.speed(SpeedOffLine);
chris1996 17:df86b0e2bb40 209 B.speed(SpeedOnLine);
chris1996 17:df86b0e2bb40 210 }
chris1996 17:df86b0e2bb40 211 if((OnLineRight > 200) && (OnLineLeft < 200)){
chris1996 17:df86b0e2bb40 212 A.speed(SpeedOnLine);
chris1996 17:df86b0e2bb40 213 B.speed(SpeedOffLine);
chris1996 17:df86b0e2bb40 214 }
chris1996 15:e7fc3271bf53 215 }
chris1996 15:e7fc3271bf53 216 break;
chris1996 15:e7fc3271bf53 217 //OA
chris1996 15:e7fc3271bf53 218 case 3:
chris1996 15:e7fc3271bf53 219 break;
chris1996 15:e7fc3271bf53 220 //LA
chris1996 15:e7fc3271bf53 221 case 4:
chris1996 15:e7fc3271bf53 222 scan(10);
chris1996 15:e7fc3271bf53 223 _cds[0] = scan(10);
chris1996 15:e7fc3271bf53 224 scan(8);
chris1996 15:e7fc3271bf53 225 _cds[1] = scan(8);
chris1996 17:df86b0e2bb40 226 float speedLeft = (_cds[0]-200)/200;
chris1996 17:df86b0e2bb40 227 float speedRight = (_cds[1]-200)/200;
chris1996 17:df86b0e2bb40 228 if (speedLeft < 0) {
chris1996 17:df86b0e2bb40 229 B.speed(speedLeft);
chris1996 17:df86b0e2bb40 230 }
chris1996 17:df86b0e2bb40 231 if (speedRight < 0) {
chris1996 17:df86b0e2bb40 232 A.speed(speedRight);
chris1996 17:df86b0e2bb40 233 }
chris1996 17:df86b0e2bb40 234 if ((speedRight && speedLeft) >= 0) {
chris1996 17:df86b0e2bb40 235 StopMotors();
chris1996 17:df86b0e2bb40 236 }
chris1996 17:df86b0e2bb40 237 break;
chris1996 15:e7fc3271bf53 238 //TRC
chris1996 15:e7fc3271bf53 239 case 5:
chris1996 17:df86b0e2bb40 240 DriveStraight();
chris1996 15:e7fc3271bf53 241 break;
chris1996 15:e7fc3271bf53 242 //WiiC
chris1996 15:e7fc3271bf53 243 case 6:
chris1996 15:e7fc3271bf53 244 DriveSquare();
chris1996 15:e7fc3271bf53 245 break;
blu12758 11:10a7bb4bc714 246 }
chris1996 15:e7fc3271bf53 247 return (int)(ain.read()*100);
blu12758 7:0f8c3dfbbb86 248 }