void Autonomous()
{
compressor.Start();//start compressor
myRobot.SetSafetyEnabled(false);
myRobot.Drive(-0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot.Drive(0.0, 0.0); // stop robot
while (!shoot.Get()){//while shooter isnt cocked pull it back
shooter.SetSpeed(1); //set motor
}
shooterSole.Set(shooterSole.kForward);//Sets Solenoid forward, shoot ball
shooterSole.Set(shooterSole.kReverse);//Sets Solenoid backward
myRobot.Drive(1.0, -1.0);//turn around for 0.5 seconds, we have to check to see if it takes that long
Wait(0.5);//wait for 0.5 seconds
myRobot.Drive(0.0, 0.0);//stop robot
}
void MotorControlRight(float speed)
{
right_1->SetSpeed(speed);
right_2->SetSpeed(speed);
}
/********************************* Teleop methods *****************************************/
void MotorControlLeft(float speed)
{
left_1->SetSpeed(speed);
left_2->SetSpeed(speed);
}
void OperatorControl(void)
{
float counter;
timer.Start();
float percent;
deadband = .05;
float pi = 3.141592653589793238462643;
float bpotval, fpotval;
float min = 600, max;
float fchgval = .5;
float bchgval = .5;
while (IsOperatorControl())
{
// comp.checkCompressor();
ShootModeSet();
Shoot(true);
fpotval = fpot.GetValue();
bpotval = bpot.GetValue();
counter = timer.Get();
if (controller.GetRawButton(3))
{
frot.SetSpeed(0);
brot.SetSpeed(0);
flmot.SetSpeed(0);
frmot.SetSpeed(0);
blmot.SetSpeed(0);
brmot.SetSpeed(0);
}
else if (controller.GetRawButton(BTN_L1) == false)
{
if (controller.GetRawButton(7))
{
if (fpotval < 860)
frot.SetSpeed(-0.5);
if (bpotval < 725)
brot.SetSpeed(-0.5);
if (fpotval > 860 and bpotval > 725)
{
frot.Set(0);
brot.Set(0);
flmot.Set(0.5);
frmot.Set(0.5);
blmot.Set(0.5);
brmot.Set(0.5);
}
}
else if (controller.GetRawButton(8))
{
if (fpotval < 860)
frot.SetSpeed(-0.5);
if (bpotval < 725)
brot.SetSpeed(-0.5);
if (fpotval > 860 and bpotval > 725)
{
frot.Set(0);
brot.Set(0);
flmot.Set(-0.5);
frmot.Set(-0.5);
blmot.Set(-0.5);
brmot.Set(-0.5);
}
}
else
{
if (controller.GetRawAxis(4) <= 0)
percent = ((acos(controller.GetRawAxis(3)) / pi));
else if (controller.GetRawAxis(4) > 0)
percent = ((acos(-controller.GetRawAxis(3)) / pi));
fpotval = percent * 550 + 330;
percent = (fpotval - 330) / 550;
bpotval = percent * 500 + 245;
if (fpot.GetValue() < fpotval)
fchgval = -.5;
else if (fpot.GetValue() > fpotval)
fchgval = .5;
if (fpot.GetValue() < fpotval + 10 && fpot.GetValue() > fpotval - 10)
fchgval = 0;
if (bpot.GetValue() > bpotval)
bchgval = -.5;
else if (bpot.GetValue() < bpotval)
bchgval = .5;
if (bpot.GetValue() < bpotval + 20 && bpot.GetValue() > bpotval - 20)
bchgval = 0;
frot.Set(fchgval);
brot.Set(bchgval);
if (pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2) > deadband && controller.GetRawButton(BTN_R1))
{
if (controller.GetRawAxis(4) > 0)
{
flmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
frmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
blmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
brmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
}
else if (controller.GetRawAxis(4) < 0)
{
flmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
frmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
blmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
brmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
}
}
else
{
flmot.Set(0);
frmot.Set(0);
blmot.Set(0);
brmot.Set(0);
}
contaxis4 = 0;
}
}
else
{
if (contaxis4 == 0)
contaxis4 = controller.GetRawAxis(4);
if (controller.GetRawButton(BTN_R1))
{
if (contaxis4 > 0)
{
flmot.Set(-0.25);
frmot.Set(0.25);
blmot.Set(-0.25);
brmot.Set(0.25);
}
else if (contaxis4 < 0)
{
flmot.Set(0.25);
frmot.Set(-0.25);
blmot.Set(0.25);
brmot.Set(-0.25);
}
}
}
if (float (fpot.GetValue()) < min)
min = float (fpot.GetValue());
else if (float (fpot.GetValue()) > max)
max = float (fpot.GetValue());
if (counter >= .1)
{
lcda->Clear();
lcda->Printf(DriverStationLCD::kUser_Line3, 1, "FPot :: %d", fpotval);
lcda->Printf(DriverStationLCD::kUser_Line4, 1, "BPot :: %d", bpotval);
lcda->UpdateLCD();
timer.Reset();
}
}
}
void OperatorControl()
{
compressor.Start();
myRobot.SetSafetyEnabled(true);
myRobot.SetInvertedMotor(myRobot.kRearLeftMotor, true);//Invert rear left Motor
myRobot.SetInvertedMotor(myRobot.kRearRightMotor, true);//Invert rear right motor
myRobot.SetInvertedMotor(myRobot.kFrontLeftMotor, true);//Invert rear right motor
myRobot.SetInvertedMotor(myRobot.kFrontRightMotor, true);
DriverStation *ds;
DriverStationLCD *dsLCD;
ds = DriverStation::GetInstance();
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Printf(DriverStationLCD::kUser_Line1,1, "Starting Teleop");
dsLCD->UpdateLCD();
while (true)
{
if (!compressor.GetPressureSwitchValue()){
compressor.Start();
}
myRobot.ArcadeDrive(stick);
/*PNEUMATIC CODE*/
if (stick.GetRawButton(8)){
shooterSole.Set(shooterSole.kForward);
}
if (stick.GetRawButton(1)){
shooterSole.Set(shooterSole.kReverse);
}
/*SHOOTER CODE*/
if (stick.GetRawButton(2)){
shooter.SetSpeed(1);
}
if (stick.GetRawButton(4)){
shooter.SetSpeed(-1);
dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Shooter Should be negative");
dsLCD->UpdateLCD();
}
if (!stick.GetRawButton(4) || !stick.GetRawButton(2)){
shooter.SetSpeed(0);
}
/* FORKLIFT CODE*/
if (!stick.GetRawButton(5) || !stick.GetRawButton(6)){
forklift.SetSpeed(0);
}
if (stick.GetRawButton(5)){
forklift.SetSpeed(1);
}
if (stick.GetRawButton(6)){
forklift.SetSpeed(-1);
dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Forklift Should be negative");
dsLCD->UpdateLCD();
}
if (!shoot.Get()){
shooter.SetSpeed(0);
shooterSole.Set(shooterSole.kForward);
}
if (stick.GetRawButton(11)){
//myRobot.m_rearLeftMotor.SetSpeed(1);
//myRobot.m_rearRightMotor.SetSpeed(1);
//myRobot.m_frontLeftMotor.SetSpeed(1);
//myRobot.m_frontRightMotor.SetSpeed(1);
}
//Wait(0.005);// wait for a motor update time
}
}