move motors when priming cata to get full extension

RIT-VEX-U · Apr 4, 2024 · 1ebdf3f · 1ebdf3f
1 parent 4b5335b
commit 1ebdf3f
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 1 deletion.
diff --git a/include/cata/common.h b/include/cata/common.h
@@ -23,6 +23,7 @@ const double intake_drop_seconds = 0.5;
 
 // Cata Params
 const double cata_target_charge = 22;
+const double cata_target_extension = 65;
 const double done_firing_angle = 50;
 
 const double intake_drop_seconds_until_enable = 0.25;

diff --git a/src/cata/cata.cpp b/src/cata/cata.cpp
@@ -123,6 +123,12 @@ class PrimeClimb : public CataOnlySys::State {
   };
 
   CataOnlySys::MaybeMessage work(CataOnlySys &sys) override {
+    double ang = sys.pot.angle(vex::deg);
+
+    if(ang < cata_target_extension)
+      sys.mot.spin(vex::directionType::fwd, 12, vex::volt);
+    else
+      sys.mot.stop(vex::brakeType::hold);
 
     return {};
   }; 
@@ -186,7 +192,7 @@ CataOnlySys::State *WaitingForDrop::respond(CataOnlySys &sys, CataOnlyMessage m)
   // Ignore other messages
   return this;
 }
-
+ 
 CataOnlySys::State *Reloading::respond(CataOnlySys &sys, CataOnlyMessage m) {
   if (m == CataOnlyMessage::DoneReloading) {
     return new ReadyToFire();