Activities

Diff: DumpTruck.cpp

Revision:

14:fd6090cddc2e

Parent:

13:112b6543909a

Child:

15:a4bbdde8ed69

--- a/DumpTruck.cpp	Thu Apr 13 17:32:44 2017 +0000
+++ b/DumpTruck.cpp	Tue Apr 25 05:24:47 2017 +0000
@@ -1,9 +1,10 @@
 #include "DumpTruck.h"
-
+#include <iostream>
+#include <string>
 
 DumpTruck::DumpTruck(int truckId) : truckNumber(truckId) {
     truckNumber = truckId;
-        //track = new Tracker(p29, p30, p15);
+    //track = new Tracker(p29, p30, p15);
     // PWM: 21, 22, 23
     // DIR: 27, 28, 29
     frontMotor = new Motor(p23, p28);
@@ -18,19 +19,25 @@
     txDataCnt = 0;
     rxDataCnt = 0;
     received = false;
+    numCommands = sizeof(commands)/sizeof(*commands);
     
-    void (*commands[2]);
-    commands[0] = DumpTruck::stopBed;
-    commands[1] = DumpTruck::stopDrive;
+    commArr[0] = &DumpTruck::reportData;
+    commArr[1] = &DumpTruck::driveDistance;
+    commArr[2] = &DumpTruck::drive;
+    commArr[3] = &DumpTruck::turn;
+    commArr[4] = &DumpTruck::moveBed;
+    commArr[5] = &DumpTruck::stopBed;
+    commArr[6] = &DumpTruck::stopTurn;
+    commArr[7] = &DumpTruck::stopDrive;
 }
 
 void DumpTruck::startComms() {
+    //must call first in main.cpp
     nrf->powerUp();
-    printf( "nRF24L01+ Frequency    : %d MHz\r\n",  nrf->getRfFrequency() );
-    printf( "nRF24L01+ Output power : %d dBm\r\n",  nrf->getRfOutputPower() );
-    printf( "nRF24L01+ Data Rate    : %d kbps\r\n", nrf->getAirDataRate() );
-    printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", nrf->getTxAddress() );
-    printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", nrf->getRxAddress() );
+    printf("\n\r--------\r\n");
+    printf("DUMP TRUCK\r\n");
+    printf("ID: %d \r\n", truckNumber);
+    printf("--------\r\n");
     nrf->setTransferSize( TRANSFER_SIZE );
     nrf->setReceiveMode();
     nrf->enable();
@@ -40,20 +47,19 @@
         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");
+    printf("ACK 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");
+    printf("NACK SENT\n\r");
 }
 
 void DumpTruck::getCommand() {
@@ -77,59 +83,124 @@
 }
 
 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:
+    printf("Processing command...\n\r");
+    int commandEnd = 99;
+    int valueEnd = 99;
+    for(int h = 0; h < TRANSFER_SIZE; h++) {
+        if(rxData[h] == ' ' || h == TRANSFER_SIZE - 1) {
+            if(commandEnd == 99) {
+                commandEnd = h + 1;
+            }
+            if((rxData[h+1] != ' ')&&(h < TRANSFER_SIZE-1)) {
+                valueEnd = h+1;
+            }
+        }
+    }
+    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);
+        } else {            //one digit
+            commandValue = (int)(rxData[valueEnd]-'0');
+            printf("1- command val is %i \r\n", commandValue);
+        }
+    }
+
+    printf("There are %d commands available\n\r", sizeof(commands)/sizeof(*commands));
+    int f;
+    for(f = 0; f < numCommands; f++ ) {
+        if(strcmp(commandChar,commands[f])==32) {
+            printf("COMMAND #%i: ", f+1);
+            for(int a = 0; a < commandEnd; a++) {
+                printf("%c", commandChar[a]);
+            }
+            printf("\r\nORIGINAL: ");
+            for(int b = 0; b < TRANSFER_SIZE; b++) {
+                printf("%c", rxData[b]);
+            }  
+            printf("\r\n");       
+            // syntax: (this->*func)(foo, bar);
+            // commArr[0] = &DumpTruck::reportData;
+            (this->*commArr[f])();
             sendAck();
-            printf("ack\n\r");
             break;
-        default:
-            sendNack();
-            printf("nack\n\r");
-            break;
+        }
     }
+    if(f == numCommands) {
+        //base station still interprets this as an ack because 
+        //the dump truck tells it that it's ready to receive another command
+        sendNack();
+        printf("Command not recognized. \r\n");
+    }
+    printf("Waiting for next command\n\r");
     getCommand();
 }
 
-void DumpTruck::reportData() {
+void DumpTruck::reportData(void) {
+    printf("Report Data \r\n");
     //should run last in main thread
+    getCommand();
 }
 
-void DumpTruck::driveDistance(float speed, float distance) {
+void DumpTruck::driveDistance() {
+    //float speed, float distance
+    printf("Drive distance: %d \r\n", commandValue);
     frontMotor->write(speed);
     getCommand();
 }
 
-void DumpTruck::drive(float speed) {
+void DumpTruck::drive() {
+    //commandValue = speed etc
+    //float speed
+    printf("Drive speed: %d \r\n", commandValue);
     frontMotor->write(speed);
     getCommand();
 }
 
-void DumpTruck::turn(float angle) {
+void DumpTruck::turn() {
+    //commandValue = angle etc
+    //float angle
+    printf("turn: %d degrees\r\n", commandValue);
     getCommand();
     return;
 }
 
-void DumpTruck::moveBed(bool raise, float angle) {
+void DumpTruck::moveBed() {
+    //bool upper, bool lower, float angle
+    printf("Move bed to angle: %d degrees \r\n", commandValue);
     getCommand();
     return;
 }
 
 void DumpTruck::stopDrive() {
+    printf("Stop drive \r\n");
     frontMotor->write(0);
     getCommand();
 }
 
 void DumpTruck::stopTurn() {
+    printf("Stop turn \r\n");
     turnMotor->write(0);
     getCommand();
 }
 
 void DumpTruck::stopBed() {
+    printf("Stop bed \r\n");
     bedMotor->write(0);
     getCommand();
 }
@@ -142,235 +213,4 @@
         tooClose = false;
     }
     return tooClose;
-}
-
-
-//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
-     // placeholder
-    // speed = 0.5;
-    //distance = 5.0;
-    distance = setTruckDistance();
-    speed = setTruckSpeed();
-    // terminal confirmation of distance and 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
-    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;
-    DrivePin = speed;
-    } // else, go backwards at specified speed
-    else {
-    DirPin = 0;
-    DrivePin = -1*speed;
-         }
-    }
-    //pc.printf("Check 3:\r\n");
-    // stop motors
-    // stop();
-    // calibrate motors
-    //Calibrate();
-}
-
-
-
-
-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;
-
-    char c;
-    char buffer[128];
-    pc.gets(buffer, 4);
-    float input = strtod(buffer,NULL);
-    return input;
-
-    }
-
-float DumpTruck::setTruckSpeed(){
-//      float mod = 0;
-//      float div = 10;
-//      pc.printf("Enter a direction - 1 = forward, 0 = backward:\r\n");
-//      float dir = pc.getc();
-//      while ((dir<48)||(dir>49)){
-//          pc.printf("Invalid input. Please enter a valid direction, 1 or 0: \r\n");
-//          dir = pc.getc();
-//          }
-//      if (dir == 48) {
-//          mod = -1; }
-//          else {
-//          mod = 1; }
-//
-//      // terminal prompt for speed
-//      pc.printf("Please enter a truck speed from 0-9 or F for full speed:\r\n");
-//      // sets return value to given value
-//      float input = pc.getc();
-//      while ((input<48)||(input>57)){
-//          if((input == 70) || (input == 102)) {
-//              input = 1;
-//              input = input * mod;
-//              return input;
-//        }
-//          pc.printf("Invalid input. Please enter a valid speed from 0-9 or F: \r\n");
-//          input = pc.getc();
-//          }
-//      input = input - 48;
-//      input = input * (1/div);
-//      // returns value
-//      input = input * mod;
-
-
-    pc.printf("Enter a speed from -10 to +10:\r\n");
-    char c;
-    char buffer[128];
-    pc.gets(buffer, 4);
-    float input = strtod(buffer,NULL);
-
-    while ((buffer[0] < 43) || (buffer[0] > 45)) {
-        pc.printf("Invalid input. Please correct input and try again:\r\n");
-        setTruckSpeed();
-    }
-
-    pc.printf("buffer[0]: %d\r\n",buffer[0]);
-    pc.printf("buffer[1]: %d\r\n",buffer[1]);
-    pc.printf("buffer[2]: %d\r\n",buffer[2]);
-    input = input / 10;
-
-    return input;}
-
-
-void DumpTruck::stop(){
-    pc.printf("Truck will now stop.\r\n");
-    //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: ");
-//    DesiredPivotAngle = pc.getc();
-//    float CurrentPivotAngle =  getPivotAngle();
-//    Difference = DesiredPivotAngle-CurrentPivotAngle;
-//    if (Difference > limit)
-//    {
-//        PivotSpeed = setPivotSpeed();
-//        pc.printf("Rotating to %s /n", DesiredPivotAngle);
-//        //Drive Motors
-//        }
-//    else
-//    {stop()}
-//
-//}
-//
-//void DumpTruck::BedDown() {
-//     SwitchState = bool GetBedUpperSwitch();
-//     if SwitchState
-//     {return 0;}
-//     else
-//
-//     while(SwitchState == 0)
-//     {
-//         //driveMotor
-//         }
-//         calibrate();
-//    }
-//void DumpTruck::LowerBed() {
-//    SwitchState = bool GetLowerUpperSwitch();
-//     if SwitchState
-//     {return 0;}
-//     else
-//
-//     while(SwitchState == 0)
-//     {
-//         //driveMotor
-//         }
-//         calibrate();
-//    }
-//}
-//void DumpTruck::raiseTo(int angle) {
-//      pc.printf("Desired Angle: ");
-//    DesiredBedAngle = pc.getc();
-//    float CurrentBedAngle =  getBedAngle();
-//    Difference = DesiredBedAngle-CurrentBedAngle;
-//    if (Difference > limit)
-//    {
-//        BedSpeed = setBedSpeed();
-//        pc.printf("Rotating to %s /n", DesiredBedAngle);
-//        //Drive Motors
-//        }
-//    else
-//    {stop()}
-//}
-//
-//float DumpTruck::getPivotAngle(){
-//    float input = AnalogIn(x);
-//    return input
-//    }
-//float DumpTruck::setPivotSpeed(){
-//     pc.printf("Please enter the Pivot speed /n");
-//    float input = pc.getc();
-//    return input;
-//    }
-//
-//float DumpTruck::setBedSpeed(){
-//    pc.printf("Please enter the Bed speed /n");
-//    float input = pc.getc();
-//    return input;
-//    }
-//float DumpTruck::getBedAngle(){
-//    float input = AnalogIn(x); // data from potentiometer
-//    return input;
-//    }
-//
\ No newline at end of file
+}
\ No newline at end of file