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@3:cd6e2d7c7c9c, 2016-11-15 (annotated)

Committer:
Josahty
Date:
Tue Nov 15 21:30:12 2016 +0000
Revision:
3:cd6e2d7c7c9c
Parent:
2:7811df5a6052
Child:
4:2f1a0f628875
Updated drive() to monitor distance travelled by the encoder and to stop moving when the distance travelled = desired distance

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