fixed test mode

src/main/cpp/Robot.cpp

@@ -87,12 +87,6 @@ void Robot::TestPeriodic() {
 }
 
 void Robot::TestInit() {
-    m_container.GetArm()->SetControlMode(Test);
-
-    teleop_command_ = m_container.GetDriveCommand();
-    VOKC_CALL(teleop_command_ != nullptr);
-    teleop_command_->Schedule();
-    VOKC_CHECK_MSG(false, "we are in fact reaching test init");
 }
 
 /**

src/main/cpp/RobotContainer.cpp

@@ -58,11 +58,11 @@ bool RobotContainer::ConfigureButtonBindings() {
     driver_right_bumper_->WhenPressed(*other_intake_command).WhenReleased(*stop_intake_command);
 
     // HACK XXX BUG TODO temporary first driver controls arm stuff for testing so only one person is needed to test the robot
-    // driver_a_button_->WhileActiveContinous(*lowerArmCommand);
-    // driver_y_button_->WhileActiveContinous(*raiseArmCommand);
+    manip_a_button_->WhileActiveContinous(*lowerArmCommand);
+    manip_y_button_->WhileActiveContinous(*raiseArmCommand);
 
-    // driver_x_button_->WhileActiveContinous(*retractArmCommand);
-    // driver_b_button_->WhileActiveContinous(*extendArmCommand);
+    manip_x_button_->WhileActiveContinous(*retractArmCommand);
+    manip_b_button_->WhileActiveContinous(*extendArmCommand);
 
 
     // second driver controls

src/main/cpp/commands/arm/ArmFieldOrientedCommand.cpp

@@ -23,7 +23,7 @@ ArmFieldOrientedCommand::ArmFieldOrientedCommand(std::shared_ptr<Arm> arm, std::
 void ArmFieldOrientedCommand::Initialize() {
     VOKC_CHECK(arm_ != nullptr);
 
-    VOKC_CALL(arm_->SetControlMode(Auto));
+
 }
 
 void ArmFieldOrientedCommand::Execute() {

src/main/cpp/commands/arm/ArmSetStateCommand.cpp

@@ -21,7 +21,7 @@ ArmSetStateCommand::ArmSetStateCommand(std::shared_ptr<Arm> arm, TeamOKC::ArmSta
 void ArmSetStateCommand::Initialize() {
     VOKC_CHECK(arm_ != nullptr);
 
-    VOKC_CALL(arm_->SetControlMode(Auto));
+
 }
 
 void ArmSetStateCommand::Execute() {

src/main/cpp/commands/arm/ArmSetStateDpadCommand.cpp

@@ -31,7 +31,7 @@ ArmSetStateDpadCommand::ArmSetStateDpadCommand(std::shared_ptr<Arm> arm, std::sh
 void ArmSetStateDpadCommand::Initialize() {
     VOKC_CHECK(arm_ != nullptr);
 
-    VOKC_CALL(arm_->SetControlMode(Auto));
+
 }
 
 void ArmSetStateDpadCommand::Execute() {

src/main/cpp/commands/arm/IncrementArmExtendCommand.cpp

@@ -18,7 +18,7 @@ IncrementArmExtendCommand::IncrementArmExtendCommand(std::shared_ptr<Arm> arm,
 }
 
 void IncrementArmExtendCommand::Initialize() {
-    arm_->SetControlMode(Test);
+
 }
 
 

src/main/cpp/commands/arm/IncrementArmPresetPositionCommand.cpp

@@ -19,17 +19,15 @@ IncrementArmPresetPositionCommand::IncrementArmPresetPositionCommand(std::shared
 }
 
 void IncrementArmPresetPositionCommand::Initialize() {
-    arm_->SetControlMode(Test);
+
 }
 
 void IncrementArmPresetPositionCommand::Execute() {
     VOKC_CHECK(arm_ != nullptr);
     VOKC_CALL(arm_->IncrementRotation(increment_));
 }
 
-
-
-    bool IncrementArmPresetPositionCommand::IsFinished() {
+bool IncrementArmPresetPositionCommand::IsFinished() {
     bool at = false;
 
     OKC_CALL(arm_->AtExtendSetpoint(&at));

src/main/cpp/subsystems/Arm.cpp

@@ -74,7 +74,7 @@ bool Arm::SetDesiredState(TeamOKC::ArmState state) {
 }
 
 bool Arm::IncrementRotation(double increment) {
-    this->desired_state_.rotation += increment;
+    this->test_state_.rotation += increment;
 
     has_been_commanded_ = true;
     control_state_ = ROTATING;
@@ -83,7 +83,7 @@ bool Arm::IncrementRotation(double increment) {
 }
 
 bool Arm::IncrementExtend(double increment) {
-    this->desired_state_.extension += increment;
+    this->test_state_.extension += increment;
 
     has_been_commanded_ = true;
     control_state_ = ROTATING;
@@ -132,8 +132,8 @@ bool Arm::TestControl() {
                 has_calibrated_ = true;
 
                 // initialize the desired state to the state when we are calibrated                
-                this->desired_state_.rotation = this->interface_->arm_duty_cycle_encoder;
-                this->desired_state_.extension = 1; // slightly not 0 just because
+                this->test_state_.rotation = this->interface_->arm_duty_cycle_encoder;
+                this->test_state_.extension = 1; // slightly not 0 just because
             } else {
                 // otherwise, we haven't hit it yet, so set the motor to a small negative power until we do
                 this->interface_->arm_extend_power = -0.1;
@@ -151,19 +151,19 @@ bool Arm::TestControl() {
     }
 
     // limit extension
-    if (this->desired_state_.extension > extend_limit_) {
-        this->desired_state_.extension = extend_limit_;
-    } else if (this->desired_state_.extension < 0) {
-        this->desired_state_.extension = 0;
+    if (this->test_state_.extension > extend_limit_) {
+        this->test_state_.extension = extend_limit_;
+    } else if (this->test_state_.extension < 1) {
+        this->test_state_.extension = 1;
     }
 
     // set setpoints
-    this->arm_pid_->SetSetpoint(this->desired_state_.rotation);
-    this->inches_pid_->SetSetpoint(this->desired_state_.extension);
+    this->arm_pid_->SetSetpoint(this->test_state_.rotation);
+    this->inches_pid_->SetSetpoint(this->test_state_.extension);
 
     // calculate feedforward
     // take the sign of the desired state, multiply it by the feedforward gain times the desired rotation squared. this looks like output = signum(setpoint) * kF * setpoint^2
-    double ff = TeamOKC::sign(this->desired_state_.rotation) * arm_kF_ * this->desired_state_.rotation * this->desired_state_.rotation;
+    double ff = TeamOKC::sign(this->test_state_.rotation) * arm_kF_ * this->test_state_.rotation * this->test_state_.rotation;
 
     this->interface_->arm_lift_power = this->arm_pid_->Calculate(this->state_.rotation) + ff;
     this->interface_->arm_extend_power = this->inches_pid_->Calculate(this->state_.extension);
@@ -223,10 +223,10 @@ bool Arm::AutoControl() {
     // alright, we've made it thus far, so we need to get down to business and start moving
     if (control_state_ == ROTATING) {
         // bring the extension in whenever we rotate the arm, to reduce bounce
-        this->inches_pid_->SetSetpoint(0.5);
+        this->inches_pid_->SetSetpoint(1);
 
         // if we have brought the extension in
-        if (abs(1.0 - state_.extension) < 1.0) {
+        if (abs(state_.extension) < 2.0) {
             // then move the arm
             this->arm_pid_->SetSetpoint(this->desired_state_.rotation);
             this->interface_->arm_lift_power = this->arm_pid_->Calculate(this->interface_->arm_duty_cycle_encoder);
@@ -261,7 +261,7 @@ bool Arm::AutoControl() {
         // and keep rotation where it is
 
         // if the arm is trying to go to 0, and we're close to 0
-        if (abs(this->desired_state_.rotation) < 2.0 && abs(this->state_.rotation) < 7.0) {
+        if (abs(this->desired_state_.rotation) < 2.0 && abs(this->state_.rotation) < 20) {
             // to prevent it from oscillating just set it to 0
             this->interface_->arm_lift_power = 0.0;
         } else {
@@ -276,6 +276,7 @@ bool Arm::AutoControl() {
 }
 
 void Arm::Periodic() {
+    // std::cout << mode_ << std::endl;
 
     if (ArmUI::nt_test_mode->GetBoolean(false)) {
         mode_ = Test;
@@ -293,6 +294,9 @@ void Arm::Periodic() {
         case Manual:
             VOKC_CALL(this->ManualControl());
             break;
+        case Test:
+            VOKC_CALL(this->TestControl());
+            break;
         default:
             VOKC_CHECK_MSG(false, "Unhandled enum");
     }

src/main/include/subsystems/Arm.h

@@ -62,6 +62,7 @@ class Arm : public frc2::SubsystemBase {
     ArmControlState control_state_;
     TeamOKC::ArmState state_;
     TeamOKC::ArmState desired_state_;
+    TeamOKC::ArmState test_state_;
 
     // control flags
     bool has_been_commanded_ = false;