File content as of revision 14:192e103d5246:

#include "DumpTruck.h"


DumpTruck::DumpTruck(int truckId) : truckNumber(truckId) {
    truckNumber = truckId;
    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);
    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() {
    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() );
    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() {
    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() {
    //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() {
    //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;
    //}
    getCommand();
}

void DumpTruck::reportData() {
    //should run last in main thread
}

void DumpTruck::driveDistance(float speed, float distance) {
    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/10);
    getCommand();
}

void DumpTruck::turn(float angle) {
    getCommand();
    return;
}

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

void DumpTruck::stopTurn() {
    turnMotor->write(0);
    getCommand();
}

void DumpTruck::stopBed() {
    bedMotor->write(0);
    getCommand();
}

bool DumpTruck::detect() {
    proximity = srf->read();
    if(proximity < 20) {       //cm
        tooClose = true;
    } else {
        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;
//    }
//