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.

Committer:
simplyellow
Date:
Thu Mar 30 17:27:21 2017 +0000
Revision:
12:502cf4fc6d98
Parent:
11:87f30625b213
Child:
13:112b6543909a
began work on transceiver

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 11:87f30625b213 12 srf = new SRF05(p15, p16); //trigger, echo.
simplyellow 10:cf77da9be0b8 13 tooClose = false;
simplyellow 12:502cf4fc6d98 14
simplyellow 12:502cf4fc6d98 15 nrf = new nRF24L01P(p5, p6, p7, p8, p9, p10); // transceiver
simplyellow 12:502cf4fc6d98 16 //transceiver set up, similar to one on base station code
simplyellow 12:502cf4fc6d98 17 txDataCnt = 0;
simplyellow 12:502cf4fc6d98 18 rxDataCnt = 0;
simplyellow 12:502cf4fc6d98 19 }
simplyellow 12:502cf4fc6d98 20
simplyellow 12:502cf4fc6d98 21 void DumpTruck::startComms() {
simplyellow 12:502cf4fc6d98 22 nrf->powerUp();
simplyellow 12:502cf4fc6d98 23 printf( "nRF24L01+ Frequency : %d MHz\r\n", nrf->getRfFrequency() );
simplyellow 12:502cf4fc6d98 24 printf( "nRF24L01+ Output power : %d dBm\r\n", nrf->getRfOutputPower() );
simplyellow 12:502cf4fc6d98 25 printf( "nRF24L01+ Data Rate : %d kbps\r\n", nrf->getAirDataRate() );
simplyellow 12:502cf4fc6d98 26 printf( "nRF24L01+ TX Address : 0x%010llX\r\n", nrf->getTxAddress() );
simplyellow 12:502cf4fc6d98 27 printf( "nRF24L01+ RX Address : 0x%010llX\r\n", nrf->getRxAddress() );
simplyellow 12:502cf4fc6d98 28 nrf->setTransferSize( TRANSFER_SIZE );
simplyellow 12:502cf4fc6d98 29 nrf->setReceiveMode();
simplyellow 12:502cf4fc6d98 30 nrf->enable();
simplyellow 12:502cf4fc6d98 31 received = false;
simplyellow 12:502cf4fc6d98 32 }
simplyellow 12:502cf4fc6d98 33
simplyellow 12:502cf4fc6d98 34 void DumpTruck::getCommand() {
simplyellow 12:502cf4fc6d98 35 received = false;
simplyellow 12:502cf4fc6d98 36 //called at the end of each executable command
simplyellow 12:502cf4fc6d98 37 //print statement saying dump truck is ready to receive
simplyellow 12:502cf4fc6d98 38 //have print statement appear in base station code
simplyellow 12:502cf4fc6d98 39 //dump truck transceiver sends a code to base station, opening the channel
simplyellow 12:502cf4fc6d98 40 //code is all 0's
simplyellow 12:502cf4fc6d98 41 for(int i = 0; i < TRANSFER_SIZE; i++) {
simplyellow 12:502cf4fc6d98 42 txData[i] = 0;
simplyellow 12:502cf4fc6d98 43 }
simplyellow 12:502cf4fc6d98 44 nrf->write(NRF24L01P_PIPE_P0, txData, TRANSFER_SIZE);
simplyellow 12:502cf4fc6d98 45 //possibly implement kill switch/stop current action
simplyellow 12:502cf4fc6d98 46 //...
simplyellow 12:502cf4fc6d98 47 //code for receiving
simplyellow 12:502cf4fc6d98 48 while(!received) {
simplyellow 12:502cf4fc6d98 49 if (nrf->readable() ) {
simplyellow 12:502cf4fc6d98 50 rxDataCnt = nrf->read(NRF24L01P_PIPE_P0, rxData, sizeof(rxData));
simplyellow 12:502cf4fc6d98 51 /*
simplyellow 12:502cf4fc6d98 52 either go char by char to determine command or switch statement
simplyellow 12:502cf4fc6d98 53 for ( int j = 0; rxDataCnt > 0; rxDataCnt--, i++ ) {
simplyellow 12:502cf4fc6d98 54
simplyellow 12:502cf4fc6d98 55 pc.putc( rxData[j] );
simplyellow 12:502cf4fc6d98 56 }*/
simplyellow 12:502cf4fc6d98 57 }
simplyellow 12:502cf4fc6d98 58 //received = true after receiving command
simplyellow 12:502cf4fc6d98 59 }
simplyellow 12:502cf4fc6d98 60 //switch statement, sorting all commands
simplyellow 12:502cf4fc6d98 61
simplyellow 12:502cf4fc6d98 62 }
simplyellow 12:502cf4fc6d98 63
simplyellow 12:502cf4fc6d98 64 void DumpTruck::reportData() {
simplyellow 12:502cf4fc6d98 65 //should run last in main thread
simplyellow 12:502cf4fc6d98 66
simplyellow 10:cf77da9be0b8 67 }
simplyellow 10:cf77da9be0b8 68
simplyellow 10:cf77da9be0b8 69 void DumpTruck::driveDistance(float speed, float distance) {
simplyellow 10:cf77da9be0b8 70 frontMotor->write(speed);
simplyellow 10:cf77da9be0b8 71 }
simplyellow 10:cf77da9be0b8 72
simplyellow 10:cf77da9be0b8 73 void DumpTruck::drive(float speed) {
simplyellow 10:cf77da9be0b8 74 frontMotor->write(speed);
Josahty 0:6942f0e2198c 75 }
Josahty 0:6942f0e2198c 76
simplyellow 10:cf77da9be0b8 77 void DumpTruck::turn(float angle) {
simplyellow 10:cf77da9be0b8 78 return;
simplyellow 10:cf77da9be0b8 79 }
simplyellow 10:cf77da9be0b8 80
simplyellow 10:cf77da9be0b8 81 void DumpTruck::moveBed(bool raise, float angle) {
simplyellow 10:cf77da9be0b8 82 return;
simplyellow 10:cf77da9be0b8 83 }
simplyellow 10:cf77da9be0b8 84
simplyellow 10:cf77da9be0b8 85 void DumpTruck::stop() {
simplyellow 11:87f30625b213 86 frontMotor->write(0);
simplyellow 10:cf77da9be0b8 87 }
simplyellow 10:cf77da9be0b8 88
simplyellow 10:cf77da9be0b8 89 bool DumpTruck::detect() {
simplyellow 10:cf77da9be0b8 90 proximity = srf->read();
simplyellow 11:87f30625b213 91 if(proximity < 20) { //cm
simplyellow 10:cf77da9be0b8 92 tooClose = true;
simplyellow 10:cf77da9be0b8 93 } else {
simplyellow 10:cf77da9be0b8 94 tooClose = false;
simplyellow 10:cf77da9be0b8 95 }
simplyellow 10:cf77da9be0b8 96 return tooClose;
simplyellow 10:cf77da9be0b8 97 }
Josahty 0:6942f0e2198c 98
Josahty 0:6942f0e2198c 99
Josahty 3:cd6e2d7c7c9c 100 //void Calibrate(){}
Josahty 3:cd6e2d7c7c9c 101
Josahty 3:cd6e2d7c7c9c 102 //void DumpTruck::drive() {
Josahty 3:cd6e2d7c7c9c 103 // // get desired distance and speed
Josahty 3:cd6e2d7c7c9c 104 // distance = 0; // placeholder
Josahty 3:cd6e2d7c7c9c 105 // // speed = 0.5;
Josahty 3:cd6e2d7c7c9c 106 // // distance = setTruckDistance();
Josahty 3:cd6e2d7c7c9c 107 // speed = setTruckSpeed();
Josahty 3:cd6e2d7c7c9c 108 // // terminal confirmation of distance and speed
Josahty 3:cd6e2d7c7c9c 109 // pc.printf("Truck will drive a distance of %f at a speed of %f\r\n",distance,speed);
Josahty 3:cd6e2d7c7c9c 110 // // insert while loop here which relates to distance travelled
Josahty 3:cd6e2d7c7c9c 111 // while(1) {
Josahty 3:cd6e2d7c7c9c 112 // // if the speed is greater than zero, go forward at specified speed
Josahty 3:cd6e2d7c7c9c 113 // if (speed >= 0) {
Josahty 3:cd6e2d7c7c9c 114 // DirPin = 1;
Josahty 3:cd6e2d7c7c9c 115 // DrivePin = speed;
simplyellow 10:cf77da9be0b8 116 // } // else, go backwards at specified speed
Josahty 3:cd6e2d7c7c9c 117 // else {
Josahty 3:cd6e2d7c7c9c 118 // DirPin = 0;
Josahty 3:cd6e2d7c7c9c 119 // DrivePin = -1*speed;
Josahty 3:cd6e2d7c7c9c 120 // }
Josahty 3:cd6e2d7c7c9c 121 // }
Josahty 3:cd6e2d7c7c9c 122 // // stop motors
Josahty 3:cd6e2d7c7c9c 123 // // stop();
Josahty 3:cd6e2d7c7c9c 124 // // calibrate motors
Josahty 3:cd6e2d7c7c9c 125 // Calibrate();
Josahty 3:cd6e2d7c7c9c 126 // //
Josahty 3:cd6e2d7c7c9c 127 // travelled = 0;
Josahty 3:cd6e2d7c7c9c 128 //}
Josahty 3:cd6e2d7c7c9c 129
simplyellow 10:cf77da9be0b8 130 /*void DumpTruck::drive() {
Josahty 0:6942f0e2198c 131 // get desired distance and speed
Josahty 3:cd6e2d7c7c9c 132 // placeholder
Josahty 0:6942f0e2198c 133 // speed = 0.5;
Josahty 3:cd6e2d7c7c9c 134 //distance = 5.0;
Josahty 3:cd6e2d7c7c9c 135 distance = setTruckDistance();
Josahty 0:6942f0e2198c 136 speed = setTruckSpeed();
Josahty 0:6942f0e2198c 137 // terminal confirmation of distance and speed
Josahty 3:cd6e2d7c7c9c 138 pc.printf("Truck will drive a distance of %f inches at a speed of %f\r\n",distance,speed);
Josahty 0:6942f0e2198c 139 // insert while loop here which relates to distance travelled
Josahty 3:cd6e2d7c7c9c 140 travelled = getDistance();
Josahty 3:cd6e2d7c7c9c 141 //pc.printf("Check 1: %f, %f\r\n", distance, travelled);
simplyellow 10:cf77da9be0b8 142 while(travelled < distance) {
Josahty 3:cd6e2d7c7c9c 143 //pc.printf("Check 2:\r\n");
Josahty 3:cd6e2d7c7c9c 144 travelled = getDistance();
Josahty 3:cd6e2d7c7c9c 145 pc.printf("Travelling...: %f\n\r", (distance - travelled));
Josahty 0:6942f0e2198c 146 // if the speed is greater than zero, go forward at specified speed
Josahty 0:6942f0e2198c 147 if (speed >= 0) {
Josahty 0:6942f0e2198c 148 DirPin = 1;
Josahty 0:6942f0e2198c 149 DrivePin = speed;
simplyellow 10:cf77da9be0b8 150 } // else, go backwards at specified speed
Josahty 0:6942f0e2198c 151 else {
Josahty 0:6942f0e2198c 152 DirPin = 0;
Josahty 2:7811df5a6052 153 DrivePin = -1*speed;
Josahty 3:cd6e2d7c7c9c 154 }
Josahty 0:6942f0e2198c 155 }
Josahty 3:cd6e2d7c7c9c 156 //pc.printf("Check 3:\r\n");
Josahty 0:6942f0e2198c 157 // stop motors
Josahty 0:6942f0e2198c 158 // stop();
Josahty 0:6942f0e2198c 159 // calibrate motors
Josahty 3:cd6e2d7c7c9c 160 //Calibrate();
Josahty 0:6942f0e2198c 161 }
Josahty 0:6942f0e2198c 162
Josahty 3:cd6e2d7c7c9c 163
Josahty 3:cd6e2d7c7c9c 164
Josahty 3:cd6e2d7c7c9c 165
Josahty 3:cd6e2d7c7c9c 166 float DumpTruck::setTruckDistance(){
Josahty 3:cd6e2d7c7c9c 167 // terminal prompt for distance
Josahty 3:cd6e2d7c7c9c 168 pc.printf("Please enter the truck distance:\r\n");
Josahty 3:cd6e2d7c7c9c 169 // sets return value to given value
Josahty 4:2f1a0f628875 170 //float input = pc.getc();
Josahty 4:2f1a0f628875 171 //input = input - 48;
Josahty 3:cd6e2d7c7c9c 172 // returns value
Josahty 4:2f1a0f628875 173 //return input;
simplyellow 10:cf77da9be0b8 174
Josahty 4:2f1a0f628875 175 char c;
simplyellow 10:cf77da9be0b8 176 char buffer[128];
Josahty 4:2f1a0f628875 177 pc.gets(buffer, 4);
Josahty 4:2f1a0f628875 178 float input = strtod(buffer,NULL);
Josahty 3:cd6e2d7c7c9c 179 return input;
simplyellow 10:cf77da9be0b8 180
Josahty 3:cd6e2d7c7c9c 181 }
Josahty 3:cd6e2d7c7c9c 182
Josahty 0:6942f0e2198c 183 float DumpTruck::setTruckSpeed(){
Josahty 4:2f1a0f628875 184 // float mod = 0;
Josahty 4:2f1a0f628875 185 // float div = 10;
Josahty 4:2f1a0f628875 186 // pc.printf("Enter a direction - 1 = forward, 0 = backward:\r\n");
Josahty 4:2f1a0f628875 187 // float dir = pc.getc();
Josahty 4:2f1a0f628875 188 // while ((dir<48)||(dir>49)){
Josahty 4:2f1a0f628875 189 // pc.printf("Invalid input. Please enter a valid direction, 1 or 0: \r\n");
Josahty 4:2f1a0f628875 190 // dir = pc.getc();
Josahty 4:2f1a0f628875 191 // }
Josahty 4:2f1a0f628875 192 // if (dir == 48) {
Josahty 4:2f1a0f628875 193 // mod = -1; }
Josahty 4:2f1a0f628875 194 // else {
simplyellow 10:cf77da9be0b8 195 // mod = 1; }
simplyellow 10:cf77da9be0b8 196 //
Josahty 4:2f1a0f628875 197 // // terminal prompt for speed
Josahty 4:2f1a0f628875 198 // pc.printf("Please enter a truck speed from 0-9 or F for full speed:\r\n");
Josahty 4:2f1a0f628875 199 // // sets return value to given value
Josahty 4:2f1a0f628875 200 // float input = pc.getc();
Josahty 4:2f1a0f628875 201 // while ((input<48)||(input>57)){
Josahty 4:2f1a0f628875 202 // if((input == 70) || (input == 102)) {
Josahty 4:2f1a0f628875 203 // input = 1;
Josahty 4:2f1a0f628875 204 // input = input * mod;
Josahty 4:2f1a0f628875 205 // return input;
Josahty 4:2f1a0f628875 206 // }
Josahty 4:2f1a0f628875 207 // pc.printf("Invalid input. Please enter a valid speed from 0-9 or F: \r\n");
Josahty 4:2f1a0f628875 208 // input = pc.getc();
Josahty 4:2f1a0f628875 209 // }
Josahty 4:2f1a0f628875 210 // input = input - 48;
Josahty 4:2f1a0f628875 211 // input = input * (1/div);
Josahty 4:2f1a0f628875 212 // // returns value
Josahty 4:2f1a0f628875 213 // input = input * mod;
simplyellow 10:cf77da9be0b8 214
simplyellow 10:cf77da9be0b8 215
simplyellow 10:cf77da9be0b8 216 pc.printf("Enter a speed from -10 to +10:\r\n");
Josahty 4:2f1a0f628875 217 char c;
simplyellow 10:cf77da9be0b8 218 char buffer[128];
Josahty 4:2f1a0f628875 219 pc.gets(buffer, 4);
Josahty 4:2f1a0f628875 220 float input = strtod(buffer,NULL);
simplyellow 10:cf77da9be0b8 221
Josahty 4:2f1a0f628875 222 while ((buffer[0] < 43) || (buffer[0] > 45)) {
Josahty 4:2f1a0f628875 223 pc.printf("Invalid input. Please correct input and try again:\r\n");
Josahty 4:2f1a0f628875 224 setTruckSpeed();
Josahty 4:2f1a0f628875 225 }
simplyellow 10:cf77da9be0b8 226
Josahty 4:2f1a0f628875 227 pc.printf("buffer[0]: %d\r\n",buffer[0]);
Josahty 4:2f1a0f628875 228 pc.printf("buffer[1]: %d\r\n",buffer[1]);
Josahty 4:2f1a0f628875 229 pc.printf("buffer[2]: %d\r\n",buffer[2]);
Josahty 4:2f1a0f628875 230 input = input / 10;
Josahty 4:2f1a0f628875 231
Josahty 4:2f1a0f628875 232 return input;}
simplyellow 10:cf77da9be0b8 233
simplyellow 10:cf77da9be0b8 234
Josahty 0:6942f0e2198c 235 void DumpTruck::stop(){
Josahty 2:7811df5a6052 236 pc.printf("Truck will now stop.\r\n");
Josahty 0:6942f0e2198c 237 //stop motors
Josahty 0:6942f0e2198c 238 DrivePin = 0;
Josahty 0:6942f0e2198c 239 }
simplyellow 10:cf77da9be0b8 240
Josahty 3:cd6e2d7c7c9c 241 float DumpTruck::getDistance() {
Josahty 3:cd6e2d7c7c9c 242 //while(1){
Josahty 3:cd6e2d7c7c9c 243 pulseCount = (float) wheel.getPulses();
Josahty 3:cd6e2d7c7c9c 244 travelled = pulseCount*d/encoding/20/68;
Josahty 3:cd6e2d7c7c9c 245 wait(0.1);
Josahty 3:cd6e2d7c7c9c 246 //pc.printf("Inches travelled: %f\n\r", travelled);
Josahty 3:cd6e2d7c7c9c 247 return travelled;
Josahty 3:cd6e2d7c7c9c 248 //(pulse count / X * N) * (1 / PPI)
simplyellow 10:cf77da9be0b8 249
Josahty 3:cd6e2d7c7c9c 250 //}
simplyellow 10:cf77da9be0b8 251 }*/
Josahty 0:6942f0e2198c 252
simplyellow 10:cf77da9be0b8 253 //void DumpTruck::RotateTo() {
Josahty 0:6942f0e2198c 254 // pc.printf("Desired Angle: ");
Josahty 0:6942f0e2198c 255 // DesiredPivotAngle = pc.getc();
Josahty 0:6942f0e2198c 256 // float CurrentPivotAngle = getPivotAngle();
Josahty 0:6942f0e2198c 257 // Difference = DesiredPivotAngle-CurrentPivotAngle;
Josahty 0:6942f0e2198c 258 // if (Difference > limit)
Josahty 0:6942f0e2198c 259 // {
Josahty 0:6942f0e2198c 260 // PivotSpeed = setPivotSpeed();
Josahty 0:6942f0e2198c 261 // pc.printf("Rotating to %s /n", DesiredPivotAngle);
Josahty 0:6942f0e2198c 262 // //Drive Motors
Josahty 0:6942f0e2198c 263 // }
Josahty 0:6942f0e2198c 264 // else
Josahty 0:6942f0e2198c 265 // {stop()}
Josahty 0:6942f0e2198c 266 //
simplyellow 10:cf77da9be0b8 267 //}
simplyellow 10:cf77da9be0b8 268 //
simplyellow 10:cf77da9be0b8 269 //void DumpTruck::BedDown() {
Josahty 0:6942f0e2198c 270 // SwitchState = bool GetBedUpperSwitch();
Josahty 0:6942f0e2198c 271 // if SwitchState
Josahty 0:6942f0e2198c 272 // {return 0;}
Josahty 0:6942f0e2198c 273 // else
Josahty 0:6942f0e2198c 274 //
Josahty 0:6942f0e2198c 275 // while(SwitchState == 0)
Josahty 0:6942f0e2198c 276 // {
Josahty 0:6942f0e2198c 277 // //driveMotor
Josahty 0:6942f0e2198c 278 // }
Josahty 0:6942f0e2198c 279 // calibrate();
simplyellow 10:cf77da9be0b8 280 // }
Josahty 0:6942f0e2198c 281 //void DumpTruck::LowerBed() {
Josahty 0:6942f0e2198c 282 // SwitchState = bool GetLowerUpperSwitch();
Josahty 0:6942f0e2198c 283 // if SwitchState
Josahty 0:6942f0e2198c 284 // {return 0;}
Josahty 0:6942f0e2198c 285 // else
Josahty 0:6942f0e2198c 286 //
Josahty 0:6942f0e2198c 287 // while(SwitchState == 0)
Josahty 0:6942f0e2198c 288 // {
Josahty 0:6942f0e2198c 289 // //driveMotor
Josahty 0:6942f0e2198c 290 // }
Josahty 0:6942f0e2198c 291 // calibrate();
Josahty 0:6942f0e2198c 292 // }
Josahty 0:6942f0e2198c 293 //}
simplyellow 10:cf77da9be0b8 294 //void DumpTruck::raiseTo(int angle) {
Josahty 0:6942f0e2198c 295 // pc.printf("Desired Angle: ");
Josahty 0:6942f0e2198c 296 // DesiredBedAngle = pc.getc();
Josahty 0:6942f0e2198c 297 // float CurrentBedAngle = getBedAngle();
Josahty 0:6942f0e2198c 298 // Difference = DesiredBedAngle-CurrentBedAngle;
Josahty 0:6942f0e2198c 299 // if (Difference > limit)
Josahty 0:6942f0e2198c 300 // {
Josahty 0:6942f0e2198c 301 // BedSpeed = setBedSpeed();
Josahty 0:6942f0e2198c 302 // pc.printf("Rotating to %s /n", DesiredBedAngle);
Josahty 0:6942f0e2198c 303 // //Drive Motors
Josahty 0:6942f0e2198c 304 // }
Josahty 0:6942f0e2198c 305 // else
Josahty 0:6942f0e2198c 306 // {stop()}
simplyellow 10:cf77da9be0b8 307 //}
Josahty 0:6942f0e2198c 308 //
Josahty 0:6942f0e2198c 309 //float DumpTruck::getPivotAngle(){
Josahty 0:6942f0e2198c 310 // float input = AnalogIn(x);
Josahty 0:6942f0e2198c 311 // return input
Josahty 0:6942f0e2198c 312 // }
Josahty 0:6942f0e2198c 313 //float DumpTruck::setPivotSpeed(){
Josahty 0:6942f0e2198c 314 // pc.printf("Please enter the Pivot speed /n");
Josahty 0:6942f0e2198c 315 // float input = pc.getc();
Josahty 0:6942f0e2198c 316 // return input;
Josahty 0:6942f0e2198c 317 // }
Josahty 0:6942f0e2198c 318 //
Josahty 0:6942f0e2198c 319 //float DumpTruck::setBedSpeed(){
Josahty 0:6942f0e2198c 320 // pc.printf("Please enter the Bed speed /n");
Josahty 0:6942f0e2198c 321 // float input = pc.getc();
Josahty 0:6942f0e2198c 322 // return input;
Josahty 0:6942f0e2198c 323 // }
Josahty 0:6942f0e2198c 324 //float DumpTruck::getBedAngle(){
Josahty 0:6942f0e2198c 325 // float input = AnalogIn(x); // data from potentiometer
Josahty 0:6942f0e2198c 326 // return input;
Josahty 0:6942f0e2198c 327 // }
simplyellow 10:cf77da9be0b8 328 //