Terrabots / Mbed 2 deprecated DUMP_TRUCK_SPR2017

Main repository for dump truck API development in Spring 2017

Dependencies:   Tracker mbed MotorV2 SRF05 nRF24L01P

Fork of DUMP_TRUCK_TEST_V1 by Terrabots

Activities

This Week

If needed, please contact Milo Pan at mpan9@gatech.edu for rights and access to this repository.

DumpTruck.cpp@11:87f30625b213, 2017-03-14 (annotated)

Committer:
simplyellow
Date:
Tue Mar 14 20:10:27 2017 +0000
Revision:
11:87f30625b213
Parent:
10:cf77da9be0b8
Child:
12:502cf4fc6d98
dump truck stops as expected when within 20cm of an object (from front).

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Josahty 0:6942f0e2198c 1 #include "DumpTruck.h"
Josahty 0:6942f0e2198c 2
Josahty 0:6942f0e2198c 3 DumpTruck::DumpTruck(int truckId) : truckNumber(truckId) {
Josahty 0:6942f0e2198c 4 truckNumber = truckId;
simplyellow 11:87f30625b213 5 //track = new Tracker(p29, p30, p15);
simplyellow 10:cf77da9be0b8 6 // PWM: 21, 22, 23
simplyellow 10:cf77da9be0b8 7 // DIR: 27, 28, 29
simplyellow 11:87f30625b213 8 frontMotor = new Motor(p23, p28);
simplyellow 11:87f30625b213 9 //turnMotor = new Motor(p22, p29);
simplyellow 11:87f30625b213 10 //bedMotor = new Motor(p23, p28);
simplyellow 10:cf77da9be0b8 11 //bed = new IMU(@@@@@@@);
simplyellow 10:cf77da9be0b8 12 //nrf = new Transceiver(@@@@@@);
simplyellow 11:87f30625b213 13 srf = new SRF05(p15, p16); //trigger, echo.
simplyellow 10:cf77da9be0b8 14 tooClose = false;
simplyellow 10:cf77da9be0b8 15 }
simplyellow 10:cf77da9be0b8 16
simplyellow 10:cf77da9be0b8 17 void DumpTruck::driveDistance(float speed, float distance) {
simplyellow 10:cf77da9be0b8 18 frontMotor->write(speed);
simplyellow 10:cf77da9be0b8 19 }
simplyellow 10:cf77da9be0b8 20
simplyellow 10:cf77da9be0b8 21 void DumpTruck::drive(float speed) {
simplyellow 10:cf77da9be0b8 22 frontMotor->write(speed);
Josahty 0:6942f0e2198c 23 }
Josahty 0:6942f0e2198c 24
simplyellow 10:cf77da9be0b8 25 void DumpTruck::turn(float angle) {
simplyellow 10:cf77da9be0b8 26 return;
simplyellow 10:cf77da9be0b8 27 }
simplyellow 10:cf77da9be0b8 28
simplyellow 10:cf77da9be0b8 29 void DumpTruck::moveBed(bool raise, float angle) {
simplyellow 10:cf77da9be0b8 30 return;
simplyellow 10:cf77da9be0b8 31 }
simplyellow 10:cf77da9be0b8 32
simplyellow 10:cf77da9be0b8 33 void DumpTruck::stop() {
simplyellow 11:87f30625b213 34 frontMotor->write(0);
simplyellow 10:cf77da9be0b8 35 }
simplyellow 10:cf77da9be0b8 36
simplyellow 10:cf77da9be0b8 37 bool DumpTruck::detect() {
simplyellow 10:cf77da9be0b8 38 proximity = srf->read();
simplyellow 11:87f30625b213 39 if(proximity < 20) { //cm
simplyellow 10:cf77da9be0b8 40 tooClose = true;
simplyellow 10:cf77da9be0b8 41 } else {
simplyellow 10:cf77da9be0b8 42 tooClose = false;
simplyellow 10:cf77da9be0b8 43 }
simplyellow 10:cf77da9be0b8 44 return tooClose;
simplyellow 10:cf77da9be0b8 45 }
Josahty 0:6942f0e2198c 46
Josahty 0:6942f0e2198c 47
Josahty 3:cd6e2d7c7c9c 48 //void Calibrate(){}
Josahty 3:cd6e2d7c7c9c 49
Josahty 3:cd6e2d7c7c9c 50 //void DumpTruck::drive() {
Josahty 3:cd6e2d7c7c9c 51 // // get desired distance and speed
Josahty 3:cd6e2d7c7c9c 52 // distance = 0; // placeholder
Josahty 3:cd6e2d7c7c9c 53 // // speed = 0.5;
Josahty 3:cd6e2d7c7c9c 54 // // distance = setTruckDistance();
Josahty 3:cd6e2d7c7c9c 55 // speed = setTruckSpeed();
Josahty 3:cd6e2d7c7c9c 56 // // terminal confirmation of distance and speed
Josahty 3:cd6e2d7c7c9c 57 // pc.printf("Truck will drive a distance of %f at a speed of %f\r\n",distance,speed);
Josahty 3:cd6e2d7c7c9c 58 // // insert while loop here which relates to distance travelled
Josahty 3:cd6e2d7c7c9c 59 // while(1) {
Josahty 3:cd6e2d7c7c9c 60 // // if the speed is greater than zero, go forward at specified speed
Josahty 3:cd6e2d7c7c9c 61 // if (speed >= 0) {
Josahty 3:cd6e2d7c7c9c 62 // DirPin = 1;
Josahty 3:cd6e2d7c7c9c 63 // DrivePin = speed;
simplyellow 10:cf77da9be0b8 64 // } // else, go backwards at specified speed
Josahty 3:cd6e2d7c7c9c 65 // else {
Josahty 3:cd6e2d7c7c9c 66 // DirPin = 0;
Josahty 3:cd6e2d7c7c9c 67 // DrivePin = -1*speed;
Josahty 3:cd6e2d7c7c9c 68 // }
Josahty 3:cd6e2d7c7c9c 69 // }
Josahty 3:cd6e2d7c7c9c 70 // // stop motors
Josahty 3:cd6e2d7c7c9c 71 // // stop();
Josahty 3:cd6e2d7c7c9c 72 // // calibrate motors
Josahty 3:cd6e2d7c7c9c 73 // Calibrate();
Josahty 3:cd6e2d7c7c9c 74 // //
Josahty 3:cd6e2d7c7c9c 75 // travelled = 0;
Josahty 3:cd6e2d7c7c9c 76 //}
Josahty 3:cd6e2d7c7c9c 77
simplyellow 10:cf77da9be0b8 78 /*void DumpTruck::drive() {
Josahty 0:6942f0e2198c 79 // get desired distance and speed
Josahty 3:cd6e2d7c7c9c 80 // placeholder
Josahty 0:6942f0e2198c 81 // speed = 0.5;
Josahty 3:cd6e2d7c7c9c 82 //distance = 5.0;
Josahty 3:cd6e2d7c7c9c 83 distance = setTruckDistance();
Josahty 0:6942f0e2198c 84 speed = setTruckSpeed();
Josahty 0:6942f0e2198c 85 // terminal confirmation of distance and speed
Josahty 3:cd6e2d7c7c9c 86 pc.printf("Truck will drive a distance of %f inches at a speed of %f\r\n",distance,speed);
Josahty 0:6942f0e2198c 87 // insert while loop here which relates to distance travelled
Josahty 3:cd6e2d7c7c9c 88 travelled = getDistance();
Josahty 3:cd6e2d7c7c9c 89 //pc.printf("Check 1: %f, %f\r\n", distance, travelled);
simplyellow 10:cf77da9be0b8 90 while(travelled < distance) {
Josahty 3:cd6e2d7c7c9c 91 //pc.printf("Check 2:\r\n");
Josahty 3:cd6e2d7c7c9c 92 travelled = getDistance();
Josahty 3:cd6e2d7c7c9c 93 pc.printf("Travelling...: %f\n\r", (distance - travelled));
Josahty 0:6942f0e2198c 94 // if the speed is greater than zero, go forward at specified speed
Josahty 0:6942f0e2198c 95 if (speed >= 0) {
Josahty 0:6942f0e2198c 96 DirPin = 1;
Josahty 0:6942f0e2198c 97 DrivePin = speed;
simplyellow 10:cf77da9be0b8 98 } // else, go backwards at specified speed
Josahty 0:6942f0e2198c 99 else {
Josahty 0:6942f0e2198c 100 DirPin = 0;
Josahty 2:7811df5a6052 101 DrivePin = -1*speed;
Josahty 3:cd6e2d7c7c9c 102 }
Josahty 0:6942f0e2198c 103 }
Josahty 3:cd6e2d7c7c9c 104 //pc.printf("Check 3:\r\n");
Josahty 0:6942f0e2198c 105 // stop motors
Josahty 0:6942f0e2198c 106 // stop();
Josahty 0:6942f0e2198c 107 // calibrate motors
Josahty 3:cd6e2d7c7c9c 108 //Calibrate();
Josahty 0:6942f0e2198c 109 }
Josahty 0:6942f0e2198c 110
Josahty 3:cd6e2d7c7c9c 111
Josahty 3:cd6e2d7c7c9c 112
Josahty 3:cd6e2d7c7c9c 113
Josahty 3:cd6e2d7c7c9c 114 float DumpTruck::setTruckDistance(){
Josahty 3:cd6e2d7c7c9c 115 // terminal prompt for distance
Josahty 3:cd6e2d7c7c9c 116 pc.printf("Please enter the truck distance:\r\n");
Josahty 3:cd6e2d7c7c9c 117 // sets return value to given value
Josahty 4:2f1a0f628875 118 //float input = pc.getc();
Josahty 4:2f1a0f628875 119 //input = input - 48;
Josahty 3:cd6e2d7c7c9c 120 // returns value
Josahty 4:2f1a0f628875 121 //return input;
simplyellow 10:cf77da9be0b8 122
Josahty 4:2f1a0f628875 123 char c;
simplyellow 10:cf77da9be0b8 124 char buffer[128];
Josahty 4:2f1a0f628875 125 pc.gets(buffer, 4);
Josahty 4:2f1a0f628875 126 float input = strtod(buffer,NULL);
Josahty 3:cd6e2d7c7c9c 127 return input;
simplyellow 10:cf77da9be0b8 128
Josahty 3:cd6e2d7c7c9c 129 }
Josahty 3:cd6e2d7c7c9c 130
Josahty 0:6942f0e2198c 131 float DumpTruck::setTruckSpeed(){
Josahty 4:2f1a0f628875 132 // float mod = 0;
Josahty 4:2f1a0f628875 133 // float div = 10;
Josahty 4:2f1a0f628875 134 // pc.printf("Enter a direction - 1 = forward, 0 = backward:\r\n");
Josahty 4:2f1a0f628875 135 // float dir = pc.getc();
Josahty 4:2f1a0f628875 136 // while ((dir<48)||(dir>49)){
Josahty 4:2f1a0f628875 137 // pc.printf("Invalid input. Please enter a valid direction, 1 or 0: \r\n");
Josahty 4:2f1a0f628875 138 // dir = pc.getc();
Josahty 4:2f1a0f628875 139 // }
Josahty 4:2f1a0f628875 140 // if (dir == 48) {
Josahty 4:2f1a0f628875 141 // mod = -1; }
Josahty 4:2f1a0f628875 142 // else {
simplyellow 10:cf77da9be0b8 143 // mod = 1; }
simplyellow 10:cf77da9be0b8 144 //
Josahty 4:2f1a0f628875 145 // // terminal prompt for speed
Josahty 4:2f1a0f628875 146 // pc.printf("Please enter a truck speed from 0-9 or F for full speed:\r\n");
Josahty 4:2f1a0f628875 147 // // sets return value to given value
Josahty 4:2f1a0f628875 148 // float input = pc.getc();
Josahty 4:2f1a0f628875 149 // while ((input<48)||(input>57)){
Josahty 4:2f1a0f628875 150 // if((input == 70) || (input == 102)) {
Josahty 4:2f1a0f628875 151 // input = 1;
Josahty 4:2f1a0f628875 152 // input = input * mod;
Josahty 4:2f1a0f628875 153 // return input;
Josahty 4:2f1a0f628875 154 // }
Josahty 4:2f1a0f628875 155 // pc.printf("Invalid input. Please enter a valid speed from 0-9 or F: \r\n");
Josahty 4:2f1a0f628875 156 // input = pc.getc();
Josahty 4:2f1a0f628875 157 // }
Josahty 4:2f1a0f628875 158 // input = input - 48;
Josahty 4:2f1a0f628875 159 // input = input * (1/div);
Josahty 4:2f1a0f628875 160 // // returns value
Josahty 4:2f1a0f628875 161 // input = input * mod;
simplyellow 10:cf77da9be0b8 162
simplyellow 10:cf77da9be0b8 163
simplyellow 10:cf77da9be0b8 164 pc.printf("Enter a speed from -10 to +10:\r\n");
Josahty 4:2f1a0f628875 165 char c;
simplyellow 10:cf77da9be0b8 166 char buffer[128];
Josahty 4:2f1a0f628875 167 pc.gets(buffer, 4);
Josahty 4:2f1a0f628875 168 float input = strtod(buffer,NULL);
simplyellow 10:cf77da9be0b8 169
Josahty 4:2f1a0f628875 170 while ((buffer[0] < 43) || (buffer[0] > 45)) {
Josahty 4:2f1a0f628875 171 pc.printf("Invalid input. Please correct input and try again:\r\n");
Josahty 4:2f1a0f628875 172 setTruckSpeed();
Josahty 4:2f1a0f628875 173 }
simplyellow 10:cf77da9be0b8 174
Josahty 4:2f1a0f628875 175 pc.printf("buffer[0]: %d\r\n",buffer[0]);
Josahty 4:2f1a0f628875 176 pc.printf("buffer[1]: %d\r\n",buffer[1]);
Josahty 4:2f1a0f628875 177 pc.printf("buffer[2]: %d\r\n",buffer[2]);
Josahty 4:2f1a0f628875 178 input = input / 10;
Josahty 4:2f1a0f628875 179
Josahty 4:2f1a0f628875 180 return input;}
simplyellow 10:cf77da9be0b8 181
simplyellow 10:cf77da9be0b8 182
Josahty 0:6942f0e2198c 183 void DumpTruck::stop(){
Josahty 2:7811df5a6052 184 pc.printf("Truck will now stop.\r\n");
Josahty 0:6942f0e2198c 185 //stop motors
Josahty 0:6942f0e2198c 186 DrivePin = 0;
Josahty 0:6942f0e2198c 187 }
simplyellow 10:cf77da9be0b8 188
Josahty 3:cd6e2d7c7c9c 189 float DumpTruck::getDistance() {
Josahty 3:cd6e2d7c7c9c 190 //while(1){
Josahty 3:cd6e2d7c7c9c 191 pulseCount = (float) wheel.getPulses();
Josahty 3:cd6e2d7c7c9c 192 travelled = pulseCount*d/encoding/20/68;
Josahty 3:cd6e2d7c7c9c 193 wait(0.1);
Josahty 3:cd6e2d7c7c9c 194 //pc.printf("Inches travelled: %f\n\r", travelled);
Josahty 3:cd6e2d7c7c9c 195 return travelled;
Josahty 3:cd6e2d7c7c9c 196 //(pulse count / X * N) * (1 / PPI)
simplyellow 10:cf77da9be0b8 197
Josahty 3:cd6e2d7c7c9c 198 //}
simplyellow 10:cf77da9be0b8 199 }*/
Josahty 0:6942f0e2198c 200
simplyellow 10:cf77da9be0b8 201 //void DumpTruck::RotateTo() {
Josahty 0:6942f0e2198c 202 // pc.printf("Desired Angle: ");
Josahty 0:6942f0e2198c 203 // DesiredPivotAngle = pc.getc();
Josahty 0:6942f0e2198c 204 // float CurrentPivotAngle = getPivotAngle();
Josahty 0:6942f0e2198c 205 // Difference = DesiredPivotAngle-CurrentPivotAngle;
Josahty 0:6942f0e2198c 206 // if (Difference > limit)
Josahty 0:6942f0e2198c 207 // {
Josahty 0:6942f0e2198c 208 // PivotSpeed = setPivotSpeed();
Josahty 0:6942f0e2198c 209 // pc.printf("Rotating to %s /n", DesiredPivotAngle);
Josahty 0:6942f0e2198c 210 // //Drive Motors
Josahty 0:6942f0e2198c 211 // }
Josahty 0:6942f0e2198c 212 // else
Josahty 0:6942f0e2198c 213 // {stop()}
Josahty 0:6942f0e2198c 214 //
simplyellow 10:cf77da9be0b8 215 //}
simplyellow 10:cf77da9be0b8 216 //
simplyellow 10:cf77da9be0b8 217 //void DumpTruck::BedDown() {
Josahty 0:6942f0e2198c 218 // SwitchState = bool GetBedUpperSwitch();
Josahty 0:6942f0e2198c 219 // if SwitchState
Josahty 0:6942f0e2198c 220 // {return 0;}
Josahty 0:6942f0e2198c 221 // else
Josahty 0:6942f0e2198c 222 //
Josahty 0:6942f0e2198c 223 // while(SwitchState == 0)
Josahty 0:6942f0e2198c 224 // {
Josahty 0:6942f0e2198c 225 // //driveMotor
Josahty 0:6942f0e2198c 226 // }
Josahty 0:6942f0e2198c 227 // calibrate();
simplyellow 10:cf77da9be0b8 228 // }
Josahty 0:6942f0e2198c 229 //void DumpTruck::LowerBed() {
Josahty 0:6942f0e2198c 230 // SwitchState = bool GetLowerUpperSwitch();
Josahty 0:6942f0e2198c 231 // if SwitchState
Josahty 0:6942f0e2198c 232 // {return 0;}
Josahty 0:6942f0e2198c 233 // else
Josahty 0:6942f0e2198c 234 //
Josahty 0:6942f0e2198c 235 // while(SwitchState == 0)
Josahty 0:6942f0e2198c 236 // {
Josahty 0:6942f0e2198c 237 // //driveMotor
Josahty 0:6942f0e2198c 238 // }
Josahty 0:6942f0e2198c 239 // calibrate();
Josahty 0:6942f0e2198c 240 // }
Josahty 0:6942f0e2198c 241 //}
simplyellow 10:cf77da9be0b8 242 //void DumpTruck::raiseTo(int angle) {
Josahty 0:6942f0e2198c 243 // pc.printf("Desired Angle: ");
Josahty 0:6942f0e2198c 244 // DesiredBedAngle = pc.getc();
Josahty 0:6942f0e2198c 245 // float CurrentBedAngle = getBedAngle();
Josahty 0:6942f0e2198c 246 // Difference = DesiredBedAngle-CurrentBedAngle;
Josahty 0:6942f0e2198c 247 // if (Difference > limit)
Josahty 0:6942f0e2198c 248 // {
Josahty 0:6942f0e2198c 249 // BedSpeed = setBedSpeed();
Josahty 0:6942f0e2198c 250 // pc.printf("Rotating to %s /n", DesiredBedAngle);
Josahty 0:6942f0e2198c 251 // //Drive Motors
Josahty 0:6942f0e2198c 252 // }
Josahty 0:6942f0e2198c 253 // else
Josahty 0:6942f0e2198c 254 // {stop()}
simplyellow 10:cf77da9be0b8 255 //}
Josahty 0:6942f0e2198c 256 //
Josahty 0:6942f0e2198c 257 //float DumpTruck::getPivotAngle(){
Josahty 0:6942f0e2198c 258 // float input = AnalogIn(x);
Josahty 0:6942f0e2198c 259 // return input
Josahty 0:6942f0e2198c 260 // }
Josahty 0:6942f0e2198c 261 //float DumpTruck::setPivotSpeed(){
Josahty 0:6942f0e2198c 262 // pc.printf("Please enter the Pivot speed /n");
Josahty 0:6942f0e2198c 263 // float input = pc.getc();
Josahty 0:6942f0e2198c 264 // return input;
Josahty 0:6942f0e2198c 265 // }
Josahty 0:6942f0e2198c 266 //
Josahty 0:6942f0e2198c 267 //float DumpTruck::setBedSpeed(){
Josahty 0:6942f0e2198c 268 // pc.printf("Please enter the Bed speed /n");
Josahty 0:6942f0e2198c 269 // float input = pc.getc();
Josahty 0:6942f0e2198c 270 // return input;
Josahty 0:6942f0e2198c 271 // }
Josahty 0:6942f0e2198c 272 //float DumpTruck::getBedAngle(){
Josahty 0:6942f0e2198c 273 // float input = AnalogIn(x); // data from potentiometer
Josahty 0:6942f0e2198c 274 // return input;
Josahty 0:6942f0e2198c 275 // }
simplyellow 10:cf77da9be0b8 276 //