#include "Autonomous.h"

#include "Sensors.h"

#include "BTDrive.h"

BTAutonomous::BTAutonomous()

{

selector = new AnalogChannel(PORTS::AUTONOMOUS_SELECTOR);

enabled = true;

previousInches = 1000;

position = 1;

firstTime = true;

secondTime = true;

startTime = 0;

wall = false;

counter = 0;

left = false;

}

void BTAutonomous::Switch(void)

{

const int POSITION_1 = 50;

const int POSITION_2 = 150;

const int POSITION_3 = 400;

const int POSITION_4 = 600;

const int POSITION_5 = 850;

if (selector->GetValue() < POSITION_1)

position = 1;

if ((selector->GetValue() > POSITION_1)&&(selector->GetValue() < POSITION_2))

position = 2;

if ((selector->GetValue() > POSITION_2)&&(selector->GetValue() < POSITION_3))

position = 3;

if ((selector->GetValue() > POSITION_3)&&(selector->GetValue() < POSITION_4))

position = 4;

if ((selector->GetValue() > POSITION_4)&&(selector->GetValue() < POSITION_5))

position = 5;

if (selector->GetValue() > POSITION_5)

position = 6;

// printf("selector = %d\n", selector->GetValue());

}

void BTAutonomous::RunAutonomous(BTDrive *driveTrain, Sensors *sensors, ControlBoard *controlBoard, BTShooter *cannon)

{

if (position == 1)

Straight(driveTrain, sensors, controlBoard, cannon);

if (position == 2)

Right90(driveTrain, sensors, controlBoard, cannon);

if (position == 3)

Left90(driveTrain, sensors, controlBoard, cannon);

if (position == 4)

Right45(driveTrain, sensors, controlBoard, cannon);

if (position == 5)

Left45(driveTrain, sensors, controlBoard, cannon);

if (position == 6)

Powerslide(driveTrain, sensors, controlBoard, cannon);

/*if (controlBoard->GetDigitalIn(ControlBoard::AUTONOMOUS_SWITCH))

{

Straight(driveTrain, sensors, controlBoard, cannon);

}

else

{

Powerslide(driveTrain, sensors, controlBoard, cannon);

}*/

/*

cannon->arbitrator->shoot = 1;

if (cannon->turret->Update())

{

cannon->Fire(position);

}*/

if (!cannon->turret->centered)

cannon->turret->Center();

driveTrain->DoDrive(6, true);

};

void BTAutonomous::Left90(BTDrive *driveTrain, Sensors *sensors, ControlBoard *controlBoard, BTShooter *cannon)

{

sensors->GetSensors();

if (sensors->leftInches > 6)

{

// printf("Left\n");

driveTrain->targetLeftSpeed = -0.25;

driveTrain->targetRightSpeed = -1.00;

}

else

{

// printf("Right\n");

driveTrain->targetLeftSpeed = -1.00;

driveTrain->targetRightSpeed = -0.25;

}

printf("left speed = %f, right speed = %f\n", driveTrain->targetLeftSpeed, driveTrain->targetRightSpeed);

};

void BTAutonomous::Right90(BTDrive *driveTrain, Sensors *sensors, ControlBoard *controlBoard, BTShooter *cannon)

{

sensors->GetSensors();

if (sensors->rightInches > 6)

{

driveTrain->targetLeftSpeed = -1.00;

driveTrain->targetRightSpeed = -0.25;

}

else

{

driveTrain->targetLeftSpeed = -0.25;

driveTrain->targetRightSpeed = -1.00;

}

};

void BTAutonomous::Straight(BTDrive *driveTrain, Sensors *sensors, ControlBoard *controlBoard, BTShooter *cannon)

{

if (firstTime)

{

startTime = GetClock();

firstTime = false;

}

if (GetClock() - startTime < 5)

{

driveTrain->targetLeftSpeed = -1.00;

driveTrain->targetRightSpeed = -1.00;

}

else

{

driveTrain->targetLeftSpeed = 0.00;

driveTrain->targetRightSpeed = 0.00;

}

};

void BTAutonomous::Left45(BTDrive *driveTrain, Sensors *sensors, ControlBoard *controlBoard, BTShooter *cannon)

{

sensors->GetSensors();

if ((sensors->leftInches > 24)&&(wall == true))

enabled = false;

if (enabled == true)

{

if (sensors->leftInches > 6)

{

driveTrain->targetLeftSpeed = -0.50;

driveTrain->targetRightSpeed = -1.00;

}

else

{

counter++;

driveTrain->targetLeftSpeed = -1.00;

driveTrain->targetRightSpeed = -0.50;

if (counter > 3)

wall = true;

}

}

else

{

driveTrain->targetLeftSpeed = 0;

driveTrain->targetRightSpeed = 0;

}

};

void BTAutonomous::Right45(BTDrive *driveTrain, Sensors *sensors, ControlBoard *controlBoard, BTShooter *cannon)

{

sensors->GetSensors();

if ((sensors->rightInches > 24)&&(wall == true))

enabled = false;

if (enabled == true)

{

if (sensors->rightInches > 6)

{

driveTrain->targetLeftSpeed = -1.00;

driveTrain->targetRightSpeed = -0.50;

}

else

{

counter++;

driveTrain->targetLeftSpeed = -0.50;

driveTrain->targetRightSpeed = -1.00;

if (counter > 3)

wall = true;

}

}

else

{

driveTrain->targetLeftSpeed = 0;

driveTrain->targetRightSpeed = 0;

}

};

void BTAutonomous::Powerslide(BTDrive *driveTrain, Sensors *sensors, ControlBoard *controlBoard, BTShooter *cannon)

{

int temp = (int)GetClock();

if (firstTime)

{

startTime = GetClock();

firstTime = false;

}

if (GetClock() - startTime < 5)

{

driveTrain->targetLeftSpeed = -0.75;

driveTrain->targetRightSpeed = -0.75;

}

else

{

if ((temp%2 == 0)&&(secondTime))

{

left = true;

}

secondTime = false;

if (left == true)

{

driveTrain->targetLeftSpeed = -0.10;

driveTrain->targetRightSpeed = -1.00;

}

else

{

driveTrain->targetLeftSpeed = -1.00;

driveTrain->targetRightSpeed = -0.10;

}

}

printf("left = %d\n", left);

};