Activities

File content as of revision 15:a4bbdde8ed69:

#include "DumpTruck.h"
#include <iostream>
#include <string>

DumpTruck::DumpTruck(int truckId) : truckNumber(truckId) {
    truckNumber = truckId;
    //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(p24, p30);
    pot = new AnalogIn(p19);
    //bed = new IMU(@@@@@@@);
    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
    txDataCnt = 0;
    rxDataCnt = 0;
    received = false;
    numCommands = sizeof(commands)/sizeof(*commands);
    
    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
    //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");
    printf("ID: %d \r\n", truckNumber);
    printf("--------\r\n");
    nrf->setTransferSize( TRANSFER_SIZE );
    nrf->setReceiveMode();
    nrf->enable();
    
    // fill acked array
    for(int i = 0; i < TRANSFER_SIZE; i++) {
        acked[i] = '0';
        nacked[i] = '1';
    }
}

void DumpTruck::sendAck() {
    wait(1);
    nrf->write(NRF24L01P_PIPE_P0, acked, TRANSFER_SIZE);
    printf("ACK SENT\n\r");
}

void DumpTruck::sendNack() {
    wait(1);
    nrf->write(NRF24L01P_PIPE_P0, nacked, TRANSFER_SIZE);
    printf("NACK SENT\n\r");
}

void DumpTruck::getCommand() {
    //called at the end of each executable command
    //dump truck transceiver sends a code to base station, opening the channel
    //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()) {
        // ...read the data into the receive buffer
            rxDataCnt = nrf->read(NRF24L01P_PIPE_P0, rxData, sizeof(rxData));
            received = true; //after receiving command
            printf("received command\n\r");
            processCommand();
        }
    }
}

void DumpTruck::processCommand() {
    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
    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();
            sendAck();
            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) {
    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();
}

void DumpTruck::driveDistance() {
    //float speed, float distance
    printf("Drive distance: %d \r\n", commandValue);
    speed = (float) commandValue/10;
    frontMotor->write(speed);
    getCommand();
}

void DumpTruck::drive() {
    //commandValue = speed etc
    //float speed
    speed = (float) commandValue/10;
    printf("Drive speed: %d \r\n", commandValue);
    frontMotor->write(speed);
    getCommand();
}

void DumpTruck::turn() {
    //commandValue = angle etc
    //float angle
    printf("turn: %d degrees\r\n", commandValue);
    turnMotor->write(0.6f);
    wait(1);
    turnMotor->write(0);
    getCommand();
    return;
}

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

void DumpTruck::stopDrive() {
    printf("Stop drive \r\n");
    frontMotor->write(0);
    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);
    getCommand();
}

void DumpTruck::stopBed() {
    printf("Stop bed \r\n");
    bedMotor->write(0);
    getCommand();
}

bool DumpTruck::detect() {
    proximity = srf->read();
    if(proximity < 10) {       //cm
        tooClose = true;
       // detectStop();
    } else {
        tooClose = false;
    }
    return tooClose;
}