Added stabilizer solenoid & logic

RIT-VEX-U · Apr 7, 2024 · bdbd87b · bdbd87b
1 parent 1a771dc
commit bdbd87b
Show file tree

Hide file tree

Showing 6 changed files with 18 additions and 13 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": 7,
+		"slot": 8,
 		"sdkVersion": "V5_20240223_11_00_00",
 		"cpp": {
 			"includePath": [],

diff --git a/include/cata/cata.h b/include/cata/cata.h
@@ -1,3 +1,4 @@
+#pragma once
 #include "../core/include/utils/controls/pidff.h"
 #include "cata/common.h"
 #include "vex.h"

diff --git a/include/robot-config.h b/include/robot-config.h
@@ -37,10 +37,11 @@ extern OdometryTank odom;
 extern TankDrive drive_sys;
 
 extern CataSys cata_sys;
-extern digital_out left_wing;
-extern digital_out right_wing;
+extern pneumatics left_wing;
+extern pneumatics right_wing;
 
 extern digital_out vision_light;
+extern pneumatics stabilizer_sol;
 extern pneumatics l_endgame_sol;
 extern pneumatics r_endgame_sol;
 extern pneumatics cata_sol;

diff --git a/src/cata/cata.cpp b/src/cata/cata.cpp
@@ -1,4 +1,5 @@
 #include "cata/cata.h"
+#include "robot-config.h"
 
 bool intake_can_be_enabled(double cata_pos) {
   return (cata_pos == 0.0) || (cata_pos > inake_enable_lower_threshold && cata_pos < intake_enable_upper_threshold);
@@ -119,6 +120,7 @@ class PrimeClimb : public CataOnlySys::State {
   void entry(CataOnlySys &sys) override {
     sys.cata_sol.open();
     sys.mot.stop(vex::coast);
+    stabilizer_sol.open();
   };
 
   CataOnlySys::MaybeMessage work(CataOnlySys &sys) override {
@@ -149,6 +151,7 @@ class ClimbHold : public CataOnlySys::State {
     sys.l_endgame_sol.open();
     sys.r_endgame_sol.open();
     sys.cata_sol.close();
+    stabilizer_sol.close();
   };
 
   CataOnlySys::MaybeMessage work(CataOnlySys &sys) override {

diff --git a/src/competition/opcontrol.cpp b/src/competition/opcontrol.cpp
@@ -195,8 +195,8 @@ void testing() {
     // if (!disable_drive)
     //   drive_sys.drive_tank(f, s, 1, TankDrive::BrakeType::None);
     if (!disable_drive)
-      drive_tank_autoaim(TankDrive::BrakeType::None);
-      // drive_sys.drive_tank(l, r, 1, TankDrive::BrakeType::None);
+      // drive_tank_autoaim(TankDrive::BrakeType::None);
+      drive_sys.drive_tank(l, r, 1, TankDrive::BrakeType::None);
 
     // ================ Drive Tuning =================
     static bool done_a = false;

diff --git a/src/robot-config.cpp b/src/robot-config.cpp
@@ -36,17 +36,16 @@ motor intake_roller(PORT9, gearSetting::ratio18_1, false);
 motor cata_r(PORT2, gearSetting::ratio36_1, true);
 motor cata_l(PORT10, gearSetting::ratio36_1, false);
 
-vex::digital_out left_wing(Brain.ThreeWirePort.B); // Check if swapped
-vex::digital_out right_wing(Brain.ThreeWirePort.F);
+// ================ Three-Wire Ports ==============
+vex::pneumatics left_wing(Brain.ThreeWirePort.A); 
+vex::pneumatics right_wing(Brain.ThreeWirePort.F);
 
-// endgame_sol output 1 to CLIMB pistons
-// endgame_sol output 2 to cata_sol (single acting)
-// cata_sol output 1 to catapult assist pistons
+vex::pneumatics stabilizer_sol(Brain.ThreeWirePort.C);
 vex::pneumatics l_endgame_sol(Brain.ThreeWirePort.G);
-vex::pneumatics r_endgame_sol(Brain.ThreeWirePort.A);
-vex::pneumatics cata_sol(Brain.ThreeWirePort.D);
+vex::pneumatics r_endgame_sol(Brain.ThreeWirePort.D);
+vex::pneumatics cata_sol(Brain.ThreeWirePort.B);
 
-vex::digital_out vision_light(Brain.ThreeWirePort.C);
+vex::digital_out vision_light(Brain.ThreeWirePort.E);
 
 #else
 // NEMO CONFIG
@@ -224,6 +223,7 @@ void robot_init() {
   l_endgame_sol.set(false);
   r_endgame_sol.set(false);
   cata_sol.set(false);
+  stabilizer_sol.close();
 
   gps_sensor.calibrate();
 }