Climb is restarting! D: almost fixed...

Needs one more test and maybe more tweaks
Current fix is a HasClimbed bool

Author: FaceInCake

replay_pid140352.log

@@ -8,6 +8,7 @@
 #include "Autonomous/AutoClimbHigh.h"
 #include "Robot.h"
 
+
 AutoClimbHigh::AutoClimbHigh() {
 	Requires(Robot::m_Arm);
 	Requires(Robot::m_Leg);
@@ -16,6 +17,7 @@ AutoClimbHigh::AutoClimbHigh() {
 	this->pTimer = new frc::Timer();
 	this->stage = S_LOWER_ARM;
 	this->onFloor = true;
+    this->hasClimbed = false;
 }
 
 void AutoClimbHigh::Initialize() {
@@ -27,14 +29,16 @@ void AutoClimbHigh::Initialize() {
 
 void AutoClimbHigh::Execute() {
 	// Execute the appropriate function for the current stage of climb
-	switch (this->stage) {
-		case S_LOWER_ARM : this->Execute_LowerArm() ; break;
-		case S_LOWER_LEG : this->Execute_LowerLeg() ; break;
-		case S_CRAWL	 : this->Execute_Crawl()	; break;
-		case S_DRIVE	 : this->Execute_Drive()	; break;
-		case S_RAISE_LEG : this->Execute_Raiseleg() ; break;
-		default : this->stage = S_FINISHED ; break;
-	}
+    if (!this->hasClimbed) {
+        switch (this->stage) {
+            case S_LOWER_ARM : this->Execute_LowerArm() ; break;
+            case S_LOWER_LEG : this->Execute_LowerLeg() ; break;
+            case S_CRAWL	 : this->Execute_Crawl()	; break;
+            case S_DRIVE	 : this->Execute_Drive()	; break;
+            case S_RAISE_LEG : this->Execute_Raiseleg() ; break;
+            default : this->stage = S_FINISHED ; break;
+        }
+    }
 }
 
 void AutoClimbHigh::Execute_LowerArm(void) {
@@ -127,6 +131,8 @@ void AutoClimbHigh::End() {
 	Robot::m_Leg->MoveLeg(0.0);
 	Robot::m_DriveTrain->TankDrive(0.0, 0.0);
 	Robot::m_CrawlDrive->Move(0.0);
+    ClimbManager::CurrentClimbState = ClimbManager::ClimbState::kInactive;
+    this->hasClimbed = true;
 	this->pTimer->Stop();
 }
 
@@ -136,5 +142,7 @@ void AutoClimbHigh::Interrupted() {
 	Robot::m_Leg->MoveLeg(0.0);
 	Robot::m_DriveTrain->TankDrive(0.0, 0.0);
 	Robot::m_CrawlDrive->Move(0.0);
+    ClimbManager::CurrentClimbState = ClimbManager::ClimbState::kInactive;
+    this->hasClimbed = true;
 	this->pTimer->Stop();
 }

replay_pid93516.log

@@ -206,10 +206,14 @@ void Robot::SharedPeriodic(){
       this->pClimb->Start();
     }
   }
-  if (ClimbManager::CurrentClimbSate == ClimbManager::ClimbSate::kAuto) {
+  if (ClimbManager::CurrentClimbState == ClimbManager::ClimbState::kAuto) {
     // Start the auto climb
-    if (this->pAutoClimbHigh != nullptr) {
-      this->pAutoClimbHigh->Start();
+    if (this->pAutoClimbHigh->hasClimbed == false) {
+        if (this->pAutoClimbHigh != nullptr) {
+            this->pAutoClimbHigh->Start();
+        }
+    } else {
+        ClimbManager::CurrentClimbState = ClimbManager::ClimbState::kInactive;
     }
   }
 

replay_pid94836.log

@@ -11,6 +11,8 @@
 #include <frc/commands/Command.h>
 #include <frc/Timer.h>
 
+
+
 class AutoClimbHigh : public frc::Command {
 public:
 	AutoClimbHigh();
@@ -24,6 +26,8 @@ class AutoClimbHigh : public frc::Command {
 	void Execute_Crawl(void);		//!< Execute stage CRAWL stuff
 	void Execute_Drive(void);		//!< Execute stage DRIVE stuff
 	void Execute_Raiseleg(void);	//!< Execute stage RAISE_LEG stuff
+
+    bool hasClimbed ;
 private:
 	/**
 	 * @brief Enum for tracking which stage of climb the robot is on