Diff: DumpTruck.cpp

Revision:

13:112b6543909a

Parent:

12:502cf4fc6d98

Child:

14:fd6090cddc2e

--- a/DumpTruck.cpp	Thu Mar 30 17:27:21 2017 +0000
+++ b/DumpTruck.cpp	Thu Apr 13 17:32:44 2017 +0000
@@ -1,21 +1,27 @@
 #include "DumpTruck.h"
 
+
 DumpTruck::DumpTruck(int truckId) : truckNumber(truckId) {
     truckNumber = truckId;
         //track = new Tracker(p29, p30, p15);
     // PWM: 21, 22, 23
     // DIR: 27, 28, 29
     frontMotor = new Motor(p23, p28);
-        //turnMotor = new Motor(p22, p29);
-        //bedMotor = new Motor(p23, p28);
+    turnMotor = new Motor(p22, p29);
+    bedMotor = new Motor(p23, p28);
     //bed = new IMU(@@@@@@@);
     srf = new SRF05(p15, p16); //trigger, echo.
     tooClose = false;
-    
+
     nrf = new nRF24L01P(p5, p6, p7, p8, p9, p10); // transceiver
     //transceiver set up, similar to one on base station code
     txDataCnt = 0;
     rxDataCnt = 0;
+    received = false;
+    
+    void (*commands[2]);
+    commands[0] = DumpTruck::stopBed;
+    commands[1] = DumpTruck::stopDrive;
 }
 
 void DumpTruck::startComms() {
@@ -28,62 +34,104 @@
     nrf->setTransferSize( TRANSFER_SIZE );
     nrf->setReceiveMode();
     nrf->enable();
-    received = false;
+    
+    // fill acked array
+    for(int i = 0; i < TRANSFER_SIZE; i++) {
+        acked[i] = '0';
+        nacked[i] = '1';
+    }
+}
+
+void DumpTruck::sendAck() {
+    printf("SEND acked\n\r");
+    wait(1);
+    nrf->write(NRF24L01P_PIPE_P0, acked, TRANSFER_SIZE);
+    printf("SENT\n\r");
+}
+
+void DumpTruck::sendNack() {
+    printf("SEND nacked\n\r");
+    wait(1);
+    nrf->write(NRF24L01P_PIPE_P0, nacked, TRANSFER_SIZE);
+    printf("SENT\n\r");
 }
 
 void DumpTruck::getCommand() {
-    received = false;
     //called at the end of each executable command
-    //print statement saying dump truck is ready to receive
-        //have print statement appear in base station code
     //dump truck transceiver sends a code to base station, opening the channel
-        //code is all 0's
-            for(int i = 0; i < TRANSFER_SIZE; i++) {
-                txData[i] = 0;
-            }
-            nrf->write(NRF24L01P_PIPE_P0, txData, TRANSFER_SIZE);
-        //possibly implement kill switch/stop current action
-    //...
+    //code is all 0's
+    received = false;
+    //acked to receive command
+    sendAck();
+    //possibly implement kill switch/stop current action
     //code for receiving
     while(!received) {
-        if (nrf->readable() ) {
+        if (nrf->readable()) {
+        // ...read the data into the receive buffer
             rxDataCnt = nrf->read(NRF24L01P_PIPE_P0, rxData, sizeof(rxData));
-            /*
-            either go char by char to determine command or switch statement
-            for ( int j = 0; rxDataCnt > 0; rxDataCnt--, i++ ) {
+            received = true; //after receiving command
+            printf("received command\n\r");
+            processCommand();
+        }
+    }
+}
 
-                pc.putc( rxData[j] );
-            }*/
-        }
-        //received = true after receiving command
+void DumpTruck::processCommand() {
+    //strcmp(rxData,"on")==0
+    //perhaps have an array of all commands..??
+        //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 statement, sorting all commands
-    
+    getCommand();
 }
 
 void DumpTruck::reportData() {
     //should run last in main thread
-
 }
 
 void DumpTruck::driveDistance(float speed, float distance) {
     frontMotor->write(speed);
+    getCommand();
 }
 
 void DumpTruck::drive(float speed) {
     frontMotor->write(speed);
+    getCommand();
 }
 
 void DumpTruck::turn(float angle) {
+    getCommand();
     return;
 }
 
 void DumpTruck::moveBed(bool raise, float angle) {
+    getCommand();
     return;
 }
 
-void DumpTruck::stop() {
+void DumpTruck::stopDrive() {
     frontMotor->write(0);
+    getCommand();
+}
+
+void DumpTruck::stopTurn() {
+    turnMotor->write(0);
+    getCommand();
+}
+
+void DumpTruck::stopBed() {
+    bedMotor->write(0);
+    getCommand();
 }
 
 bool DumpTruck::detect() {