Activities

Diff: DumpTruck.cpp

Revision:

15:a4bbdde8ed69

Parent:

14:fd6090cddc2e

Child:

16:6530f62dd28b

diff -r fd6090cddc2e -r a4bbdde8ed69 DumpTruck.cpp
--- a/DumpTruck.cpp	Tue Apr 25 05:24:47 2017 +0000
+++ b/DumpTruck.cpp	Tue Apr 25 20:54:15 2017 +0000
@@ -7,12 +7,16 @@
     //track = new Tracker(p29, p30, p15);
     // PWM: 21, 22, 23
     // DIR: 27, 28, 29
+    
+    //pot pin 19 analogin
     frontMotor = new Motor(p23, p28);
     turnMotor = new Motor(p22, p29);
-    bedMotor = new Motor(p23, p28);
+    bedMotor = new Motor(p24, p30);
+    pot = new AnalogIn(p19);
     //bed = new IMU(@@@@@@@);
-    srf = new SRF05(p15, p16); //trigger, echo.
+    srf = new SRF05(p20, p21); //trigger, echo.
     tooClose = false;
+    interrupted = false;
 
     nrf = new nRF24L01P(p5, p6, p7, p8, p9, p10); // transceiver
     //transceiver set up, similar to one on base station code
@@ -33,6 +37,10 @@
 
 void DumpTruck::startComms() {
     //must call first in main.cpp
+    //zeroAngle = (float) pot.read();
+    frontMotor->write(0);
+    turnMotor->write(0);
+    bedMotor->write(0);
     nrf->powerUp();
     printf("\n\r--------\r\n");
     printf("DUMP TRUCK\r\n");
@@ -47,7 +55,6 @@
         acked[i] = '0';
         nacked[i] = '1';
     }
-    
 }
 
 void DumpTruck::sendAck() {
@@ -96,28 +103,22 @@
             }
         }
     }
-    printf("%d, %d \r\n", commandEnd - 1, valueEnd);
+    //printf("%d, %d \r\n", commandEnd - 1, valueEnd);
     char commandChar[commandEnd];
     for(int g = 0; g < commandEnd; g++) {
         commandChar[g] = rxData[g];
     }
     
     //limit user input value to 2 digits
-    // "turn 360"
-    //commandEnd - 1 = 4
-    //valueEnd = 5
-    // "drived 5"
-    //commandEnd = 6
-    //valueEnd = 7
     int valSize = valueEnd - commandEnd + 1;
     //printf("no cast %i, cast %i \r\n", rxData[valueEnd]-'0', (int)rxData[valueEnd]-'0');
     if(valSize < 2) {       //filters out commands with no # appended
         if(valueEnd < 7) {  //two digit
             commandValue = 10*(int)((rxData[valueEnd])-'0') + (int)((rxData[valueEnd+1])-'0');
-            printf("2- command val is %i \r\n", commandValue);
+            //printf("2- command val is %i \r\n", commandValue);
         } else {            //one digit
             commandValue = (int)(rxData[valueEnd]-'0');
-            printf("1- command val is %i \r\n", commandValue);
+            //printf("1- command val is %i \r\n", commandValue);
         }
     }
 
@@ -138,6 +139,7 @@
             // commArr[0] = &DumpTruck::reportData;
             (this->*commArr[f])();
             sendAck();
+            sendAck();
             break;
         }
     }
@@ -153,6 +155,8 @@
 
 void DumpTruck::reportData(void) {
     printf("Report Data \r\n");
+    printf("Proximity %.2f\n\r", proximity);
+    //printf("Dump Truck is turned %0.2f degrees", ((float) pot - zeroAngle) * 300);
     //should run last in main thread
     getCommand();
 }
@@ -160,6 +164,7 @@
 void DumpTruck::driveDistance() {
     //float speed, float distance
     printf("Drive distance: %d \r\n", commandValue);
+    speed = (float) commandValue/10;
     frontMotor->write(speed);
     getCommand();
 }
@@ -167,6 +172,7 @@
 void DumpTruck::drive() {
     //commandValue = speed etc
     //float speed
+    speed = (float) commandValue/10;
     printf("Drive speed: %d \r\n", commandValue);
     frontMotor->write(speed);
     getCommand();
@@ -176,6 +182,9 @@
     //commandValue = angle etc
     //float angle
     printf("turn: %d degrees\r\n", commandValue);
+    turnMotor->write(0.6f);
+    wait(1);
+    turnMotor->write(0);
     getCommand();
     return;
 }
@@ -183,6 +192,13 @@
 void DumpTruck::moveBed() {
     //bool upper, bool lower, float angle
     printf("Move bed to angle: %d degrees \r\n", commandValue);
+    if(commandValue > 500) {
+        bedMotor->write(0.9f);
+    } else {
+        bedMotor->write(-0.1f);
+    }
+    wait(1);
+    bedMotor->write(0);
     getCommand();
     return;
 }
@@ -193,6 +209,13 @@
     getCommand();
 }
 
+void DumpTruck::detectStop() {
+    printf("Stopped all systems \r\n");
+    frontMotor->write(0);
+    bedMotor->write(0);
+    turnMotor->write(0);
+}
+
 void DumpTruck::stopTurn() {
     printf("Stop turn \r\n");
     turnMotor->write(0);
@@ -207,8 +230,9 @@
 
 bool DumpTruck::detect() {
     proximity = srf->read();
-    if(proximity < 20) {       //cm
+    if(proximity < 10) {       //cm
         tooClose = true;
+       // detectStop();
     } else {
         tooClose = false;
     }