Revision 14:192e103d5246, committed 2017-04-23

Comitter:

jcallahan1

Date:

Sun Apr 23 18:59:43 2017 +0000

Parent:

13:112b6543909a

Child:

15:d473d109983a

Commit message:

Test for dump truck

Changed in this revision

--- 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();
 }
 

--- a/DumpTruck.h	Thu Apr 13 17:32:44 2017 +0000
+++ b/DumpTruck.h	Sun Apr 23 18:59:43 2017 +0000
@@ -16,6 +16,7 @@
 #include "Motor.h"
 #include "nRF24L01P.h" //Transceiver library
 #include "SRF05.h"
+#include "LSM9DS1.h"
 
 //using strings to process commands
 #include <string> 
@@ -89,21 +90,21 @@
     /*
     *   Move the dump truck's bed up/down a certain angle.
     */
-    void moveBed(bool raise, float angle);  // bedMotor
+    void moveBed(bool upper, bool lower, float angle);  // bedMotor
     /*
     *   Stop driving.
     */
-    int stopDrive();                            // all Motors
+    void stopDrive();                            // all Motors
     
     /*
     *   Stop turning.
     */
-    int stopTurn();                            // all Motors
+    void stopTurn();                            // all Motors
 
     /*
     *   Stop bed.
     */
-    int stopBed();                            // all Motors
+    void stopBed();                            // all Motors
 
     // ultrasonic functions
     /*
@@ -128,6 +129,7 @@
     Motor *bedMotor;
     nRF24L01P *nrf; //Transceiver *nrf;
     SRF05 *srf;
+    LSM9DS1 *IMU;
 
     // add direct control for motor, switch, and ultrasonic sensor.
 
@@ -136,11 +138,19 @@
     // motor variables
     float speed;                            // drive
     float distance;                         // drive
+    float startDist;                        // drive
+    float curDist;                          // drive
     float potAngle;                         // turn
     float bedAngle;                         // bed
 
     // bed-specific variables
     bool switchState;   // BusIn the two limit switches
+    float ax0;
+    float ay0;
+    float az0;
+    float ax;
+    float ay;
+    float az;
 
     // ultrasonic-specific variables
     bool tooClose;

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM9DS1_Library.lib	Sun Apr 23 18:59:43 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/4180_1/code/LSM9DS1_Library/#83768042e0f3

--- a/Tracker.lib	Thu Apr 13 17:32:44 2017 +0000
+++ b/Tracker.lib	Sun Apr 23 18:59:43 2017 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/Terrabots/code/Tracker/#eb8680da1721
+http://developer.mbed.org/teams/Terrabots/code/Tracker/#58157608cfbe

--- a/main.cpp	Thu Apr 13 17:32:44 2017 +0000
+++ b/main.cpp	Sun Apr 23 18:59:43 2017 +0000
@@ -7,9 +7,10 @@
 to the dump truck.
 */
 
-int main() {   
-    dump.startComms();
-    dump.getCommand();
+int main() {
+   dump.driveDistance(5,6);   
+   // dump.startComms();
+   // dump.getCommand();
     /*
     for(int i = 0; i < dump.rxDataCnt; i++) {
         pc.putc(dump.rxData[i]);