diff --git a/FRC2018/src/PenguinEmpire.h b/FRC2018/src/PenguinEmpire.h index 538940a..b1173cb 100644 --- a/FRC2018/src/PenguinEmpire.h +++ b/FRC2018/src/PenguinEmpire.h @@ -49,14 +49,15 @@ class Robot : public IterativeRobot { Spark lift1, lift2; // Lifter motor controllers AHRS *ahrs; // Purple sensor board Compressor compressor; - DoubleSolenoid leftGearbox, rightGearbox, liftGearbox; // Gearbox shifters - DoubleSolenoid omniDropper; + DoubleSolenoid driveGearboxes, liftGearbox, omniDropper; // Gearbox shifters and omni actuator Timer* mainTimer; Timer* lidarTimer; Lidar* lidar; DigitalInput* bottomSensor; DigitalInput* switchSensor; DigitalInput* topSensor; + DigitalInput* centerPosSwitch; + DigitalInput* rightPosSwitch; Encoder leftEnc, rightEnc; // Values and Structures @@ -71,6 +72,7 @@ class Robot : public IterativeRobot { bool turnSetup; bool comboLift; bool comboDrive; + bool autoDrop; bool gl90, gr90, gl180, gr180; @@ -105,7 +107,7 @@ class Robot : public IterativeRobot { // }; std::vector> autosteps; - std::vector> ll, lr, cl, cr, rl, rr; + std::vector> lll, llr, lrl, lrr, cl, cr, rrr, rrl, rlr, rll; int numsteps, curstep; bool stepSetup, stepComplete; int fpos; @@ -114,6 +116,14 @@ class Robot : public IterativeRobot { int lastLiftState; bool haltLifter; bool goingPastSwitch; + bool checkSwitch; + bool ioForward, ioBackward; + bool released; + bool autoRaise; + long raiseCounter; + int sightCounter; + + bool visionAligned; std::shared_ptr contour; std::vector centerX, centerY, area, width; @@ -153,6 +163,8 @@ class Robot : public IterativeRobot { void ManualCubeIO(bool inBtn, bool outBtn); void DropOmnis(bool dropBtn, bool raiseBtn); void HoldOmnis(bool btn); + void ToggleSwitchSensor(bool on, bool off); + void ManualVision(bool btn); // Test void TestInit(); @@ -164,6 +176,7 @@ class Robot : public IterativeRobot { void RunCubeIO(Direction dir); void RunLifter(bool up, bool down); void CheckHallSensor(); + void ToggleIO(bool forward, bool backward); }; diff --git a/FRC2018/src/Robot.cpp b/FRC2018/src/Robot.cpp index 4439f78..504e639 100644 --- a/FRC2018/src/Robot.cpp +++ b/FRC2018/src/Robot.cpp @@ -84,21 +84,20 @@ Robot::Robot() : // Robot constructor - Initialize all subsystem and component c leftStick(usb0), rightStick(usb1), handheld(usb2), - l1(pwm0), - l2(pwm1), - r1(pwm2), - r2(pwm3), - leftIO(pwm4), - rightIO(pwm5), - lift1(pwm6), - lift2(pwm7), + l1(pwm0), //PDP0 + l2(pwm1), //PDP1 + r1(pwm2), //PDP14 + r2(pwm3), //PDP15 + leftIO(pwm4), //PDP13 + rightIO(pwm5), //PDP12 + lift1(pwm6), //PDP2 + lift2(pwm7), //PDP3 compressor(pcm0), - leftGearbox(pcm0, pch0, pch1), - rightGearbox(pcm0, pch2, pch3), - liftGearbox(pcm0, pch4, pch5), - omniDropper(pcm0, pch6, pch7), - leftEnc(dio1, dio0), - rightEnc(dio3, dio2) + driveGearboxes(pcm0, pch0, pch1), + liftGearbox(pcm0, pch6, pch7), + omniDropper(pcm0, pch4, pch5), + leftEnc(dio3, dio2), + rightEnc(dio1, dio0) /* * things to change @@ -114,7 +113,6 @@ Robot::Robot() : // Robot constructor - Initialize all subsystem and component c * spark 7 and 6, one is flipped polarity * left stick drives right * left trigger opposite of right trigger - * */ { leftSwitch = false; @@ -132,7 +130,8 @@ Robot::Robot() : // Robot constructor - Initialize all subsystem and component c gl180 = false; gr180 = false; - ahrs = new AHRS(SerialPort::kMXP); + // USE SPI AND NOT SERIALPORT + ahrs = new AHRS(SPI::kMXP); // fpos = centerPos; fpos = 1; @@ -173,48 +172,59 @@ Robot::Robot() : // Robot constructor - Initialize all subsystem and component c * 99: Loop all previous steps */ -// rrr = {{3, 1}, -// {2, 115, 1.0}, -// {1, -90, 0.75}, -// {2, 2, 0.5}, -// {5, 0.5, 1.0}, -// {2, -5, 0.75}, -// {3, 0}, -// {1, 90, 0.75}, -// {2, 43, 0.85}, -// {1, -90, 0.75}, -// {2, 18, 0.65}, -// {1, -75, 0.75}, -// {7, 256, 384, 0.5}, -// {6, -1.0}, -// {4, 13, 0.65}, -// {6, 0.0}, -// {3, 1}, -// {1, 10, 1.0}, -// {2, 2, 0.5}, -// {5, 0.5, 1.0}, -// {2, -5, 0.5}, -// {3, 0} -// }; - - rr = {{8}, - {9, 1, 75, 1.0}, + rrr = {{8}, + {9, 1, 260, 1.0}, + {10, 1}, + {1, -40, 0.5}, + {10, 0}, + {3, 2}, + {2, 24, 0.5}, + {5, 0.5, 1.0}, + {2, -10, 0.5}, + {11, 1}, + {9, 0, -14, 0.5}, + {11, 0}, + {10, 1}, + {1, -75, 0.75}, + {10, 0}, + {2, 24, 0.65}, + {7, 256, 384, 0.5}, + {6, -1.0}, + {4, 20, 0.65}, + {6, 0.0}, + {3, 1}, + {2, 2, 0.5}, + {5, 0.5, 1.0}, + {2, -5, 0.5}, + {3, 0} + }; + + rrl = {{8}, + {9, 1, 125, 1.0}, // {3, 1}, // {2, 75, 1.0}, - {1, -90, 0.75}, - {2, 2, 0.5}, + {10, 1}, + {1, -60, 0.75}, + {10, 0}, + {2, 26, 0.75}, {5, 0.5, 1.0}, - {9, 0, -5, 0.75}, + {9, 0, -26, 0.75}, // {2, -5, 0.75}, // {3, 0}, - {1, 90, 0.65}, - {2, 40, 0.75}, - {1, -90, 0.65}, - {2, 18, 0.65}, + {10, 1}, + {1, 60, 0.65}, + {10, 0}, + {2, 85, 0.90}, + {10, 1}, + {1, -60, 0.65}, + {10, 0}, + {2, 54, 0.65}, + {10, 1}, {1, -75, 0.65}, + {10, 0}, {7, 256, 384, 0.5}, {6, -1.0}, - {4, 13, 0.65}, + {4, 20, 0.65}, {6, 0.0}, {3, 1}, {2, 2, 0.5}, @@ -223,30 +233,158 @@ Robot::Robot() : // Robot constructor - Initialize all subsystem and component c {3, 0} }; - rl = {{8}, + rlr = {{8}, + {9, 1, 260, 1.0}, + {10, 1}, + {1, -30, 0.5}, + {10, 0}, + {3, 2}, + {2, 24, 0.5}, + {5, 0.5, 1.0}, + {2, -10, 0.5}, + {11, 1}, + {9, 0, -14, 0.5}, + {11, 0}, + {10, 1}, + {1, -150, 0.75}, + {10, 0}, + {7, 256, 384, 0.5} + }; + + rll = {{8}, {9, 1, 204, 1.0}, // {3, 1}, -// {2, 204, 1.0}, - {1, -90, 0.65} +// {2, 205, 1.0}, + {10, 1}, + {1, -60, 0.65}, + {10, 0} }; - ll = {{8}, - {9, 1, 120, 1.0}, + cl = {{8}, + {2, 27, 1.0}, // {3, 1}, -// {2, 120, 1.0}, - {1, 90, 0.75}, - {2, 2, 0.5}, +// {2, 48, 0.65}, + {10, 1}, + {1, -30, 0.5}, + {10, 0}, + {9, 1, 63, 0.65}, + {10, 1}, + {1, 30, 0.5}, + {10, 0}, +// {2, 3, 0.65}, + {2, 36, 0.65}, + {5, 0.5, 1.0}, + {2, -5, 0.5}, + {10, 1}, + {1, -60, 0.75}, + {10, 0}, + {3, 0} + }; + + cr = {{8}, + {2, 30, 0.65}, +// {3, 1}, +// {2, 48, 0.65}, + {10, 1}, + {1, 30, 0.5}, + {10, 0}, + {9, 1, 60, 0.65}, + {10, 1}, + {1, -30, 0.5}, + {10, 0}, +// {2, 3, 0.65}, + {2, 36, 0.65}, + {5, 0.5, 1.0}, + {2, -5, 0.5}, + {10, 1}, + {1, 60, 0.75}, + {10, 0}, + {3, 0} + }; + lll = {{8}, + {9, 1, 260, 1.0}, + {10, 1}, + {1, 40, 0.5}, + {10, 0}, + {2, 24, 0.5}, + {5, 0.5, 1.0}, + {2, -10, 0.5}, + {11, 1}, + {9, 0, -14, 0.5}, + {11, 0}, + {10, 1}, + {1, 75, 0.75}, + {10, 0}, + {7, 256, 384, 0.5}, + {6, -1.0}, + {4, 20, 0.65}, + {6, 0.0}, + {3, 1}, + {2, 2, 0.5}, + {5, 0.5, 1.0}, + {2, -5, 0.5}, + {3, 0} + +// {8}, +// {9, 1, 125, 1.0}, +//// {3, 1}, +//// {2, 75, 1.0}, +// {10, 1}, +// {1, 60, 0.75}, +// {10, 0}, +// {2, 26, 0.75}, +// {5, 0.5, 1.0}, +// {9, 0, -26, 0.75}, +//// {2, -5, 0.75}, +//// {3, 0}, +// {10, 1}, +// {1, -60, 0.65}, +// {10, 0}, +// {2, 85, 0.90}, +// {10, 1}, +// {1, 60, 0.65}, +// {10, 0}, +// {2, 54, 0.65}, +// {10, 1}, +// {1, 75, 0.65}, +// {10, 0}, +// {7, 256, 384, 0.5}, +// {6, -1.0}, +// {4, 20, 0.65}, +// {6, 0.0}, +// {3, 1}, +// {2, 2, 0.5}, +// {5, 0.5, 1.0}, +// {2, -5, 0.5}, +// {3, 0} + }; + + llr = {{8}, + {9, 1, 125, 1.0}, +// {3, 1}, +// {2, 75, 1.0}, + {10, 1}, + {1, 60, 0.75}, + {10, 0}, + {2, 26, 0.75}, {5, 0.5, 1.0}, - {2, -5, 0.75}, - {3, 0}, - {1, -90, 0.65}, - {2, 47, 0.75}, - {1, -90, 0.65}, - {2, 18, 0.65}, + {9, 0, -26, 0.75}, +// {2, -5, 0.75}, +// {3, 0}, + {10, 1}, + {1, -60, 0.65}, + {10, 0}, + {2, 85, 0.90}, + {10, 1}, + {1, 60, 0.65}, + {10, 0}, + {2, 54, 0.65}, + {10, 1}, {1, 75, 0.65}, + {10, 0}, {7, 256, 384, 0.5}, {6, -1.0}, - {4, 13, 0.65}, + {4, 20, 0.65}, {6, 0.0}, {3, 1}, {2, 2, 0.5}, @@ -255,34 +393,33 @@ Robot::Robot() : // Robot constructor - Initialize all subsystem and component c {3, 0} }; - lr = {{8}, - {9, 1, 204, 1.0}, -// {3, 1}, -// {2, 204, 1.0}, - {1, 90, 0.65} + lrl = {{8}, + {9, 1, 260, 1.0}, + {10, 1}, + {1, 30, 0.5}, + {10, 0}, + {3, 2}, + {2, 24, 0.5}, + {5, 0.5, 1.0}, + {2, -10, 0.5}, + {11, 1}, + {9, 0, -14, 0.5}, + {11, 0}, + {10, 1}, + {1, 150, 0.75}, + {10, 0}, + {7, 256, 384, 0.5} }; - cl = {{8}, - {9, 1, 48, 0.65}, + lrr = {{8}, + {9, 1, 204, 1.0}, // {3, 1}, -// {2, 48, 0.65}, - {1, -45, 0.75}, - {2, 60, 0.65}, - {1, 45, 0.75}, - {2, 24, 0.65}, - {5, 0.5, 1.0} +// {2, 205, 1.0}, + {10, 1}, + {1, 60, 0.65}, + {10, 0} }; - cr = {{8}, - {9, 1, 48, 0.65}, -// {3, 1}, -// {2, 48, 0.65}, - {1, 45, 0.75}, - {2, 60, 0.65}, - {1, -45, 0.75}, - {2, 24, 0.65}, - {5, 0.5, 1.0} - }; mode = "lll"; @@ -292,6 +429,7 @@ Robot::Robot() : // Robot constructor - Initialize all subsystem and component c stepComplete = false; comboLift = false; comboDrive = false; + autoDrop = false; l1.SetExpiration(0.1); l2.SetExpiration(0.1); @@ -304,6 +442,8 @@ Robot::Robot() : // Robot constructor - Initialize all subsystem and component c bottomSensor = new DigitalInput(dio7); switchSensor = new DigitalInput(dio8); topSensor = new DigitalInput(dio9); + centerPosSwitch = new DigitalInput(dio4); + rightPosSwitch = new DigitalInput(dio5); testStep = 0; leftEnc.SetDistancePerPulse(pulseIn); @@ -312,6 +452,15 @@ Robot::Robot() : // Robot constructor - Initialize all subsystem and component c lastLiftState = 0; haltLifter = false; goingPastSwitch = false; + checkSwitch = true; + ioForward = false; + ioBackward = false; + released = true; + autoRaise = false; + raiseCounter = 0; + sightCounter = 0; + + visionAligned = false; lidarTimer = new Timer(); lidar = new Lidar; @@ -330,6 +479,9 @@ Robot::~Robot() { // Robot destructor - Delete pointer values here } void Robot::RobotInit() { // Runs only when robot code starts initially + // Initialize camera server +// CameraServer::GetInstance()->StartAutomaticCapture("cam0", 0); + // Set Spark inversion l1.SetInverted(false); l2.SetInverted(false); @@ -360,13 +512,21 @@ void Robot::AutonomousInit() { // Runs at start of autonomous phase, only once CheckSide(); fpos = CheckPos(); - if (fpos == 0 && leftSwitch) { - autosteps = ll; - mode = "ll"; + if (fpos == 0 && leftSwitch && leftScale) { + autosteps = lll; + mode = "lll"; + } + else if (fpos == 0 && !leftSwitch && leftScale) { + autosteps = lrl; + mode = "lrl"; } - else if (fpos == 0 && !leftSwitch) { - autosteps = lr; - mode = "lr"; + else if (fpos == 0 && leftSwitch && !leftScale) { + autosteps = llr; + mode = "llr"; + } + else if (fpos == 0 && !leftSwitch && !leftScale) { + autosteps = lrr; + mode = "lrr"; } else if (fpos == 1 && leftSwitch) { autosteps = cl; @@ -376,13 +536,21 @@ void Robot::AutonomousInit() { // Runs at start of autonomous phase, only once autosteps = cr; mode = "cr"; } - else if (fpos == 2 && leftSwitch) { - autosteps = rl; - mode = "rl"; + else if (fpos == 2 && leftSwitch && leftScale) { + autosteps = rll; + mode = "rll"; + } + else if (fpos == 2 && !leftSwitch && leftScale) { + autosteps = rrl; + mode = "rrl"; } - else if (fpos == 2 && !leftSwitch) { - autosteps = rr; - mode = "rr"; + else if (fpos == 2 && leftSwitch && !leftScale) { + autosteps = rlr; + mode = "rlr"; + } + else if (fpos == 2 && !leftSwitch && !leftScale) { + autosteps = rrr; + mode = "rrr"; } //Auto Aim Params: minX = 256, maxX = 384 @@ -420,6 +588,8 @@ void Robot::AutonomousInit() { // Runs at start of autonomous phase, only once // {3, 0}}; numsteps = autosteps.size(); + ShiftLift(down); + checkSwitch = true; } void Robot::AutonomousPeriodic() { // Looped through iteratively during autonomous phase - do not put loops here! @@ -464,17 +634,41 @@ void Robot::CheckSide() { else { leftSwitch = false; } + + if (gamedata [1] == 'L') { + leftScale = true; + } + else { + leftScale = false; + } } int Robot::CheckPos() { /* - * Check TBD physical switch + * Check physical switch */ - return 2; + if (centerPosSwitch->Get()) { + return 1; + } + else { + if (rightPosSwitch->Get()) { + return 2; + } + else { + return 0; + } + } + + // Comment out above conditionals and uncomment/change below value to spoof position on practice bot +// return 1; } void Robot::RunSteps() { + /* + * Runs through the selected autonomous mode step by step + */ + if (curstep < numsteps) { std::vector step = autosteps[curstep]; SmartDashboard::PutNumber("Current Step", curstep); @@ -741,6 +935,24 @@ void Robot::RunSteps() { } } } + else if (step[0] == 10) { // Set omni wheels {10, set} + if (step[1] == 1) { + omniDropper.Set(DoubleSolenoid::kReverse); + } + else { + omniDropper.Set(DoubleSolenoid::kForward); + } + stepComplete = true; + } + else if (step[0] == 11) { + if (step[1] == 1) { + ShiftLift(up); + } + else { + ShiftLift(down); + } + stepComplete = true; + } else if (step[0] == 97) { // IO Hold {97, max dist, speed} if (stepSetup) { ResetAll(); @@ -791,6 +1003,7 @@ void Robot::RunSteps() { } } + void Robot::ResetAll() { ahrs->ZeroYaw(); leftEnc.Reset(); @@ -843,12 +1056,17 @@ void Robot::TeleopPeriodic() { // Looped through iteratively during teleoperated // Gyro180L(m_left.ReadButton(11)); // Gyro180R(m_left.ReadButton(12)); ManualShiftGears(m_right.ReadButton(6), m_right.ReadButton(4)); - ManualShiftLift(m_left.ReadButton(6), m_left.ReadButton(4)); - ManualCubeIO(m_left.ReadButton(1), m_right.ReadButton(1)); - RunLifter(m_right.ReadButton(5), m_right.ReadButton(3)); + ManualShiftLift(m_left.ReadButton(6) || m_handheld.ReadButton(4), m_left.ReadButton(4) || m_handheld.ReadButton(2)); + ManualCubeIO(m_handheld.ReadButton(8), m_handheld.ReadButton(6)); + RunLifter(m_handheld.ReadButton(5) || m_right.ReadButton(1), m_handheld.ReadButton(7) || m_left.ReadButton(1)); // DropOmnis(m_left.ReadButton(5), m_left.ReadButton(3)); HoldOmnis(m_right.ReadButton(2)); + ToggleSwitchSensor(m_handheld.ReadButton(1), m_handheld.ReadButton(3)); CheckHallSensor(); + ToggleIO(m_handheld.ReadButton(9), m_handheld.ReadButton(10)); + ManualVision(m_left.ReadButton(2)); + + centerX = contour->GetNumberArray("centerX", llvm::ArrayRef()); //Send dashboard values @@ -857,8 +1075,15 @@ void Robot::TeleopPeriodic() { // Looped through iteratively during teleoperated SmartDashboard::PutNumber("Encoder L", leftEnc.GetDistance()); SmartDashboard::PutNumber("Encoder R", rightEnc.GetDistance()); SmartDashboard::PutNumber("Average Distance", (rightEnc.GetDistance() - leftEnc.GetDistance()) / 2); - SmartDashboard::PutBoolean("Hall Sensor", topSensor->Get()); + SmartDashboard::PutBoolean("Top Sensor", topSensor->Get()); + SmartDashboard::PutBoolean("Switch Sensor", switchSensor->Get()); + SmartDashboard::PutBoolean("Bottom Sensor", bottomSensor->Get()); SmartDashboard::PutNumber("LIDAR", dist); + SmartDashboard::PutBoolean("centerPos", centerPosSwitch->Get()); + SmartDashboard::PutBoolean("rightPos", rightPosSwitch->Get()); + SmartDashboard::PutNumberArray("Center X", centerX); + SmartDashboard::PutBoolean("Auto Raise?", autoRaise); + SmartDashboard::PutNumber("Raise Counter", raiseCounter); } /* @@ -904,7 +1129,7 @@ void Robot::TankDrive() { SetLeftSpeed(leftInput * -inputMultiplier); } else if (fabs(leftInput) >= 0.7) { - SetLeftSpeed(-leftInput); + SetLeftSpeed(-leftInput * 0.9); } else { SetLeftSpeed(0.0); @@ -914,11 +1139,19 @@ void Robot::TankDrive() { SetRightSpeed(rightInput * -inputMultiplier); } else if (fabs(rightInput) >= 0.7) { - SetRightSpeed(-rightInput); + SetRightSpeed(-rightInput * 0.9); } else { SetRightSpeed(0.0); } + +// if (fabs(rightInput - leftInput) > 0.5 && fabs(rightInput) > 0.5 && fabs(leftInput) > 0.5) { +// omniDropper.Set(DoubleSolenoid::kReverse); +// autoDrop = true; +// } +// else { +// autoDrop = false; +// } } void Robot::Gyro90L(bool btn) { @@ -1046,9 +1279,9 @@ void Robot::ManualShiftLift(bool upBtn, bool downBtn) { } void Robot::ManualCubeIO(bool in, bool out) { - float inSpeedL = -0.65; - float inSpeedR = -0.65; - float outSpeed = 0.75; + float inSpeedL = -1.0; + float inSpeedR = -1.0; + float outSpeed = 1.0; if (in && !out) { leftIO.Set(inSpeedL); @@ -1058,7 +1291,7 @@ void Robot::ManualCubeIO(bool in, bool out) { leftIO.Set(outSpeed); rightIO.Set(outSpeed); } - else if (!in && !out) { + else if (!in && !out && !ioForward && !ioBackward) { leftIO.Set(0.0); rightIO.Set(0.0); } @@ -1078,11 +1311,58 @@ void Robot::HoldOmnis(bool btn) { if (btn) { omniDropper.Set(DoubleSolenoid::kReverse); } - else { + else if (!autoDrop){ omniDropper.Set(DoubleSolenoid::kForward); } } +void Robot::ToggleSwitchSensor(bool on, bool off) { + if (on) { + checkSwitch = true; + } + + if (off) { + checkSwitch = false; + } +} + +void Robot::ManualVision(bool btn) { + if (btn) { + if (centerX.size() > 0) { + if (centerX[0] < 256) { + SetLeftSpeed(-0.5); + SetRightSpeed(0.5); + visionAligned = false; + } + else if (centerX[0] > 384) { + SetLeftSpeed(0.5); + SetRightSpeed(-0.5); + visionAligned = false; + } + else { + SetLeftSpeed(0.0); + SetRightSpeed(0.0); + visionAligned = true; + } + } + + if (visionAligned) { + if (lidar->AquireDistance() > 25) { + leftIO.Set(-1.0); + rightIO.Set(-1.0); + SetLeftSpeed(0.65); + SetRightSpeed(0.65); + } + else { + leftIO.Set(0.0); + rightIO.Set(0.0); + SetLeftSpeed(0.0); + SetRightSpeed(0.0); + } + } + } +} + void Robot::TestInit() { // Runs at start of test phase, only once } @@ -1116,8 +1396,7 @@ void Robot::ShiftGears(Direction dir) { state = DoubleSolenoid::kReverse; } - leftGearbox.Set(state); - rightGearbox.Set(state); + driveGearboxes.Set(state); } void Robot::ShiftLift(Direction dir) { @@ -1166,66 +1445,91 @@ void Robot::RunLifter(bool up, bool down) { lastLiftState = 0; } - if (!bottomSensor->Get()) { - if (down) { - lift1.Set(0.0); - lift2.Set(0.0); - } - else if (up) { - lift1.Set(upSpeed); - lift2.Set(upSpeed); - } + if (!bottomSensor->Get() || !autoRaise) { + raiseCounter = 0; } - else if (!switchSensor->Get()) { - if ((up || down) && !goingPastSwitch) { - haltLifter = true; - lift1.Set(0.0); - lift2.Set(0.0); - } - if (haltLifter && !up && !down) { - haltLifter = false; - goingPastSwitch = true; + if (lidar->AquireDistance() < 15 && !bottomSensor->Get()) { + sightCounter++; + if (sightCounter >= 3) { + autoRaise = true; } + } + else { + sightCounter = 0; + } - if (!haltLifter) { - if (up) { + if (autoRaise) { + lift1.Set(0.75); + lift2.Set(0.75); + } + else { + if (!bottomSensor->Get()) { + if (down) { + lift1.Set(0.0); + lift2.Set(0.0); + } + else if (up) { lift1.Set(upSpeed); lift2.Set(upSpeed); } + } + else if (!switchSensor->Get() && checkSwitch) { + if ((up || down) && !goingPastSwitch) { + haltLifter = true; + lift1.Set(0.0); + lift2.Set(0.0); + } + + if (haltLifter && !up && !down) { + haltLifter = false; + goingPastSwitch = true; + } + + if (!haltLifter) { + if (up) { + lift1.Set(upSpeed); + lift2.Set(upSpeed); + } + else if (down) { + lift1.Set(downSpeed); + lift2.Set(downSpeed); + } + } + } + else if (!topSensor->Get()) { + if (up) { + lift1.Set(0.0); + lift2.Set(0.0); + } else if (down) { lift1.Set(downSpeed); lift2.Set(downSpeed); } } - } - else if (!topSensor->Get()) { - if (up) { - lift1.Set(0.0); - lift2.Set(0.0); - } - else if (down) { - lift1.Set(downSpeed); - lift2.Set(downSpeed); + else { + if (up) { + lift1.Set(upSpeed); + lift2.Set(upSpeed); + } + + if (down) { + lift1.Set(downSpeed); + lift2.Set(downSpeed); + } } } - else { - if (up) { - lift1.Set(upSpeed); - lift2.Set(upSpeed); - } - if (down) { - lift1.Set(downSpeed); - lift2.Set(downSpeed); - } + if ((!switchSensor->Get() || !topSensor->Get() || raiseCounter >= 15 || up || down) && autoRaise) { + autoRaise = false; + raiseCounter = 0; } if (switchSensor->Get()) { goingPastSwitch = false; } - if (!up && !down) { + if (!up && !down && !autoRaise) { lift1.Set(0.0); lift2.Set(0.0); } @@ -1248,12 +1552,44 @@ void Robot::RunLifter(bool up, bool down) { // lift1.Set(0.0); // lift2.Set(0.0); // } + + raiseCounter++; } void Robot::CheckHallSensor() { // SmartDashboard::PutBoolean("Sensor Detecting?", hallSensor->Get()); } +void Robot::ToggleIO(bool forward, bool backward) { + if ((forward || backward) && released) { + if (forward && ioForward) { + ioForward = false; + } + else if (forward && !ioForward){ + ioForward = true; + } + + if (backward && ioBackward) { + ioBackward = false; + } + else if (backward && !ioBackward) { + ioBackward = true; + } + released = false; + } + else if (!forward && !backward){ + released = true; + } + + if (ioForward) { + leftIO.Set(1.0); + rightIO.Set(1.0); + } + else if (ioBackward) { + leftIO.Set(1.0); + rightIO.Set(1.0); + } +} //Robot::Step::Step(Robot* r , StepType steptype, std::vector parameters) : robot(r) { // params = parameters; // type = steptype;