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@5:dc4cf6cc24b3, 2017-02-07 (annotated)

Committer:
simplyellow
Date:
Tue Feb 07 21:01:20 2017 +0000
Revision:
5:dc4cf6cc24b3
Parent:
4:2f1a0f628875
Child:
6:5cc6a9b6f60e
Dump Truck

Who changed what in which revision?

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