File content as of revision 3:cd6e2d7c7c9c:

#include "DumpTruck.h"
#include "stdlib.h"
#include "QEI.h"


Serial pc(USBTX, USBRX);

DumpTruck::DumpTruck(int truckId) : truckNumber(truckId) {
    truckNumber = truckId;
}

QEI wheel (p15, p16, NC, 128, QEI::X4_ENCODING); //orange, yellow
float pulseCount; //number of pulses detected by encoder
float encoding = 2; //X2
float d = 13.25; //inches of circumference
float distance;
PwmOut DrivePin(p24);
DigitalOut DirPin(p30);
float travelled;


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

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