//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");
}
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");
}
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 LinearAcceleration(double startSpeed, double endSpeed, double time,
double angle) {
const double UPDATE_FREQ = 0.05;
double toChange = endSpeed - startSpeed;
double tickCount = time / UPDATE_FREQ;
double speed = startSpeed;
double tickChange = toChange / tickCount;
for (int tick = 0; tick < tickCount; tick++) {
robotDrive.MecanumDrive_Polar(speed, angle, 0);
speed += tickChange;
}
}
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");
}
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 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;
}
}