Started work on new climb code

RIT-VEX-U · Mar 24, 2024 · b334877 · b334877
1 parent 5f33174
commit b334877
Show file tree

Hide file tree

Showing 7 changed files with 200 additions and 86 deletions.
diff --git a/.vscode/vex_project_settings.json b/.vscode/vex_project_settings.json
@@ -9,7 +9,7 @@
 		"creationDate": "Sun, 10 Mar 2024 19:16:09 GMT",
 		"platform": "V5",
 		"language": "cpp",
-		"slot": 1,
+		"slot": 8,
 		"sdkVersion": "V5_20220726_10_00_00",
 		"cpp": {
 			"includePath": [],

diff --git a/include/cata/cata.h b/include/cata/cata.h
@@ -13,19 +13,24 @@ enum class CataOnlyMessage {
   StartDrop,
   EnableCata,
   DisableCata,
-
+  StartClimb,
+  ClimbReleaseMotors,
+  StopClimb,
+  FinishClimb,
 };
-enum class CataOnlyState { CataOff, WaitingForDrop, Firing, Reloading, ReadyToFire };
+enum class CataOnlyState { CataOff, WaitingForDrop, Firing, Reloading, ReadyToFire, PrimeClimb, ClimbHold };
 std::string to_string(CataOnlyState s);
 std::string to_string(CataOnlyMessage s);
 
-class CataOnlySys : public StateMachine<CataOnlySys, CataOnlyState, CataOnlyMessage, 5, false> {
+class CataOnlySys : public StateMachine<CataOnlySys, CataOnlyState, CataOnlyMessage, 5, true> {
 public:
   friend struct Reloading;
   friend class Firing;
   friend class ReadyToFire;
   friend class WaitingForDrop;
   friend class CataOff;
+  friend class PrimeClimb;
+  friend class ClimbHold;
 
   friend class CataSysPage;
   CataOnlySys(vex::pot &cata_pot, vex::optical &cata_watcher, vex::motor_group &cata_motor, PIDFF &cata_pid,

diff --git a/include/cata/common.h b/include/cata/common.h
@@ -5,8 +5,8 @@ enum class DropMode {
 };
 
 // Intake Params
-const double inake_enable_lower_threshold = 90;
-const double intake_enable_upper_threshold = 110;
+const double inake_enable_lower_threshold = 20;
+const double intake_enable_upper_threshold = 30;
 
 const double intake_upper_outer_volt = 12.0;
 const double intake_lower_outer_volt = 9.0;
@@ -22,8 +22,10 @@ const double intake_sensor_dist_mm = 150;
 const double intake_drop_seconds = 0.5;
 
 // Cata Params
-const double cata_target_charge = 100;
-const double done_firing_angle = 110;
+const double cata_target_charge = 22;
+const double cata_target_climb_primed = 48;
+const double cata_target_climb_up = 25;
+const double done_firing_angle = 50;
 
 const double intake_drop_seconds_until_enable = 0.25;
 const double fire_voltage = 12.0;
diff --git a/include/cata_system.h b/include/cata_system.h
@@ -18,6 +18,9 @@ class CataSys {
     StartDropping,
     StartFiring,
     ToggleCata,
+    StartClimb,
+    StopClimb,
+    FinishClimb,
   };
 
   CataSys(

diff --git a/src/cata/cata.cpp b/src/cata/cata.cpp
@@ -99,11 +99,62 @@ class ReadyToFire : public CataOnlySys::State {
   CataOnlyState id() const override { return CataOnlyState::ReadyToFire; }
 };
 
+class PrimeClimb : public CataOnlySys::State {
+  public:
+
+  void entry(CataOnlySys &sys) override {
+    sys.pid.set_target(cata_target_climb_primed);
+  };
+
+  CataOnlySys::MaybeMessage work(CataOnlySys &sys) override {
+    sys.pid.update(sys.pot.angle(vex::deg));
+    sys.mot.spin(vex::fwd, sys.pid.get(), vex::volt);
+
+    return {};
+  }; 
+
+  void exit(CataOnlySys &sys) override {
+
+  };
+
+  CataOnlyState id() const override {
+    return CataOnlyState::PrimeClimb;
+  };
+
+  CataOnlySys::State *respond(CataOnlySys &s, CataOnlyMessage m) override;
+};
+
+class ClimbHold : public CataOnlySys::State {
+  public:
+  void entry(CataOnlySys &sys) override {
+    sys.pid.set_target(cata_target_climb_up);
+  };
+
+  CataOnlySys::MaybeMessage work(CataOnlySys &sys) override {
+    sys.pid.update(sys.pot.angle(vex::deg));
+    sys.mot.spin(vex::fwd, sys.pid.get(), vex::volt);
+
+    return {};
+  }; 
+
+  void exit(CataOnlySys &sys) override {
+
+  };
+
+  CataOnlyState id() const override {
+    return CataOnlyState::ClimbHold;
+  };
+
+  CataOnlySys::State *respond(CataOnlySys &s, CataOnlyMessage m) override;
+};
+
 CataOnlySys::State *CataOff::respond(CataOnlySys &sys, CataOnlyMessage m) {
   if (m == CataOnlyMessage::EnableCata) {
     return new Reloading();
   } else if (m == CataOnlyMessage::StartDrop) {
     return new WaitingForDrop();
+  } else if (m == CataOnlyMessage::StartClimb) {
+    return new PrimeClimb();
   }
   // Ignore other messages
   return this;
@@ -128,6 +179,8 @@ CataOnlySys::State *Reloading::respond(CataOnlySys &sys, CataOnlyMessage m) {
     } else {
       return this;
     }
+  } else if (m == CataOnlyMessage::StartClimb) {
+    return new PrimeClimb();
   }
   // Ignore other messages
   return this;
@@ -144,6 +197,8 @@ CataOnlySys::State *ReadyToFire::respond(CataOnlySys &sys, CataOnlyMessage m) {
     return new Reloading();
   } else if (m == CataOnlyMessage::DisableCata) {
     return new CataOff();
+  } else if (m == CataOnlyMessage::StartClimb) {
+    return new PrimeClimb();
   }
 
   // Ignore other messages
@@ -159,6 +214,27 @@ CataOnlySys::State *Firing::respond(CataOnlySys &sys, CataOnlyMessage m) {
   // Ignore other messages
   return this;
 }
+
+CataOnlySys::State *PrimeClimb::respond(CataOnlySys &s, CataOnlyMessage m) {
+  switch(m) {
+    case CataOnlyMessage::StopClimb:
+      return new Reloading();
+    case CataOnlyMessage::FinishClimb:
+      return new ClimbHold();
+    default:
+      return this;
+  }
+};
+
+CataOnlySys::State *ClimbHold::respond(CataOnlySys &s, CataOnlyMessage m) {
+  switch(m) {
+    case CataOnlyMessage::StopClimb:
+      return new Reloading();
+    default:
+      return this;
+  }
+};
+
 std::string to_string(CataOnlyState s) {
   switch (s) {
   case CataOnlyState::Firing:
@@ -171,6 +247,10 @@ std::string to_string(CataOnlyState s) {
     return "CataOff";
   case CataOnlyState::WaitingForDrop:
     return "WaitingForDrop";
+  case CataOnlyState::PrimeClimb:
+    return "PrimeClimb";
+  case CataOnlyState::ClimbHold:
+    return "ClimbHold";
   default:
     return "UNKNOWN CATA STATE";
   }
@@ -193,6 +273,12 @@ std::string to_string(CataOnlyMessage m) {
     return "DisableCata";
   case CataOnlyMessage::StartDrop:
     return "StartDrop";
+  case CataOnlyMessage::StartClimb:
+    return "StartClimb";
+  case CataOnlyMessage::StopClimb:
+    return "StopClimb";
+  case CataOnlyMessage::FinishClimb:
+    return "FinishClimb";
   }
   return "UNHANDLED CATA MESSAGE";
 }

diff --git a/src/cata_system.cpp b/src/cata_system.cpp
@@ -49,6 +49,15 @@ void CataSys::send_command(Command next_cmd) {
       cata_sys.send_message(CataOnlyMessage::DisableCata);
     }
     break;
+  case CataSys::Command::StartClimb:
+    cata_sys.send_message(CataOnlyMessage::StartClimb);
+    break;
+  case CataSys::Command::StopClimb:
+    cata_sys.send_message(CataOnlyMessage::StopClimb);
+    break;
+  case CataSys::Command::FinishClimb:
+    cata_sys.send_message(CataOnlyMessage::FinishClimb);
+    break;
   default:
     break;
   }