Activities

Diff: DumpTruck.cpp

Revision:

3:cd6e2d7c7c9c

Parent:

2:7811df5a6052

Child:

4:2f1a0f628875

--- a/DumpTruck.cpp	Tue Nov 15 20:04:54 2016 +0000
+++ b/DumpTruck.cpp	Tue Nov 15 21:30:12 2016 +0000
@@ -1,5 +1,7 @@
 #include "DumpTruck.h"
 #include "stdlib.h"
+#include "QEI.h"
+
 
 Serial pc(USBTX, USBRX);
 
@@ -7,23 +9,63 @@
     truckNumber = truckId;
 }
 
+QEI wheel (p15, p16, NC, 128, QEI::X4_ENCODING); //orange, yellow
+float pulseCount; //number of pulses detected by encoder
+float encoding = 2; //X2
+float d = 13.25; //inches of circumference
+float distance;
 PwmOut DrivePin(p24);
 DigitalOut DirPin(p30);
-int travelled;
+float travelled;
 
 
-void Calibrate(){}
+//void Calibrate(){}
+
+//void DumpTruck::drive() {
+//    // get desired distance and speed
+//    distance = 0; // placeholder
+//    // speed = 0.5;
+//    // distance = setTruckDistance();
+//    speed = setTruckSpeed();
+//    // terminal confirmation of distance and speed
+//    pc.printf("Truck will drive a distance of %f at a speed of %f\r\n",distance,speed);
+//    // insert while loop here which relates to distance travelled
+//    while(1) {
+//    // if the speed is greater than zero, go forward at specified speed
+//    if (speed >= 0) {
+//    DirPin = 1;
+//    DrivePin = speed;
+//    } // else, go backwards at specified speed  
+//    else {
+//    DirPin = 0;
+//    DrivePin = -1*speed;
+//    }
+//    }
+//    // stop motors
+//    // stop();
+//    // calibrate motors
+//    Calibrate();
+//    //
+//    travelled = 0;
+//}
+
 
 void DumpTruck::drive() {
     // get desired distance and speed
-    distance = 0; // placeholder
+     // placeholder
     // speed = 0.5;
-    // distance = setTruckDistance();
+    //distance = 5.0;
+    distance = setTruckDistance();
     speed = setTruckSpeed();
     // terminal confirmation of distance and speed
-    pc.printf("Truck will drive a distance of %f at a speed of %f\r\n",distance,speed);
+    pc.printf("Truck will drive a distance of %f inches at a speed of %f\r\n",distance,speed);
     // insert while loop here which relates to distance travelled
-    while(1) {
+    travelled = getDistance();
+    //pc.printf("Check 1: %f, %f\r\n", distance, travelled);
+    while(travelled < distance) {    
+    //pc.printf("Check 2:\r\n");
+    travelled = getDistance();
+    pc.printf("Travelling...: %f\n\r", (distance - travelled));
     // if the speed is greater than zero, go forward at specified speed
     if (speed >= 0) {
     DirPin = 1;
@@ -32,25 +74,28 @@
     else {
     DirPin = 0;
     DrivePin = -1*speed;
+         }
     }
-    }
+    //pc.printf("Check 3:\r\n");
     // stop motors
     // stop();
     // calibrate motors
-    Calibrate();
-    //
-    travelled = 0;
+    //Calibrate();
 }
 
-//float DumpTruck::setTruckDistance(){
-//    // terminal prompt for distance
-//    pc.printf("Please enter the truck distance:");
-//    // sets return value to given value
-//    float input = pc.getc();
-//    // returns value
-//    return input;
-//    }
-//
+
+
+
+float DumpTruck::setTruckDistance(){
+    // terminal prompt for distance
+    pc.printf("Please enter the truck distance:\r\n");
+    // sets return value to given value
+    float input = pc.getc();
+    input = input - 48;
+    // returns value
+    return input;
+    }
+
 float DumpTruck::setTruckSpeed(){
       float mod = 0;
       float div = 10;
@@ -90,6 +135,18 @@
     //stop motors
     DrivePin = 0;
     }
+    
+float DumpTruck::getDistance() {
+    //while(1){
+        pulseCount = (float) wheel.getPulses();
+        travelled = pulseCount*d/encoding/20/68;
+        wait(0.1);
+        //pc.printf("Inches travelled: %f\n\r", travelled);
+        return travelled;
+        //(pulse count / X * N) * (1 / PPI)
+        
+    //}
+}    
 
 //void DumpTruck::RotateTo() {  
 //    pc.printf("Desired Angle: ");