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@4:2f1a0f628875, 2016-11-29 (annotated)

Committer:
Josahty
Date:
Tue Nov 29 20:04:34 2016 +0000
Revision:
4:2f1a0f628875
Parent:
3:cd6e2d7c7c9c
Child:
5:dc4cf6cc24b3
trying to improve the Drive function by allowing it to take more than one character at a time from the terminal

Who changed what in which revision?

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