File content as of revision 10:cf77da9be0b8:

#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(p21, p27);
    turnMotor = new Motor(p22, p28);
    bedMotor = new Motor(p23, p29);
    //bed = new IMU(@@@@@@@);
    //nrf = new Transceiver(@@@@@@);
    srf = new SRF05(p15, p16);
    tooClose = false;
}

void DumpTruck::driveDistance(float speed, float distance) {
    frontMotor->write(speed);
}

void DumpTruck::drive(float speed) {
    frontMotor->write(speed);
}

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

void DumpTruck::moveBed(bool raise, float angle) {
    return;
}

void DumpTruck::stop() {
    frontMotor->write(0.0f);
}

bool DumpTruck::detect() {
    proximity = srf->read();
    if(proximity < 100) {       //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;
//    }
//