add tests

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() {}

SimulationTest/src/main/cpp/simulation/testWaited.cpp

@@ -0,0 +1,64 @@
+#include "testWaited.h"
+#include <fstream>
+
+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;
+}

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)

SimulationTest/src/main/include/Robot.h

@@ -12,6 +12,7 @@
 #include <source/Shooter.h>
 #include <source/Climber.h>
 #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;
 };

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;
 };

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;
+};

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"
+            ]
+        }
+    ]
+}