diff --git a/SimulationTest/src/main/cpp/Robot.cpp b/SimulationTest/src/main/cpp/Robot.cpp index c2cbef5..1946f92 100644 --- a/SimulationTest/src/main/cpp/Robot.cpp +++ b/SimulationTest/src/main/cpp/Robot.cpp @@ -27,16 +27,24 @@ void Robot::SimulationPeriodic() { void Robot::TeleopInit() { - test.setState(testClimbOneBar::State::IDLE); + // test.setState(testClimbOneBar::State::IDLE); + waitTest.setState(testWaited::State::IDLE); } void Robot::TeleopPeriodic() { - if (test.getState() == testClimbOneBar::State::IDLE) test.setState(testClimbOneBar::State::WAITING_FOR_EXTEND_BUTTON); - if (joystick.GetRawButton(1)) test.setState(testClimbOneBar::State::TESTING); + if (waitTest.getState() == testWaited::State::IDLE) waitTest.setState(testWaited::State::WAITING_FOR_EXTEND_BUTTON); + if (joystick.GetRawButton(1)) waitTest.setState(testWaited::State::TESTING); // else if (joystick.GetRawButton(2)) test.setState(testClimbOneBar::State::WAITING_FOR_RETRACT_BUTTON); - else test.setState(testClimbOneBar::State::WAITING_FOR_EXTEND_BUTTON); + else waitTest.setState(testWaited::State::WAITING_FOR_EXTEND_BUTTON); + + waitTest.periodic(); + + // if (test.getState() == testClimbOneBar::State::IDLE) test.setState(testClimbOneBar::State::WAITING_FOR_EXTEND_BUTTON); + // if (joystick.GetRawButton(1)) test.setState(testClimbOneBar::State::TESTING); + // // else if (joystick.GetRawButton(2)) test.setState(testClimbOneBar::State::WAITING_FOR_RETRACT_BUTTON); + // else test.setState(testClimbOneBar::State::WAITING_FOR_EXTEND_BUTTON); - test.periodic(); + // test.periodic(); } void Robot::DisabledInit() {} diff --git a/SimulationTest/src/main/cpp/simulation/testWaited.cpp b/SimulationTest/src/main/cpp/simulation/testWaited.cpp new file mode 100644 index 0000000..abbe793 --- /dev/null +++ b/SimulationTest/src/main/cpp/simulation/testWaited.cpp @@ -0,0 +1,64 @@ +#include "testWaited.h" +#include + +testWaited::testWaited(Climber& c) : climber(c) { + initTestWaited(); +} + +void testWaited::initTestWaited() { + climber.getFullExtend().Set(false); + climber.getMedExtend().Set(false); + climber.getMotor().SetNeutralMode(NeutralMode::Brake); + climber.getMotor().SetSelectedSensorPosition(0); + climber.SetState(Climber::State::IDLE); +} + +void testWaited::periodic() { + // std::cout << "passed: " << passed << "\n"; + if (passed == 2) { + std::cout << "test passed\n"; + return; + } + if (passed == 1) { + std::cout << "test failed\n"; + return; + } + switch(state) { + case WAITING_FOR_EXTEND_BUTTON: + std::cout << "Waiting for raw button 1...\n"; + break; + case TESTING: + std::cout << "Wait testing...\n"; + test(); + break; + default: + break; + + } +} + +bool testWaited::test() { + climber.SetState(Climber::State::TEST_DIAGONAL_ARM_EXTEND); + climber.Periodic(0, 0, timer.GetMatchTime().value(), false, false, false, false, false); + + // std::cout << timer.GetMatchTime().value() << "\n"; + + if (climber.GetState() == Climber::State::TEST_DIAGONAL_ARM_EXTEND) { + std::cout << "Not switched yet\n"; + } + else if (climber.GetState() == Climber::State::DIAGONAL_ARM_EXTEND) { + std::cout << "Switched to Diagonal Arm Extend\n"; + passed = 1; // TESTING + } + else { + std::cout << "huh- switched to a different state, TEST FAILED\n"; + passed = 1; // + } + + return true; // TESTING +} + +void testWaited::setState(State newState){ + prevState = state; + state = newState; +} \ No newline at end of file diff --git a/SimulationTest/src/main/cpp/source/Climber.cpp b/SimulationTest/src/main/cpp/source/Climber.cpp index 3031722..0a7afbc 100644 --- a/SimulationTest/src/main/cpp/source/Climber.cpp +++ b/SimulationTest/src/main/cpp/source/Climber.cpp @@ -103,9 +103,18 @@ Climber::State Climber::TestDiagonalArmExtend() { std::clamp(motorPIDController.Calculate(gearboxMaster.GetSelectedSensorPosition(), ClimbConstants::motorTestExtendPose), -ClimbConstants::motorMaxOutput, ClimbConstants::motorMaxOutput)); - if (stateJustChanged()) waitStartTime = currTime; + if (stateJustChanged()) { + waitStartTime = currTime; + std::cout << "stateJustChanged, setting waitStartTime - Climber::TestDiagonalArmExtend" << "\n"; + std::cout << waitStartTime << "\n"; + } + + // std::cout << waited(ClimbConstants::timeToTestExtension, waitStartTime) << "\n"; // TESTING - if (waited(ClimbConstants::timeToTestExtension, waitStartTime) && hooked()) return DIAGONAL_ARM_EXTEND; + if (!waited(ClimbConstants::timeToTestExtension, waitStartTime)) { + return TEST_DIAGONAL_ARM_EXTEND; + } + else if (hooked()) return DIAGONAL_ARM_EXTEND; else return VERTICAL_ARM_RETRACT; } @@ -114,7 +123,9 @@ Climber::State Climber::DiagonalArmExtend(double pitch, double delta_pitch){ brake.Set(false); gearboxMaster.SetNeutralMode(NeutralMode::Coast); - if (stateJustChanged()) waitStartTime = currTime; //so only at start of state. i might try to implement a better way to determine this + if (stateJustChanged()) { + waitStartTime = currTime; + } //so only at start of state. i might try to implement a better way to determine this climbMedExtend.Set(true); climbFullExtend.Set(true); if (waited(ClimbConstants::diagonalArmExtendWaitTime, waitStartTime)) { @@ -163,7 +174,9 @@ Climber::State Climber::DiagonalArmRetract(bool doSecondClimb, double pitch, dou //have the static hooks engaged bool Climber::hooked() { - return (gearboxMaster.GetStatorCurrent() >= ClimbConstants::hookedCurrent); + // testing!!! (Change back later); + return true; + // return (gearboxMaster.GetStatorCurrent() >= ClimbConstants::hookedCurrent); } //is the motor at set point (kinda redundant now that I know about AtSetpoint(), may erase later) @@ -189,7 +202,7 @@ Climber::SetState(State newState){ //has the time passed bool Climber::waited(double time, double startTime) { - return (startTime + time) >= currTime; + return (startTime + time) <= currTime; } //are we in a new state (this is important for waited) diff --git a/SimulationTest/src/main/include/Robot.h b/SimulationTest/src/main/include/Robot.h index 4c0a308..8713bb1 100644 --- a/SimulationTest/src/main/include/Robot.h +++ b/SimulationTest/src/main/include/Robot.h @@ -12,6 +12,7 @@ #include #include #include "simulation/testClimbOneBar.h" +#include "simulation/testWaited.h" #include "simulation/PhysicsSim.h" @@ -36,6 +37,7 @@ class Robot : public frc::TimedRobot { private: Climber climber; frc::Joystick joystick{0}; - testClimbOneBar test{climber}; - + // testClimbOneBar test{climber}; + testWaited waitTest{climber}; + // frc::Timer timer; }; diff --git a/SimulationTest/src/main/include/simulation/testClimbOneBar.h b/SimulationTest/src/main/include/simulation/testClimbOneBar.h index 06f73f7..a66ce2d 100644 --- a/SimulationTest/src/main/include/simulation/testClimbOneBar.h +++ b/SimulationTest/src/main/include/simulation/testClimbOneBar.h @@ -1,5 +1,6 @@ #include "Climber.h" #include "Constants.h" +#include "frc/Timer.h" class testClimbOneBar { @@ -33,4 +34,5 @@ class testClimbOneBar { int passedExtending; int passedRetracting; + frc::Timer timer; }; \ No newline at end of file diff --git a/SimulationTest/src/main/include/simulation/testWaited.h b/SimulationTest/src/main/include/simulation/testWaited.h new file mode 100644 index 0000000..1ee6248 --- /dev/null +++ b/SimulationTest/src/main/include/simulation/testWaited.h @@ -0,0 +1,38 @@ +#include "Climber.h" +#include "Constants.h" +#include "frc/Timer.h" + + +class testWaited { + public: + enum State { + IDLE, + WAITING_FOR_EXTEND_BUTTON, + TESTING + }; + + testWaited(Climber& c); + void initTestWaited(); + void periodic(); + + bool test(); + + void setState(State newState); + State getState() {return state;} + + private: + + bool extending(); + bool retracting(); + + Climber& climber; + + State prevState; + State state; + + int passed; + int passedExtending; + int passedRetracting; + + frc::Timer timer; +}; \ No newline at end of file diff --git a/SimulationTest/vendordeps/WPILibOldCommands.json b/SimulationTest/vendordeps/WPILibOldCommands.json new file mode 100644 index 0000000..acc8879 --- /dev/null +++ b/SimulationTest/vendordeps/WPILibOldCommands.json @@ -0,0 +1,37 @@ +{ + "fileName": "WPILibOldCommands.json", + "name": "WPILib-Old-Commands", + "version": "2020.0.0", + "uuid": "b066afc2-5c18-43c4-b758-43381fcb275e", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibOldCommands", + "artifactId": "wpilibOldCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibOldCommands", + "artifactId": "wpilibOldCommands-cpp", + "version": "wpilib", + "libName": "wpilibOldCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxaarch64bionic", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxx86-64" + ] + } + ] +} \ No newline at end of file