Diff: DumpTruck.cpp

Revision:

14:192e103d5246

Parent:

13:112b6543909a

--- a/DumpTruck.cpp	Thu Apr 13 17:32:44 2017 +0000
+++ b/DumpTruck.cpp	Sun Apr 23 18:59:43 2017 +0000
@@ -3,7 +3,8 @@
 
 DumpTruck::DumpTruck(int truckId) : truckNumber(truckId) {
     truckNumber = truckId;
-        //track = new Tracker(p29, p30, p15);
+    track = new Tracker(p15, p16, p15);
+    IMU = new LSM9DS1(p17, p18, 0xD6, 0x3C);
     // PWM: 21, 22, 23
     // DIR: 27, 28, 29
     frontMotor = new Motor(p23, p28);
@@ -19,9 +20,9 @@
     rxDataCnt = 0;
     received = false;
     
-    void (*commands[2]);
-    commands[0] = DumpTruck::stopBed;
-    commands[1] = DumpTruck::stopDrive;
+    //void (*commands[2]);
+    //commands[0] = DumpTruck::stopBed;
+    //commands[1] = DumpTruck::stopDrive;
 }
 
 void DumpTruck::startComms() {
@@ -82,16 +83,16 @@
         //maybe commands.h is just the array
     //and the "on" becomes commands[k]?
     // real time driving
-    switch(rxData) {
-        case 1:
-            sendAck();
-            printf("ack\n\r");
-            break;
-        default:
-            sendNack();
-            printf("nack\n\r");
-            break;
-    }
+  //  switch(rxData) {
+    //    case 1:
+      //      sendAck();
+      //      printf("ack\n\r");
+      //      break;
+        //default:
+          //  sendNack();
+            //printf("nack\n\r");
+           // break;
+    //}
     getCommand();
 }
 
@@ -100,12 +101,21 @@
 }
 
 void DumpTruck::driveDistance(float speed, float distance) {
-    frontMotor->write(speed);
+    startDist = track->distance;
+    curDist   = track->distance;  
+    frontMotor->write(speed/10);
+    
+    while (abs(curDist - startDist) < distance) {
+        curDist = track->distance;
+    }
+   
+    frontMotor->write(0);
+    
     getCommand();
 }
 
 void DumpTruck::drive(float speed) {
-    frontMotor->write(speed);
+    frontMotor->write(speed/10);
     getCommand();
 }
 
@@ -114,13 +124,32 @@
     return;
 }
 
-void DumpTruck::moveBed(bool raise, float angle) {
+void DumpTruck::moveBed(bool upper, bool lower, float angle) {
+    ax = IMU->calcAccel(IMU->ax);
+    ay = IMU->calcAccel(IMU->ay);
+    az = IMU->calcAccel(IMU->az);
+    bedAngle = acos((ax0*ax + ay0*ay + az0*az)/(sqrt(ax0*ax0+ay0*ay0+az0*az0)*sqrt(ax*ax+ay*ay+az*az)))*180/PI;
+        
+        if(angle - bedAngle > 0) {
+            bedMotor->write(1);
+        }else{
+            bedMotor->write(-1);
+        } 
+       
+        while (abs(angle - bedAngle) > .05 and lower == false and upper == false) {
+            bedAngle = acos((ax0*ax + ay0*ay + az0*az)/(sqrt(ax0*ax0+ay0*ay0+az0*az0)*sqrt(ax*ax+ay*ay+az*az)))*180/PI;
+        }
+   
+    bedMotor->write(0);
     getCommand();
     return;
 }
 
 void DumpTruck::stopDrive() {
     frontMotor->write(0);
+    ax0 = IMU->calcAccel(IMU->ax);
+    ay0 = IMU->calcAccel(IMU->ay);
+    az0 = IMU->calcAccel(IMU->az);
     getCommand();
 }