Charlie's Robot Code Unrevised

Blue_Robot_Code.ino

#include <NewPing.h>

#include <hcsr04.h>

#include <Servo.h>

Servo leftDrive, rightDrive, intakeServo;

//HCSR04 hcsr04(11, 12, 20, 2000);
//HCSR04 sideUltrasonic(8, 9, 20, 2000);


NewPing sideUltrasonic(8, 9, 60);
NewPing hcsr04(11, 12, 60);



void setup() {
  leftDrive.attach(10);
  rightDrive.attach(11);

  intakeServo.attach(5);

  pinMode(3, INPUT); //user input button

  pinMode(13, OUTPUT); //status LED

  pinMode(7, OUTPUT);
  pinMode(6, OUTPUT);

  digitalWrite(13, HIGH);
  driveStop();

  Serial.begin(9600); //debugging only!!!

}


//dead reckoning only
void loop() {

 //driveForwardSideBreak(32, 55);
//    while(1){
//      Serial.println(sideUltrasonic.convert_cm(sideUltrasonic.ping_median(3)));
//      delay(100);
//    }

  long functionStartTime = -1; //set local function start time

  
  driveStop();
  
//  functionStartTime = millis();
//  do {
//    //delay for three seconds at start
//  } while (timeBreak(functionStartTime, 3000));
//
//  driveForwardSideBreak(39, 43); //250 400
//  functionStartTime = millis();
//  do {
//    driveStop();
//  } while (timeBreak(functionStartTime, 1000));
//  driveForwardSideBreak(2, 10);
//  functionStartTime = millis();
//  do {
//    driveStop();
//  } while (timeBreak(functionStartTime, 1000));
//  driveForwardSideBreak(39, 43);
//
//  functionStartTime = millis();
//  do {
//    driveStop();
//  } while (timeBreak(functionStartTime, 1000));
//
//  while(1){} //debug
  //  functionStartTime = millis();
  //  do {
  //    driveTurnCoeff(1.0, 0, 1); //turn right into corridor
  //  } while (timeBreak(functionStartTime, 1750));
  //  functionStartTime = millis();
  //  do{
  //    driveStop(); //back away briefly into wall
  //  }while(timeBreak(functionStartTime, 5000));

  functionStartTime = millis();
  do {
    startCollection(0.8); //fire up collection at 80% power
  } while (timeBreak(functionStartTime, 1000));

  //  functionStartTime = millis();
  //  do{
  //    driveStraight(-1.0); //drive back to get first power up
  //  }while(timeBreak(functionStartTime, 0)); //2000
  //
  //  functionStartTime = millis();
  //  do{
  //    driveStop(); //stop at wall
  //  }while(timeBreak(functionStartTime, 2000));

  functionStartTime = millis();
  do {
    driveStraight(1.0); //drive forward towards wall
  } while (timeBreak(functionStartTime, 5000)); //12000 //10500

  functionStartTime = millis();
  do {
    driveStop(); //stop at wall
  } while (timeBreak(functionStartTime, 500));

  //START FIRST TURN

  functionStartTime = millis();
  do {
    driveStraight(-1.0); //back away briefly from wall
  } while (timeBreak(functionStartTime, 250)); //400
  //2500 for 360 point turn right
  //2400 for 270 -0.1, 1.0 turn right

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));

  functionStartTime = millis();
  do {
    driveTurnCoeff(1.0, -0.1, 1); //turn right into corridor
  } while (timeBreak(functionStartTime, 850));


  functionStartTime = millis();
  do {
    driveStraight(-1.0); //back away briefly into wall
  } while (timeBreak(functionStartTime, 1500));

  wobbleBack();

  functionStartTime = millis();
  do {
    driveStraight(1.0); //drive forward towards wall
  } while (timeBreak(functionStartTime, 2000)); //2500

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));

  //START SECOND TURN

  functionStartTime = millis();
  do {
    driveStraight(-1.0); //back away briefly from wall
  } while (timeBreak(functionStartTime, 300)); //400
  //2500 for 360 point turn right
  //2400 for 270 -0.1, 1.0 turn right

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));

  functionStartTime = millis();
  do {
    driveTurnCoeff(1.0, 1, -0.1); //turn left into corridor
  } while (timeBreak(functionStartTime, 825)); //850 before but now more for offset 900! 850


  functionStartTime = millis();
  do {
    driveStraight(-1.0); //back away briefly into wall
  } while (timeBreak(functionStartTime, 1500));

  wobbleBack();

  functionStartTime = millis();
  do {
    driveStraightOffset(1.0); //drive forward towards wall
  } while (timeBreak(functionStartTime, 3250)); //2500

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));

  //START THIRD TURN

  functionStartTime = millis();
  do {
    driveStraight(-1.0); //back away briefly from wall
  } while (timeBreak(functionStartTime, 200)); //250
  //2500 for 360 point turn right
  //2400 for 270 -0.1, 1.0 turn right

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));

  functionStartTime = millis();
  do {
    driveTurnCoeff(1.0, -1, 1); //turn right into corridor
  } while (timeBreak(functionStartTime, 675)); //700


  functionStartTime = millis();
  do {
    driveStraight(-1.0); //back away into second wall, get second power up
  } while (timeBreak(functionStartTime, 2500));

  wobbleBack();

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));

  //START MOVE FORWARD TO SECOND SIDE OF FIELD

  //driveForwardDistance(120); //1905 1700 1550 1750 1650-1715 1685! 1400 new

  driveForwardSideBreak(39, 43); //250 400 //32 55
  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));
  driveForwardSideBreak(2, 10);
  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));
  driveForwardSideBreak(39, 43);

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));

  //while(1){} //debug

  //AFTER SENSOR STOP BEFORE 3RD POWER UP

  //START FOURTH TURN

    //EXTRA FORWARD BUFFER FOR ULTRASONIC
    functionStartTime = millis();
    do{
      driveStraight(1.0); //go forward briefly to turn
    }while(timeBreak(functionStartTime, 300)); //150 300
  //  //2500 for 360 point turn right
  //  //2400 for 270 -0.1, 1.0 turn right

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));

  functionStartTime = millis();
  do {
    driveTurnCoeff(1.0, -0.1, 1); //turn right into corridor
  } while (timeBreak(functionStartTime, 850));


  functionStartTime = millis();
  do {
    driveStraight(-1.0); //back away briefly into wall
  } while (timeBreak(functionStartTime, 1500));

  wobbleBack();

  functionStartTime = millis();
  do {
    driveStraightOffset(1.0); //drive forward towards wall
  } while (timeBreak(functionStartTime, 3500)); //2500  USED TO BE REG STRAIGHT

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));

  //START FIFTH TURN

  functionStartTime = millis();
  do {
    driveStraight(-1.0); //back away briefly from wall
  } while (timeBreak(functionStartTime, 300)); //400
  //2500 for 360 point turn right
  //2400 for 270 -0.1, 1.0 turn right

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));

  functionStartTime = millis();
  do {
    driveTurnCoeff(1.0, 1, -0.1); //turn left into corridor
  } while (timeBreak(functionStartTime, 850));


  functionStartTime = millis();
  do {
    driveStraight(-1.0); //back away briefly into wall
  } while (timeBreak(functionStartTime, 1500));

  wobbleBack();

  functionStartTime = millis();
  do {
    driveStraight(1.0); //drive forward towards wall
  } while (timeBreak(functionStartTime, 2500));

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));


  //END OF FIRST DAY

  //START TURN SECOND CORRIDOR

  functionStartTime = millis();
  do {
    driveStraight(-1.0); //back away briefly from wall
  } while (timeBreak(functionStartTime, 250)); //400
  //2500 for 360 point turn right
  //2400 for 270 -0.1, 1.0 turn right

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));

  functionStartTime = millis();
  do {
    driveTurnCoeff(1.0, -0.1, 1); //turn right into corridor
  } while (timeBreak(functionStartTime, 850));


  functionStartTime = millis();
  do {
    driveStraight(-1.0); //back away briefly into wall
  } while (timeBreak(functionStartTime, 1500));

  wobbleBack();

  functionStartTime = millis();
  do {
    driveStraight(1.0); //drive forward towards wall
  } while (timeBreak(functionStartTime, 1500)); //2500

  driveForwardSideBreak(10, 1000);

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));

  driveForwardSideBreak(2, 10);

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));

  functionStartTime = millis();
  do {
    driveStraight(1.0); //drive forward towards wall
  } while (timeBreak(functionStartTime, 1000)); //2500

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));

  driveBackSideBreak(10, 1000);

  functionStartTime = millis();
  do {
    driveStop();
  } while (timeBreak(functionStartTime, 1000));




  //END OF PROGRAM

  functionStartTime = millis();
  do {
    driveStop();
    //reached end, stop program
  } while (timeBreak(millis(), 10000000));
}


//special first level functions
void wobbleBack() {
  long functionStartTime = millis();
  functionStartTime = millis();
  do {
    driveTurnCoeff(1.0, 0.1, -1); //turn right into corridor
  } while (timeBreak(functionStartTime, 300));
  functionStartTime = millis();
  do {
    driveTurnCoeff(1.0, -1, 0.1); //turn right into corridor
  } while (timeBreak(functionStartTime, 300));
  functionStartTime = millis();
  do {
    driveTurnCoeff(1.0, 0.1, -1); //turn right into corridor
  } while (timeBreak(functionStartTime, 300));
  functionStartTime = millis();
  do {
    driveTurnCoeff(1.0, -1, 0.1); //turn right into corridor
  } while (timeBreak(functionStartTime, 300));
  functionStartTime = millis();
  do {
    driveStraight(-1.0); //back away briefly from wall
  } while (timeBreak(functionStartTime, 600));
}

void driveForwardDistance(int mm) {
  int last1 = -1;
  int last2 = -1;
  //int last3 = -1;
  while (last1 < mm || last2 < mm) {
    driveStraight(1.0);
    last1 = hcsr04.convert_cm(hcsr04.ping_median(3));
    last2 = last1;
    //last3 = last2;
  }
  driveStop();
  hcsr04.timer_stop();
}

void driveForwardSideBreak(int minThresh, int maxThresh) {
  int last1 = -1;
  int last2 = -1;
  //int last3 = -1;
  while ((last1 > maxThresh || last1 < minThresh) || (last2 > maxThresh || last2 < minThresh)) {
    //leftDrive.write(180);
    driveStraight(1.0);
    last1 = sideUltrasonic.convert_cm(sideUltrasonic.ping_median(3));
    last2 = last1;
    //last3 = last2;
  }
  driveStop();
  sideUltrasonic.timer_stop();
}

void driveBackSideBreak(int minThresh, int maxThresh) {
  int last1 = -1;
  int last2 = -1;
  //int last3 = -1;
  while ((last1 > maxThresh || last1 < minThresh) || (last2 > maxThresh || last2 < minThresh)) {
    //leftDrive.write(180);
    driveStraight(-1.0);
    last1 = sideUltrasonic.convert_cm(sideUltrasonic.ping_median(3));
    last2 = last1;
    //last3 = last2;
  }
  driveStop();
  sideUltrasonic.timer_stop();
}

//runner functions
boolean timeBreak(long start, long duration) {
  if (millis() - start > duration) {
    return false; //reached duration
  }
  return true; // not currently reached yet
}
boolean sensorBreak(int sensorID) {
  return true; //TODO: code later!!!!
}


//intake actions
void startCollection(double speed) { //speed is a double 0.0 - 1.0
  intakeServo.write(90 + (speed * 90));
}

void stopCollection() { //sets speed of collection to stop
  intakeServo.write(90);
}

void reverseCollection(double speed) { //speed is a double 0.0 - 1.0
  intakeServo.write(90 - (speed * 90));
}


//drive actions
void driveStraight(double power) { //simple dead reckoning
  leftDrive.write(90 + (power * 90)); //85 start value
  rightDrive.write(90 - (power * 90));
}

void driveStraightOffset(double power) { //simple dead reckoning
  leftDrive.write(110); //85 start value //130
  rightDrive.write(0);
  digitalWrite(7, HIGH);
  digitalWrite(6, LOW);
}

void driveTurnCoeff(double power, double leftCoeff, double rightCoeff) { //turn using coefficient - positive is right turn, negative is left turn
  leftDrive.write(90 + (power * 90 * (0 + leftCoeff)));
  rightDrive.write(90 - (power * 90 * (0 + rightCoeff)));
  digitalWrite(6, HIGH);
  digitalWrite(7, LOW);
}

void driveTurnOffset() {

}

void driveTurnPoint() {

}

void driveStop() {
  leftDrive.write(90);
  rightDrive.write(90);
}