Terrabots / Mbed 2 deprecated DUMP_TRUCK_Test

test program

Dependencies:   LSM9DS1_Library2 MotorV2 SRF05 Tracker2 mbed nRF24L01P

Fork of DUMP_TRUCK_SPR2017 by Terrabots

DumpTruck.cpp@14:192e103d5246, 2017-04-23 (annotated)

Committer:
jcallahan1
Date:
Sun Apr 23 18:59:43 2017 +0000
Revision:
14:192e103d5246
Parent:
13:112b6543909a 
Test for dump truck

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Josahty 0:6942f0e2198c 1 #include "DumpTruck.h"
Josahty 0:6942f0e2198c 2
simplyellow 13:112b6543909a 3
Josahty 0:6942f0e2198c 4 DumpTruck::DumpTruck(int truckId) : truckNumber(truckId) {
Josahty 0:6942f0e2198c 5 truckNumber = truckId;
jcallahan1 14:192e103d5246 6 track = new Tracker(p15, p16, p15);
jcallahan1 14:192e103d5246 7 IMU = new LSM9DS1(p17, p18, 0xD6, 0x3C);
simplyellow 10:cf77da9be0b8 8 // PWM: 21, 22, 23
simplyellow 10:cf77da9be0b8 9 // DIR: 27, 28, 29
simplyellow 11:87f30625b213 10 frontMotor = new Motor(p23, p28);
simplyellow 13:112b6543909a 11 turnMotor = new Motor(p22, p29);
simplyellow 13:112b6543909a 12 bedMotor = new Motor(p23, p28);
simplyellow 10:cf77da9be0b8 13 //bed = new IMU(@@@@@@@);
simplyellow 11:87f30625b213 14 srf = new SRF05(p15, p16); //trigger, echo.
simplyellow 10:cf77da9be0b8 15 tooClose = false;
simplyellow 13:112b6543909a 16
simplyellow 12:502cf4fc6d98 17 nrf = new nRF24L01P(p5, p6, p7, p8, p9, p10); // transceiver
simplyellow 12:502cf4fc6d98 18 //transceiver set up, similar to one on base station code
simplyellow 12:502cf4fc6d98 19 txDataCnt = 0;
simplyellow 12:502cf4fc6d98 20 rxDataCnt = 0;
simplyellow 13:112b6543909a 21 received = false;
simplyellow 13:112b6543909a 22
jcallahan1 14:192e103d5246 23 //void (*commands[2]);
jcallahan1 14:192e103d5246 24 //commands[0] = DumpTruck::stopBed;
jcallahan1 14:192e103d5246 25 //commands[1] = DumpTruck::stopDrive;
simplyellow 12:502cf4fc6d98 26 }
simplyellow 12:502cf4fc6d98 27
simplyellow 12:502cf4fc6d98 28 void DumpTruck::startComms() {
simplyellow 12:502cf4fc6d98 29 nrf->powerUp();
simplyellow 12:502cf4fc6d98 30 printf( "nRF24L01+ Frequency : %d MHz\r\n", nrf->getRfFrequency() );
simplyellow 12:502cf4fc6d98 31 printf( "nRF24L01+ Output power : %d dBm\r\n", nrf->getRfOutputPower() );
simplyellow 12:502cf4fc6d98 32 printf( "nRF24L01+ Data Rate : %d kbps\r\n", nrf->getAirDataRate() );
simplyellow 12:502cf4fc6d98 33 printf( "nRF24L01+ TX Address : 0x%010llX\r\n", nrf->getTxAddress() );
simplyellow 12:502cf4fc6d98 34 printf( "nRF24L01+ RX Address : 0x%010llX\r\n", nrf->getRxAddress() );
simplyellow 12:502cf4fc6d98 35 nrf->setTransferSize( TRANSFER_SIZE );
simplyellow 12:502cf4fc6d98 36 nrf->setReceiveMode();
simplyellow 12:502cf4fc6d98 37 nrf->enable();
simplyellow 13:112b6543909a 38
simplyellow 13:112b6543909a 39 // fill acked array
simplyellow 13:112b6543909a 40 for(int i = 0; i < TRANSFER_SIZE; i++) {
simplyellow 13:112b6543909a 41 acked[i] = '0';
simplyellow 13:112b6543909a 42 nacked[i] = '1';
simplyellow 13:112b6543909a 43 }
simplyellow 13:112b6543909a 44 }
simplyellow 13:112b6543909a 45
simplyellow 13:112b6543909a 46 void DumpTruck::sendAck() {
simplyellow 13:112b6543909a 47 printf("SEND acked\n\r");
simplyellow 13:112b6543909a 48 wait(1);
simplyellow 13:112b6543909a 49 nrf->write(NRF24L01P_PIPE_P0, acked, TRANSFER_SIZE);
simplyellow 13:112b6543909a 50 printf("SENT\n\r");
simplyellow 13:112b6543909a 51 }
simplyellow 13:112b6543909a 52
simplyellow 13:112b6543909a 53 void DumpTruck::sendNack() {
simplyellow 13:112b6543909a 54 printf("SEND nacked\n\r");
simplyellow 13:112b6543909a 55 wait(1);
simplyellow 13:112b6543909a 56 nrf->write(NRF24L01P_PIPE_P0, nacked, TRANSFER_SIZE);
simplyellow 13:112b6543909a 57 printf("SENT\n\r");
simplyellow 12:502cf4fc6d98 58 }
simplyellow 12:502cf4fc6d98 59
simplyellow 12:502cf4fc6d98 60 void DumpTruck::getCommand() {
simplyellow 12:502cf4fc6d98 61 //called at the end of each executable command
simplyellow 12:502cf4fc6d98 62 //dump truck transceiver sends a code to base station, opening the channel
simplyellow 13:112b6543909a 63 //code is all 0's
simplyellow 13:112b6543909a 64 received = false;
simplyellow 13:112b6543909a 65 //acked to receive command
simplyellow 13:112b6543909a 66 sendAck();
simplyellow 13:112b6543909a 67 //possibly implement kill switch/stop current action
simplyellow 12:502cf4fc6d98 68 //code for receiving
simplyellow 12:502cf4fc6d98 69 while(!received) {
simplyellow 13:112b6543909a 70 if (nrf->readable()) {
simplyellow 13:112b6543909a 71 // ...read the data into the receive buffer
simplyellow 12:502cf4fc6d98 72 rxDataCnt = nrf->read(NRF24L01P_PIPE_P0, rxData, sizeof(rxData));
simplyellow 13:112b6543909a 73 received = true; //after receiving command
simplyellow 13:112b6543909a 74 printf("received command\n\r");
simplyellow 13:112b6543909a 75 processCommand();
simplyellow 13:112b6543909a 76 }
simplyellow 13:112b6543909a 77 }
simplyellow 13:112b6543909a 78 }
simplyellow 12:502cf4fc6d98 79
simplyellow 13:112b6543909a 80 void DumpTruck::processCommand() {
simplyellow 13:112b6543909a 81 //strcmp(rxData,"on")==0
simplyellow 13:112b6543909a 82 //perhaps have an array of all commands..??
simplyellow 13:112b6543909a 83 //maybe commands.h is just the array
simplyellow 13:112b6543909a 84 //and the "on" becomes commands[k]?
simplyellow 13:112b6543909a 85 // real time driving
jcallahan1 14:192e103d5246 86 // switch(rxData) {
jcallahan1 14:192e103d5246 87 // case 1:
jcallahan1 14:192e103d5246 88 // sendAck();
jcallahan1 14:192e103d5246 89 // printf("ack\n\r");
jcallahan1 14:192e103d5246 90 // break;
jcallahan1 14:192e103d5246 91 //default:
jcallahan1 14:192e103d5246 92 // sendNack();
jcallahan1 14:192e103d5246 93 //printf("nack\n\r");
jcallahan1 14:192e103d5246 94 // break;
jcallahan1 14:192e103d5246 95 //}
simplyellow 13:112b6543909a 96 getCommand();
simplyellow 12:502cf4fc6d98 97 }
simplyellow 12:502cf4fc6d98 98
simplyellow 12:502cf4fc6d98 99 void DumpTruck::reportData() {
simplyellow 12:502cf4fc6d98 100 //should run last in main thread
simplyellow 10:cf77da9be0b8 101 }
simplyellow 10:cf77da9be0b8 102
simplyellow 10:cf77da9be0b8 103 void DumpTruck::driveDistance(float speed, float distance) {
jcallahan1 14:192e103d5246 104 startDist = track->distance;
jcallahan1 14:192e103d5246 105 curDist = track->distance;
jcallahan1 14:192e103d5246 106 frontMotor->write(speed/10);
jcallahan1 14:192e103d5246 107
jcallahan1 14:192e103d5246 108 while (abs(curDist - startDist) < distance) {
jcallahan1 14:192e103d5246 109 curDist = track->distance;
jcallahan1 14:192e103d5246 110 }
jcallahan1 14:192e103d5246 111
jcallahan1 14:192e103d5246 112 frontMotor->write(0);
jcallahan1 14:192e103d5246 113
simplyellow 13:112b6543909a 114 getCommand();
simplyellow 10:cf77da9be0b8 115 }
simplyellow 10:cf77da9be0b8 116
simplyellow 10:cf77da9be0b8 117 void DumpTruck::drive(float speed) {
jcallahan1 14:192e103d5246 118 frontMotor->write(speed/10);
simplyellow 13:112b6543909a 119 getCommand();
Josahty 0:6942f0e2198c 120 }
Josahty 0:6942f0e2198c 121
simplyellow 10:cf77da9be0b8 122 void DumpTruck::turn(float angle) {
simplyellow 13:112b6543909a 123 getCommand();
simplyellow 10:cf77da9be0b8 124 return;
simplyellow 10:cf77da9be0b8 125 }
simplyellow 10:cf77da9be0b8 126
jcallahan1 14:192e103d5246 127 void DumpTruck::moveBed(bool upper, bool lower, float angle) {
jcallahan1 14:192e103d5246 128 ax = IMU->calcAccel(IMU->ax);
jcallahan1 14:192e103d5246 129 ay = IMU->calcAccel(IMU->ay);
jcallahan1 14:192e103d5246 130 az = IMU->calcAccel(IMU->az);
jcallahan1 14:192e103d5246 131 bedAngle = acos((ax0*ax + ay0*ay + az0*az)/(sqrt(ax0*ax0+ay0*ay0+az0*az0)*sqrt(ax*ax+ay*ay+az*az)))*180/PI;
jcallahan1 14:192e103d5246 132
jcallahan1 14:192e103d5246 133 if(angle - bedAngle > 0) {
jcallahan1 14:192e103d5246 134 bedMotor->write(1);
jcallahan1 14:192e103d5246 135 }else{
jcallahan1 14:192e103d5246 136 bedMotor->write(-1);
jcallahan1 14:192e103d5246 137 }
jcallahan1 14:192e103d5246 138
jcallahan1 14:192e103d5246 139 while (abs(angle - bedAngle) > .05 and lower == false and upper == false) {
jcallahan1 14:192e103d5246 140 bedAngle = acos((ax0*ax + ay0*ay + az0*az)/(sqrt(ax0*ax0+ay0*ay0+az0*az0)*sqrt(ax*ax+ay*ay+az*az)))*180/PI;
jcallahan1 14:192e103d5246 141 }
jcallahan1 14:192e103d5246 142
jcallahan1 14:192e103d5246 143 bedMotor->write(0);
simplyellow 13:112b6543909a 144 getCommand();
simplyellow 10:cf77da9be0b8 145 return;
simplyellow 10:cf77da9be0b8 146 }
simplyellow 10:cf77da9be0b8 147
simplyellow 13:112b6543909a 148 void DumpTruck::stopDrive() {
simplyellow 11:87f30625b213 149 frontMotor->write(0);
jcallahan1 14:192e103d5246 150 ax0 = IMU->calcAccel(IMU->ax);
jcallahan1 14:192e103d5246 151 ay0 = IMU->calcAccel(IMU->ay);
jcallahan1 14:192e103d5246 152 az0 = IMU->calcAccel(IMU->az);
simplyellow 13:112b6543909a 153 getCommand();
simplyellow 13:112b6543909a 154 }
simplyellow 13:112b6543909a 155
simplyellow 13:112b6543909a 156 void DumpTruck::stopTurn() {
simplyellow 13:112b6543909a 157 turnMotor->write(0);
simplyellow 13:112b6543909a 158 getCommand();
simplyellow 13:112b6543909a 159 }
simplyellow 13:112b6543909a 160
simplyellow 13:112b6543909a 161 void DumpTruck::stopBed() {
simplyellow 13:112b6543909a 162 bedMotor->write(0);
simplyellow 13:112b6543909a 163 getCommand();
simplyellow 10:cf77da9be0b8 164 }
simplyellow 10:cf77da9be0b8 165
simplyellow 10:cf77da9be0b8 166 bool DumpTruck::detect() {
simplyellow 10:cf77da9be0b8 167 proximity = srf->read();
simplyellow 11:87f30625b213 168 if(proximity < 20) { //cm
simplyellow 10:cf77da9be0b8 169 tooClose = true;
simplyellow 10:cf77da9be0b8 170 } else {
simplyellow 10:cf77da9be0b8 171 tooClose = false;
simplyellow 10:cf77da9be0b8 172 }
simplyellow 10:cf77da9be0b8 173 return tooClose;
simplyellow 10:cf77da9be0b8 174 }
Josahty 0:6942f0e2198c 175
Josahty 0:6942f0e2198c 176
Josahty 3:cd6e2d7c7c9c 177 //void Calibrate(){}
Josahty 3:cd6e2d7c7c9c 178
Josahty 3:cd6e2d7c7c9c 179 //void DumpTruck::drive() {
Josahty 3:cd6e2d7c7c9c 180 // // get desired distance and speed
Josahty 3:cd6e2d7c7c9c 181 // distance = 0; // placeholder
Josahty 3:cd6e2d7c7c9c 182 // // speed = 0.5;
Josahty 3:cd6e2d7c7c9c 183 // // distance = setTruckDistance();
Josahty 3:cd6e2d7c7c9c 184 // speed = setTruckSpeed();
Josahty 3:cd6e2d7c7c9c 185 // // terminal confirmation of distance and speed
Josahty 3:cd6e2d7c7c9c 186 // pc.printf("Truck will drive a distance of %f at a speed of %f\r\n",distance,speed);
Josahty 3:cd6e2d7c7c9c 187 // // insert while loop here which relates to distance travelled
Josahty 3:cd6e2d7c7c9c 188 // while(1) {
Josahty 3:cd6e2d7c7c9c 189 // // if the speed is greater than zero, go forward at specified speed
Josahty 3:cd6e2d7c7c9c 190 // if (speed >= 0) {
Josahty 3:cd6e2d7c7c9c 191 // DirPin = 1;
Josahty 3:cd6e2d7c7c9c 192 // DrivePin = speed;
simplyellow 10:cf77da9be0b8 193 // } // else, go backwards at specified speed
Josahty 3:cd6e2d7c7c9c 194 // else {
Josahty 3:cd6e2d7c7c9c 195 // DirPin = 0;
Josahty 3:cd6e2d7c7c9c 196 // DrivePin = -1*speed;
Josahty 3:cd6e2d7c7c9c 197 // }
Josahty 3:cd6e2d7c7c9c 198 // }
Josahty 3:cd6e2d7c7c9c 199 // // stop motors
Josahty 3:cd6e2d7c7c9c 200 // // stop();
Josahty 3:cd6e2d7c7c9c 201 // // calibrate motors
Josahty 3:cd6e2d7c7c9c 202 // Calibrate();
Josahty 3:cd6e2d7c7c9c 203 // //
Josahty 3:cd6e2d7c7c9c 204 // travelled = 0;
Josahty 3:cd6e2d7c7c9c 205 //}
Josahty 3:cd6e2d7c7c9c 206
simplyellow 10:cf77da9be0b8 207 /*void DumpTruck::drive() {
Josahty 0:6942f0e2198c 208 // get desired distance and speed
Josahty 3:cd6e2d7c7c9c 209 // placeholder
Josahty 0:6942f0e2198c 210 // speed = 0.5;
Josahty 3:cd6e2d7c7c9c 211 //distance = 5.0;
Josahty 3:cd6e2d7c7c9c 212 distance = setTruckDistance();
Josahty 0:6942f0e2198c 213 speed = setTruckSpeed();
Josahty 0:6942f0e2198c 214 // terminal confirmation of distance and speed
Josahty 3:cd6e2d7c7c9c 215 pc.printf("Truck will drive a distance of %f inches at a speed of %f\r\n",distance,speed);
Josahty 0:6942f0e2198c 216 // insert while loop here which relates to distance travelled
Josahty 3:cd6e2d7c7c9c 217 travelled = getDistance();
Josahty 3:cd6e2d7c7c9c 218 //pc.printf("Check 1: %f, %f\r\n", distance, travelled);
simplyellow 10:cf77da9be0b8 219 while(travelled < distance) {
Josahty 3:cd6e2d7c7c9c 220 //pc.printf("Check 2:\r\n");
Josahty 3:cd6e2d7c7c9c 221 travelled = getDistance();
Josahty 3:cd6e2d7c7c9c 222 pc.printf("Travelling...: %f\n\r", (distance - travelled));
Josahty 0:6942f0e2198c 223 // if the speed is greater than zero, go forward at specified speed
Josahty 0:6942f0e2198c 224 if (speed >= 0) {
Josahty 0:6942f0e2198c 225 DirPin = 1;
Josahty 0:6942f0e2198c 226 DrivePin = speed;
simplyellow 10:cf77da9be0b8 227 } // else, go backwards at specified speed
Josahty 0:6942f0e2198c 228 else {
Josahty 0:6942f0e2198c 229 DirPin = 0;
Josahty 2:7811df5a6052 230 DrivePin = -1*speed;
Josahty 3:cd6e2d7c7c9c 231 }
Josahty 0:6942f0e2198c 232 }
Josahty 3:cd6e2d7c7c9c 233 //pc.printf("Check 3:\r\n");
Josahty 0:6942f0e2198c 234 // stop motors
Josahty 0:6942f0e2198c 235 // stop();
Josahty 0:6942f0e2198c 236 // calibrate motors
Josahty 3:cd6e2d7c7c9c 237 //Calibrate();
Josahty 0:6942f0e2198c 238 }
Josahty 0:6942f0e2198c 239
Josahty 3:cd6e2d7c7c9c 240
Josahty 3:cd6e2d7c7c9c 241
Josahty 3:cd6e2d7c7c9c 242
Josahty 3:cd6e2d7c7c9c 243 float DumpTruck::setTruckDistance(){
Josahty 3:cd6e2d7c7c9c 244 // terminal prompt for distance
Josahty 3:cd6e2d7c7c9c 245 pc.printf("Please enter the truck distance:\r\n");
Josahty 3:cd6e2d7c7c9c 246 // sets return value to given value
Josahty 4:2f1a0f628875 247 //float input = pc.getc();
Josahty 4:2f1a0f628875 248 //input = input - 48;
Josahty 3:cd6e2d7c7c9c 249 // returns value
Josahty 4:2f1a0f628875 250 //return input;
simplyellow 10:cf77da9be0b8 251
Josahty 4:2f1a0f628875 252 char c;
simplyellow 10:cf77da9be0b8 253 char buffer[128];
Josahty 4:2f1a0f628875 254 pc.gets(buffer, 4);
Josahty 4:2f1a0f628875 255 float input = strtod(buffer,NULL);
Josahty 3:cd6e2d7c7c9c 256 return input;
simplyellow 10:cf77da9be0b8 257
Josahty 3:cd6e2d7c7c9c 258 }
Josahty 3:cd6e2d7c7c9c 259
Josahty 0:6942f0e2198c 260 float DumpTruck::setTruckSpeed(){
Josahty 4:2f1a0f628875 261 // float mod = 0;
Josahty 4:2f1a0f628875 262 // float div = 10;
Josahty 4:2f1a0f628875 263 // pc.printf("Enter a direction - 1 = forward, 0 = backward:\r\n");
Josahty 4:2f1a0f628875 264 // float dir = pc.getc();
Josahty 4:2f1a0f628875 265 // while ((dir<48)||(dir>49)){
Josahty 4:2f1a0f628875 266 // pc.printf("Invalid input. Please enter a valid direction, 1 or 0: \r\n");
Josahty 4:2f1a0f628875 267 // dir = pc.getc();
Josahty 4:2f1a0f628875 268 // }
Josahty 4:2f1a0f628875 269 // if (dir == 48) {
Josahty 4:2f1a0f628875 270 // mod = -1; }
Josahty 4:2f1a0f628875 271 // else {
simplyellow 10:cf77da9be0b8 272 // mod = 1; }
simplyellow 10:cf77da9be0b8 273 //
Josahty 4:2f1a0f628875 274 // // terminal prompt for speed
Josahty 4:2f1a0f628875 275 // pc.printf("Please enter a truck speed from 0-9 or F for full speed:\r\n");
Josahty 4:2f1a0f628875 276 // // sets return value to given value
Josahty 4:2f1a0f628875 277 // float input = pc.getc();
Josahty 4:2f1a0f628875 278 // while ((input<48)||(input>57)){
Josahty 4:2f1a0f628875 279 // if((input == 70) || (input == 102)) {
Josahty 4:2f1a0f628875 280 // input = 1;
Josahty 4:2f1a0f628875 281 // input = input * mod;
Josahty 4:2f1a0f628875 282 // return input;
Josahty 4:2f1a0f628875 283 // }
Josahty 4:2f1a0f628875 284 // pc.printf("Invalid input. Please enter a valid speed from 0-9 or F: \r\n");
Josahty 4:2f1a0f628875 285 // input = pc.getc();
Josahty 4:2f1a0f628875 286 // }
Josahty 4:2f1a0f628875 287 // input = input - 48;
Josahty 4:2f1a0f628875 288 // input = input * (1/div);
Josahty 4:2f1a0f628875 289 // // returns value
Josahty 4:2f1a0f628875 290 // input = input * mod;
simplyellow 10:cf77da9be0b8 291
simplyellow 10:cf77da9be0b8 292
simplyellow 10:cf77da9be0b8 293 pc.printf("Enter a speed from -10 to +10:\r\n");
Josahty 4:2f1a0f628875 294 char c;
simplyellow 10:cf77da9be0b8 295 char buffer[128];
Josahty 4:2f1a0f628875 296 pc.gets(buffer, 4);
Josahty 4:2f1a0f628875 297 float input = strtod(buffer,NULL);
simplyellow 10:cf77da9be0b8 298
Josahty 4:2f1a0f628875 299 while ((buffer[0] < 43) || (buffer[0] > 45)) {
Josahty 4:2f1a0f628875 300 pc.printf("Invalid input. Please correct input and try again:\r\n");
Josahty 4:2f1a0f628875 301 setTruckSpeed();
Josahty 4:2f1a0f628875 302 }
simplyellow 10:cf77da9be0b8 303
Josahty 4:2f1a0f628875 304 pc.printf("buffer[0]: %d\r\n",buffer[0]);
Josahty 4:2f1a0f628875 305 pc.printf("buffer[1]: %d\r\n",buffer[1]);
Josahty 4:2f1a0f628875 306 pc.printf("buffer[2]: %d\r\n",buffer[2]);
Josahty 4:2f1a0f628875 307 input = input / 10;
Josahty 4:2f1a0f628875 308
Josahty 4:2f1a0f628875 309 return input;}
simplyellow 10:cf77da9be0b8 310
simplyellow 10:cf77da9be0b8 311
Josahty 0:6942f0e2198c 312 void DumpTruck::stop(){
Josahty 2:7811df5a6052 313 pc.printf("Truck will now stop.\r\n");
Josahty 0:6942f0e2198c 314 //stop motors
Josahty 0:6942f0e2198c 315 DrivePin = 0;
Josahty 0:6942f0e2198c 316 }
simplyellow 10:cf77da9be0b8 317
Josahty 3:cd6e2d7c7c9c 318 float DumpTruck::getDistance() {
Josahty 3:cd6e2d7c7c9c 319 //while(1){
Josahty 3:cd6e2d7c7c9c 320 pulseCount = (float) wheel.getPulses();
Josahty 3:cd6e2d7c7c9c 321 travelled = pulseCount*d/encoding/20/68;
Josahty 3:cd6e2d7c7c9c 322 wait(0.1);
Josahty 3:cd6e2d7c7c9c 323 //pc.printf("Inches travelled: %f\n\r", travelled);
Josahty 3:cd6e2d7c7c9c 324 return travelled;
Josahty 3:cd6e2d7c7c9c 325 //(pulse count / X * N) * (1 / PPI)
simplyellow 10:cf77da9be0b8 326
Josahty 3:cd6e2d7c7c9c 327 //}
simplyellow 10:cf77da9be0b8 328 }*/
Josahty 0:6942f0e2198c 329
simplyellow 10:cf77da9be0b8 330 //void DumpTruck::RotateTo() {
Josahty 0:6942f0e2198c 331 // pc.printf("Desired Angle: ");
Josahty 0:6942f0e2198c 332 // DesiredPivotAngle = pc.getc();
Josahty 0:6942f0e2198c 333 // float CurrentPivotAngle = getPivotAngle();
Josahty 0:6942f0e2198c 334 // Difference = DesiredPivotAngle-CurrentPivotAngle;
Josahty 0:6942f0e2198c 335 // if (Difference > limit)
Josahty 0:6942f0e2198c 336 // {
Josahty 0:6942f0e2198c 337 // PivotSpeed = setPivotSpeed();
Josahty 0:6942f0e2198c 338 // pc.printf("Rotating to %s /n", DesiredPivotAngle);
Josahty 0:6942f0e2198c 339 // //Drive Motors
Josahty 0:6942f0e2198c 340 // }
Josahty 0:6942f0e2198c 341 // else
Josahty 0:6942f0e2198c 342 // {stop()}
Josahty 0:6942f0e2198c 343 //
simplyellow 10:cf77da9be0b8 344 //}
simplyellow 10:cf77da9be0b8 345 //
simplyellow 10:cf77da9be0b8 346 //void DumpTruck::BedDown() {
Josahty 0:6942f0e2198c 347 // SwitchState = bool GetBedUpperSwitch();
Josahty 0:6942f0e2198c 348 // if SwitchState
Josahty 0:6942f0e2198c 349 // {return 0;}
Josahty 0:6942f0e2198c 350 // else
Josahty 0:6942f0e2198c 351 //
Josahty 0:6942f0e2198c 352 // while(SwitchState == 0)
Josahty 0:6942f0e2198c 353 // {
Josahty 0:6942f0e2198c 354 // //driveMotor
Josahty 0:6942f0e2198c 355 // }
Josahty 0:6942f0e2198c 356 // calibrate();
simplyellow 10:cf77da9be0b8 357 // }
Josahty 0:6942f0e2198c 358 //void DumpTruck::LowerBed() {
Josahty 0:6942f0e2198c 359 // SwitchState = bool GetLowerUpperSwitch();
Josahty 0:6942f0e2198c 360 // if SwitchState
Josahty 0:6942f0e2198c 361 // {return 0;}
Josahty 0:6942f0e2198c 362 // else
Josahty 0:6942f0e2198c 363 //
Josahty 0:6942f0e2198c 364 // while(SwitchState == 0)
Josahty 0:6942f0e2198c 365 // {
Josahty 0:6942f0e2198c 366 // //driveMotor
Josahty 0:6942f0e2198c 367 // }
Josahty 0:6942f0e2198c 368 // calibrate();
Josahty 0:6942f0e2198c 369 // }
Josahty 0:6942f0e2198c 370 //}
simplyellow 10:cf77da9be0b8 371 //void DumpTruck::raiseTo(int angle) {
Josahty 0:6942f0e2198c 372 // pc.printf("Desired Angle: ");
Josahty 0:6942f0e2198c 373 // DesiredBedAngle = pc.getc();
Josahty 0:6942f0e2198c 374 // float CurrentBedAngle = getBedAngle();
Josahty 0:6942f0e2198c 375 // Difference = DesiredBedAngle-CurrentBedAngle;
Josahty 0:6942f0e2198c 376 // if (Difference > limit)
Josahty 0:6942f0e2198c 377 // {
Josahty 0:6942f0e2198c 378 // BedSpeed = setBedSpeed();
Josahty 0:6942f0e2198c 379 // pc.printf("Rotating to %s /n", DesiredBedAngle);
Josahty 0:6942f0e2198c 380 // //Drive Motors
Josahty 0:6942f0e2198c 381 // }
Josahty 0:6942f0e2198c 382 // else
Josahty 0:6942f0e2198c 383 // {stop()}
simplyellow 10:cf77da9be0b8 384 //}
Josahty 0:6942f0e2198c 385 //
Josahty 0:6942f0e2198c 386 //float DumpTruck::getPivotAngle(){
Josahty 0:6942f0e2198c 387 // float input = AnalogIn(x);
Josahty 0:6942f0e2198c 388 // return input
Josahty 0:6942f0e2198c 389 // }
Josahty 0:6942f0e2198c 390 //float DumpTruck::setPivotSpeed(){
Josahty 0:6942f0e2198c 391 // pc.printf("Please enter the Pivot speed /n");
Josahty 0:6942f0e2198c 392 // float input = pc.getc();
Josahty 0:6942f0e2198c 393 // return input;
Josahty 0:6942f0e2198c 394 // }
Josahty 0:6942f0e2198c 395 //
Josahty 0:6942f0e2198c 396 //float DumpTruck::setBedSpeed(){
Josahty 0:6942f0e2198c 397 // pc.printf("Please enter the Bed speed /n");
Josahty 0:6942f0e2198c 398 // float input = pc.getc();
Josahty 0:6942f0e2198c 399 // return input;
Josahty 0:6942f0e2198c 400 // }
Josahty 0:6942f0e2198c 401 //float DumpTruck::getBedAngle(){
Josahty 0:6942f0e2198c 402 // float input = AnalogIn(x); // data from potentiometer
Josahty 0:6942f0e2198c 403 // return input;
Josahty 0:6942f0e2198c 404 // }
simplyellow 10:cf77da9be0b8 405 //