void AutonomousType4() {
SmartDashboard::PutString("STATUS:", "STARTING AUTO 4");
//Lift, turn, drive
chainLift.SetSpeed(0.5);
while (midPoint.Get() && maxUp.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Polar(0, 0, 0.3);
if (WaitF(2.5))
return;
robotDrive.MecanumDrive_Polar(0.25, 0, 0);
if (WaitF(5.6))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0.3);
if (WaitF(2))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0);
//chainLift.SetSpeed(-0.4);
//while (maxDown.Get() && IsAutonomous()) {
//}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Polar(0, 0, 0);
SmartDashboard::PutString("STATUS:", "AUTO 4 COMPLETE");
}
//Grab first two and turn to go right
void AutonomousType10() {
SmartDashboard::PutString("STATUS:", "STARTING AUTO 10");
robotDrive.MecanumDrive_Cartesian(0, -0.2, 0);
if (WaitF(1.2))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0.5);
while (IsAutonomous() && maxUp.Get() && midPoint.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, 0.4, 0);
if (WaitF(1.6))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0.3);
if (WaitF(2.6))
return;
robotDrive.MecanumDrive_Cartesian(0, -0.4, 0);
if (WaitF(1))
return;
robotDrive.MecanumDrive_Polar(0, 0, -0.3);
if (WaitF(2.6))
return;
robotDrive.MecanumDrive_Cartesian(0, -0.4, 0);
if (WaitF(1.6))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
SmartDashboard::PutString("STATUS:", "AUTO 10 COMPLETE");
}
//Choose which auto to use
void AutonomousInit() {
chainLift.SetSpeed(0);
canGrabber.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
SmartDashboard::PutString("STATUS:", "STARTING AUTO");
robotDrive.SetSafetyEnabled(false);
chainLift.SetSafetyEnabled(false);
SmartDashboard::PutBoolean("Auto switch A: ", autoSwitch1.Get());
SmartDashboard::PutBoolean("Auto switch B: ", autoSwitch2.Get());
//Select auto type
if (autoSwitch1.Get()) {
if (autoSwitch2.Get())
AutonomousType4();
else
//1 on 2 grab n back
AutonomousType8();
} else {
if (autoSwitch2.Get())
//1 off, 2 on: grab n turn
AutonomousType10();
else {
SmartAutoPicker();
}
//Do Nothing
}
}
//Grab first yellow, back up to auto zone, DON'T DROP
void AutonomousType12() {
SmartDashboard::PutString("STATUS:", "STARTING AUTO 12");
chainLift.SetSpeed(0.5);
while (IsAutonomous() && IsEnabled() && maxUp.Get() && midPoint.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, 0.4, 0);
if (WaitF(3))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
SmartDashboard::PutString("STATUS:", "AUTO 12 COMPLETE");
}
//Steal cans
void AutonomousType13() {
SmartDashboard::PutString("STATUS:", "STARTING AUTO 13");
robotDrive.MecanumDrive_Cartesian(0, 0.2, 0);
if (WaitF(1.2))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
canGrabber.SetSpeed(1);
if (WaitF(4))
return;
canGrabber.SetSpeed(0);
LinearAcceleration(1, 0, 1, 0);
SmartDashboard::PutString("STATUS:", "AUTO 13 COMPLETE");
}
virtual void TeleopPeriodic() {
rightDrive->SetSpeed(-(Driver->GetRawAxis(2)));
leftDrive->SetSpeed((Driver->GetRawAxis(5)));
shooterFWD->SetSpeed(-(Operator->GetRawAxis(2)));
shooterRear->SetSpeed(-(Operator->GetRawAxis(2)));
//shoioter angle
if(Operator->GetRawButton(5))
{
cout<<"Relay 1 forward"<<endl;
shooterAngle->Set(Relay::kForward);
}
if(Operator->GetRawButton(6))
{
cout<<"Relay 1 Reverse"<<endl;
shooterAngle->Set(Relay::kReverse);
}
//Fire button
if(Operator->GetRawButton(1))
{
cout<<"Relay 1 forward"<<endl;
shooterFire->Set(Relay::kForward);
}
if(Operator->GetRawButton(2))
{
cout<<"Relay 1 Reverse"<<endl;
shooterFire->Set(Relay::kReverse);
}
if(CompressorSwitch->Get() == 0){
CompressorRelay->Set(Relay::kForward);
}else{
CompressorRelay->Set(Relay::kOff);
}
//if(canPDP == 0){
// cout << "NULL" << endl;
//}else{
//canPDP->GetVoltage(Voltage) ;
//cout << "0" << endl;
//}
}
void AutonomousType3() { //Grab a bin/trash bin, and move forward
SmartDashboard::PutString("STATUS:", "STARTING AUTO 3");
chainLift.SetSpeed(0.5);
while (midPoint.Get() && maxUp.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, -0.75, 0);
if (WaitF(1.75))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.5);
while (maxDown.Get()) {
}
chainLift.SetSpeed(0);
SmartDashboard::PutString("STATUS:", "AUTO 3 COMPLETE");
}
void AutonomousType2() { //Pick tote and bin, move to auto zone
SmartDashboard::PutString("STATUS:", "STARTING AUTO 2");
chainLift.SetSpeed(0.5);
while (midPoint.Get() && maxUp.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Polar(0.3, 0, 0);
if (WaitF(1.6))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0);
chainLift.SetSpeed(-0.2);
if (WaitF(0.8))
return;
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Polar(-0.3, 0, 0);
if (WaitF(1.6))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0);
chainLift.SetSpeed(-0.3);
while (maxDown.Get()) {
}
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Polar(0.2, 0, 0);
if (WaitF(2))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0);
chainLift.SetSpeed(0.4);
while (midPoint.Get() && maxUp.Get()) {
}
chainLift.SetSpeed(0);
//turn 90 deg
robotDrive.MecanumDrive_Polar(0, 0, -0.3);
if (WaitF(4))
return;
robotDrive.MecanumDrive_Polar(0.5, 0, 0);
if (WaitF(2.5))
return;
robotDrive.MecanumDrive_Polar(0, 0, 0);
chainLift.SetSpeed(-0.4);
while (maxDown.Get() && IsAutonomous()) {
}
chainLift.SetSpeed(0);
SmartDashboard::PutString("STATUS:", "AUTO 2 COMPLETE");
}
void CupidShuffle() {
SmartDashboard::PutString("STATUS:", "TIME 2 GET DOWWWWWWN");
//Tempo of song
static double tempo = 0.41666666667;
//Repeat # of times
for (int j = 0; j < 10 && IsAutonomous() && IsEnabled(); j++) {
//to the left to the left to the left to the left
for (int k = 0; k < 4; k++) {
robotDrive.MecanumDrive_Cartesian(-0.2, 0, 0);
Wait(tempo);
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
Wait(tempo);
}
//to the right to the right to the right
for (int k = 0; k < 4; k++) {
robotDrive.MecanumDrive_Cartesian(0.2, 0, 0);
Wait(tempo);
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
Wait(tempo);
}
//kick kick kick kick
for (int k = 0; k < 4; k++) {
chainLift.SetSpeed(0.3);
Wait(tempo);
chainLift.SetSpeed(-0.3);
Wait(tempo);
}
chainLift.SetSpeed(0);
//walk it by uself (turn 90)
robotDrive.MecanumDrive_Polar(0, 0, 0.3);
Wait(tempo * 8);
}
SmartDashboard::PutString("STATUS:", "GIT GUD");
}
virtual void AutonomousInit() {
shooterAngle->Set(Relay::kForward);
rightDrive->SetSpeed(0);
leftDrive->SetSpeed(0);
shooterFWD->SetSpeed(1);
shooterRear->SetSpeed(1);
sleep(4);
cout << "BEGIN" << endl;
shooterFire->Set(Relay::kForward);
sleep(1);
shooterFire->Set(Relay::kReverse);
sleep(1);
//nanosleep(6);
shooterFire->Set(Relay::kForward);
sleep(1);
shooterFire->Set(Relay::kReverse);
sleep(1);
shooterFire->Set(Relay::kForward);
sleep(1);
shooterFire->Set(Relay::kReverse);
sleep(1);
shooterFire->Set(Relay::kForward);
sleep(1);
shooterFire->Set(Relay::kReverse);
sleep(2);
shooterFWD->SetSpeed(0);
shooterRear->SetSpeed(0);
sleep(2);
shooterAngle->Set(Relay::kReverse);
cout << "END" << endl;
}
void AutonomousType6() { //All 3 totes with accelerometer
//Pick up 3 bins using gyroscope to correct corse
Wait(1);
SmartDashboard::PutString("STATUS:", "AUTO 6 (ACCEL)");
//Move forward
//robotDrive.MecanumDrive_Cartesian(0, -0.5, 0);
//Wait(0.1);
if (!IsAutonomous() || !IsEnabled())
return;
//Lift up first box
//robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0.8);
while (midPoint.Get() && maxUp.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
chainLift.SetSpeed(0);
for (int j = 0; j < 1400; j++) {
if (!IsAutonomous() || !IsEnabled())
return;
//Move forward
//CORRECT WITH ACCEL
robotDrive.MecanumDrive_Cartesian(-accel.GetX(), 0, 0);
Wait(0.001);
}
//Drop
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.3);
Wait(1);
if (!IsAutonomous() || !IsEnabled())
return;
//back a little to unhook from stack
robotDrive.MecanumDrive_Cartesian(0, 0.5, 0);
Wait(0.3);
if (!IsAutonomous() || !IsEnabled())
return;
//down to grab stack
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.8);
while (maxDown.Get()) {
}
if (!IsAutonomous() || !IsEnabled())
return;
//forward a little
robotDrive.MecanumDrive_Cartesian(0, -0.4, 0);
Wait(1.2);
if (!IsAutonomous() || !IsEnabled())
return;
//pick up stack
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0.8);
while (midPoint.Get() && maxUp.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
chainLift.SetSpeed(0);
for (int j = 0; j < 1500; j++) {
if (!IsAutonomous() || !IsEnabled())
return;
//Move forward
robotDrive.MecanumDrive_Cartesian(-accel.GetX(), 0, 0);
Wait(0.001);
}
//down
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.3);
Wait(0.5);
if (!IsAutonomous() || !IsEnabled())
return;
//back
robotDrive.MecanumDrive_Cartesian(0, 0.7, 0);
Wait(0.5);
if (!IsAutonomous() || !IsEnabled())
return;
//down
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.8);
while (maxDown.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
//forward
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, -0.3, 0);
Wait(2);
if (!IsAutonomous() || !IsEnabled())
return;
//up
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0.8);
while (midPoint.Get() && maxUp.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
chainLift.SetSpeed(0);
//turn 90 deg
robotDrive.MecanumDrive_Polar(0, 0, -0.3);
Wait(4);
robotDrive.MecanumDrive_Polar(0.5, 0, 0);
Wait(2.5);
robotDrive.MecanumDrive_Polar(0, 0, 0);
chainLift.SetSpeed(-0.4);
while (maxDown.Get() && IsAutonomous()) {
}
chainLift.SetSpeed(0);
SmartDashboard::PutString("STATUS:", "AUTO 6 COMPLETE");
}
void AutonomousType5() { //All 3 totes with correction constant
double CORRECTION = -0.021;
SmartDashboard::PutString("STATUS:", "STARTING AUTO 5");
chainLift.SetSpeed(0.8);
while (midPoint.Get() && maxUp.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
chainLift.SetSpeed(0);
//Move forward
robotDrive.MecanumDrive_Cartesian(0, -0.5, CORRECTION);
Wait(1.5);
if (!IsAutonomous() || !IsEnabled())
return;
//Drop
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.2);
Wait(0.35);
if (!IsAutonomous() || !IsEnabled())
return;
//back a little to unhook from stack
robotDrive.MecanumDrive_Cartesian(0, 0.2, 0);
Wait(0.5);
if (!IsAutonomous() || !IsEnabled())
return;
//down to grab stack
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.8);
while (maxDown.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
//forward a little
robotDrive.MecanumDrive_Cartesian(0, -0.3, 0);
Wait(0.4);
if (!IsAutonomous() || !IsEnabled())
return;
//pick up stack
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0.8);
while (midPoint.Get() && maxUp.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
chainLift.SetSpeed(0);
//forward
robotDrive.MecanumDrive_Cartesian(0, -0.5, CORRECTION);
Wait(1.5);
if (!IsAutonomous() || !IsEnabled())
return;
//down
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.2);
Wait(0.35);
if (!IsAutonomous() || !IsEnabled())
return;
//back
robotDrive.MecanumDrive_Cartesian(0, 0.2, 0);
Wait(0.5);
if (!IsAutonomous() || !IsEnabled())
return;
//down
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(-0.8);
while (maxDown.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
//forward
chainLift.SetSpeed(0);
robotDrive.MecanumDrive_Cartesian(0, -0.3, CORRECTION);
Wait(0.4);
if (!IsAutonomous() || !IsEnabled())
return;
//up
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0.8);
while (midPoint.Get() && maxUp.Get()) {
if (!IsAutonomous() || !IsEnabled())
return;
}
chainLift.SetSpeed(0);
//sideways to zone
robotDrive.MecanumDrive_Cartesian(0.5, 0, 0);
if (WaitF(0.5))
return;
//stop
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
SmartDashboard::PutString("STATUS:", "AUTO 5 COMPLETE");
}
void Disabled() {
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
chainLift.SetSpeed(0);
SmartDashboard::PutString("STATUS:", "DISABLED");
}
void TeleopPeriodic() {
if(tick==10) if (ds->IsSysBrownedOut()) {
ds->ReportError("[ERROR] BROWNOUT DETECTED!!");
}
if(tick == 15) if (!ds->IsNewControlData()) {
ds->ReportError(
"[ERROR] NO DATA FROM DRIVER STATION IN THIS TICK!");
}
if(tick==20) if (!ds->IsDSAttached()) {
ds->ReportError("[ERROR] DRIVER STATION NOT DETECTED!");
}
if (stick.GetRawButton(10))
zeroSanics();
if (stick.GetRawButton(8)) {
leftIRZero = 0;
rightIRZero = 0;
}
tick++;
if (liftStick.GetRawButton(2)) {
double canScale = liftStick.GetRawAxis(2);
canScale += 1;
canScale = 2 - canScale;
canScale /= 2;
canGrabber.SetSpeed(canScale);
} else if (liftStick.GetRawButton(3)) {
double canScale = liftStick.GetRawAxis(2);
canScale += 1;
canScale = 2 - canScale;
canScale /= 2;
canGrabber.SetSpeed(-canScale);
} else
canGrabber.SetSpeed(0);
double speed;
//Calculate scalar to use for POV/Adjusted drive
double scale = stick.GetRawAxis(3);
scale += 1;
scale = 2 - scale;
scale /= 2;
//Use pov/hat switch for movement if enabled
if (stick.GetRawButton(1) && stick.GetRawButton(2)) {
AutomaticLineup();
} else if (stick.GetRawButton(1)) {
double leftVolts = leftIR.GetAverageVoltage() - leftIRZero;
double rightVolts = rightIR.GetAverageVoltage() - leftIRZero;
if (rightVolts + VOLTAGE_TOLERANCE > leftVolts
&& rightVolts - VOLTAGE_TOLERANCE < leftVolts) {
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
} else if (rightVolts > leftVolts)
robotDrive.MecanumDrive_Cartesian(0, 0, 0.2);
else if (leftVolts > rightVolts)
robotDrive.MecanumDrive_Cartesian(0, 0, -0.2);
} else if (stick.GetRawButton(6)) {
//Rotate
robotDrive.MecanumDrive_Polar(0, 0, scale);
} else if (stick.GetRawButton(5)) {
//Rotate
robotDrive.MecanumDrive_Polar(0, 0, -scale);
} else if (stick.GetPOV(0) != -1) {
//If POV moved, move polar (getPOV returns an angle in degrees)
robotDrive.MecanumDrive_Polar(scale, -stick.GetPOV(0), 0);
} else if (stick.GetRawButton(2)) {
//Drive with scalar
robotDrive.MecanumDrive_Cartesian(-stick.GetRawAxis(0) * scale,
stick.GetRawAxis(1) * scale, stick.GetRawAxis(2) * scale);
} else {
//Drive normally
robotDrive.MecanumDrive_Cartesian(-stick.GetX(), stick.GetY(),
stick.GetZ());
}
speed = -liftStick.GetY();
//bool canGoUp = maxUp.Get();
bool canGoUp = true;
//bool canGoDown = maxDown.Get();
bool canGoDown = true;
//If at a limit switch and moving in that direction, stop
if (speed > 0 && !canGoUp)
speed = 0;
if (speed < 0 && !canGoDown)
speed = 0;
chainLift.SetSpeed(speed);
if (tick >50) {
if (SmartDashboard::GetBoolean("Smart Dashboard Enabled")) {
//Smart Dash outputs
//SmartDashboard::PutNumber("X Acceleration: ", accel.GetX());
//SmartDashboard::PutNumber("Y Acceleration: ", accel.GetY());
//SmartDashboard::PutNumber("Z Acceleration: ", accel.GetZ());
SmartDashboard::PutBoolean("Switch 1: (up)", maxUp.Get());
SmartDashboard::PutBoolean("Switch 2: (down)", maxDown.Get());
SmartDashboard::PutBoolean("Switch 3: (mid)", midPoint.Get());
SmartDashboard::PutBoolean("Auto switch A: ",
autoSwitch1.Get());
SmartDashboard::PutBoolean("Auto switch B: ",
autoSwitch2.Get());
//SmartDashboard::PutBoolean("RobotDrive Alive?",
// robotDrive.IsAlive());
//SmartDashboard::PutBoolean("ChainLift Alive?",
// robotDrive.IsAlive());
SmartDashboard::PutNumber("Left Sensor",
leftIR.GetAverageVoltage());
SmartDashboard::PutNumber("Right Sensor",
rightIR.GetAverageVoltage());
SmartDashboard::PutNumber("Left w zero",
leftIR.GetAverageVoltage() - leftIRZero);
SmartDashboard::PutNumber("Rigt w zero",
rightIR.GetAverageVoltage() - rightIRZero);
SmartDashboard::PutNumber("PDP 14 Current", pdp.GetCurrent(14));
SmartDashboard::PutNumber("PDP 15 Current", pdp.GetCurrent(15));
}
tick = 0;
}
}
