[will_1359]

class LineTracker {

RobotDrive drive; // robot drive base object
DigitalInput left; // digital inputs for line tracking sensors
DigitalInput middle;
DigitalInput right;
DriverStation ds; // driver station object for getting selections
double defaultSteeringGain = 0.65; // the default value for the steering gain

public LineTracker(RobotDrive d) {
// create the robot drive and correct for the wheel direction.
//drive = new RobotDrive(1, 2); because having two of these screws everything up
drive = d;


drive.setInvertedMotor(RobotDrive.MotorType.kRearL eft, false);
//drive.setInvertedMotor(RobotDrive.MotorType.kFront Left, true);
//drive.setInvertedMotor(RobotDrive.MotorType.kFront Right, true);
//we have only rear motors
drive.setInvertedMotor(RobotDrive.MotorType.kRearR ight, false);

// set the MotorSafety expiration timer
drive.setExpiration(15);

// create the digital input objects to read from the sensors
left = new DigitalInput(1);
middle = new DigitalInput(2);
right = new DigitalInput(3);

// get the driver station instance to read the digital I/O pins
ds = DriverStation.getInstance();
}

/**
* This function is called once each time the robot enters autonomous mode.
*/
public void run() {
int binaryValue; // a single binary value of the three line tracking
// sensors
int previousValue = 0; // the binary value from the previous loop
double steeringGain; // the amount of steering correction to apply

// the power profiles for the straight and forked robot path. They are
// different to let the robot drive more slowly as the robot approaches
// the fork on the forked line case.


double forkProfile[] = {0.70, 0.70, 0.55, 0.60, 0.60, 0.50, 0.40, 0.00};
double straightProfile[] = {0.7, 0.7, 0.6, 0.6, 0.35, 0.35, 0.35, 0.0};
double powerProfile[]; // the selected power profile

// set the straightLine and left-right variables depending on chosen path
boolean straightLine = ds.getDigitalIn(1);
powerProfile = (straightLine) ? straightProfile : forkProfile;
double stopTime = (straightLine) ? 2.0 : 4.0; // when the robot should look for end
boolean goLeft = !ds.getDigitalIn(2) && !straightLine;
System.out.println("StraightLine: " + straightLine);
System.out.println("GoingLeft: " + goLeft);


boolean atCross = false; // if robot has arrived at end

// time the path over the line
Timer timer = new Timer();
timer.start();
timer.reset();

//int oldTimeInSeconds = -1;
double time;
double speed, turn;

// loop until robot reaches "T" at end or 8 seconds has past
while ((time = timer.get()) < 8.0 && !atCross) {
int timeInSeconds = (int) time;
// read the sensors
int leftValue = left.get() ? 1 : 0;
int middleValue = middle.get() ? 1 : 0;
int rightValue = right.get() ? 1 : 0;
// compute the single value from the 3 sensors. Notice that the bits
// for the outside sensors are flipped depending on left or right
// fork. Also the sign of the steering direction is different for left/right.
if (goLeft) {
binaryValue = leftValue * 4 + middleValue * 2 + rightValue;
steeringGain = -defaultSteeringGain;
} else {
binaryValue = rightValue * 4 + middleValue * 2 + leftValue;
steeringGain = defaultSteeringGain;
}

// get the default speed and turn rate at this time
speed = powerProfile[timeInSeconds];
turn = 0;

// different cases for different line tracking sensor readings
switch (binaryValue) {
case 1: // on line edge
turn = 0;
break;
case 7: // all sensors on (maybe at cross)
if (time > stopTime) {
atCross = true;
speed = 0;
}
break;
case 0: // all sensors off
if (previousValue == 0 || previousValue == 1) {
turn = steeringGain;
} else {
turn = -steeringGain;
}
break;
default: // all other cases
turn = -steeringGain;
}
// print current status for debugging
if (binaryValue != previousValue) {
System.out.println("Time: " + time + " Sensor: " + binaryValue + " speed: " + speed + " turn: " + turn + " atCross: " + atCross);
}

// set the robot speed and direction
drive.arcadeDrive(speed, turn);

if (binaryValue != 0) {
previousValue = binaryValue;
}
//oldTimeInSeconds = timeInSeconds;

Timer.delay(0.01);
}
// Done with loop - stop the robot. Robot ought to be at the end of the line.
drive.arcadeDrive(0, 0);
}
}