From fdce6c75c74814f9162191dbe8c789c777c1992c Mon Sep 17 00:00:00 2001
From: ary <cjansah11@gmail.com>
Date: Wed, 27 Sep 2023 10:11:47 -0400
Subject: [PATCH] implement superstructure base

---
 RELENTLESS/include/superstructure.hpp | 21 +++++++++++
 RELENTLESS/src/superstructure.cpp     | 51 +++++++++++++++++++++++++++
 2 files changed, 72 insertions(+)

diff --git a/RELENTLESS/include/superstructure.hpp b/RELENTLESS/include/superstructure.hpp
index eea21d0..6741f71 100644
--- a/RELENTLESS/include/superstructure.hpp
+++ b/RELENTLESS/include/superstructure.hpp
@@ -1,3 +1,24 @@
+#include "main.h"
+#include "ary-lib/api.hpp"
+#include "globals.hpp"
+
+#define DRIVE_SPEED 110
+#define TURN_SPEED 90
+#define SWING_SPEED 90
+
 namespace superstruct {
+    //configs
+    void configureExitConditions();
+    void configureConstants();
+    void motorsCoast();
+    void motorsHold();
+    void motorsBrake();
+    
+
+    // Movement Methods
+    void driveChassis(double dist, double speedScale, bool useHeadingCorrection);
+    void turnChassis(double theta, double turnSpeedScale);
+    void leftSwing(double theta, double swingSpeedScale);
+    void rightSwing(double theta, double swingSpeedScale);
     
 }
\ No newline at end of file
diff --git a/RELENTLESS/src/superstructure.cpp b/RELENTLESS/src/superstructure.cpp
index e69de29..b6e578d 100644
--- a/RELENTLESS/src/superstructure.cpp
+++ b/RELENTLESS/src/superstructure.cpp
@@ -0,0 +1,51 @@
+#include "superstructure.hpp"
+
+using namespace ary;
+using namespace globals;
+namespace superstruct {
+  void configureExitConditions() {
+    chassis.set_exit_condition(chassis.turn_exit, 100, 3, 500, 7, 500, 500);
+    chassis.set_exit_condition(chassis.swing_exit, 100, 3, 500, 7, 500, 500);
+    chassis.set_exit_condition(chassis.drive_exit, 80, 50, 300, 150, 500, 500);
+  }
+
+  void configureConstants() {
+    chassis.set_slew_min_power(80, 80);
+    chassis.set_slew_distance(7, 7);
+    chassis.set_pid_constants(&chassis.headingPID, 11, 0, 20, 0);
+    chassis.set_pid_constants(&chassis.forward_drivePID, 0.45, 0, 5, 0);
+    chassis.set_pid_constants(&chassis.backward_drivePID, 0.45, 0, 5, 0);
+    chassis.set_pid_constants(&chassis.turnPID, 4, 0.003, 35, 15);
+    chassis.set_pid_constants(&chassis.swingPID, 6, 0, 40, 0);
+  }
+
+  void motorsCoast() {
+    chassis.set_drive_brake(MOTOR_BRAKE_COAST);
+  }
+
+  void motorsHold() {
+    chassis.set_drive_brake(MOTOR_BRAKE_HOLD);
+  }
+
+  void motorsBrake() {
+    chassis.set_drive_brake(MOTOR_BRAKE_BRAKE);
+  }
+
+
+  // motion and stuff
+  void driveChassis(double dist, double speedScale, bool useHeadingCorrection) {
+    chassis.set_drive(dist, DRIVE_SPEED * speedScale, (dist > 14.0) ? true : false, useHeadingCorrection); 
+  }
+
+  void turnChassis(double theta, double turnSpeedScale) {
+    chassis.set_turn(theta, TURN_SPEED * turnSpeedScale);
+  }
+
+  void leftSwing(double theta, double swingSpeedScale) {
+    chassis.set_swing(LEFT_SWING, theta, SWING_SPEED * swingSpeedScale);
+  }
+
+  void rightSwing(double theta, double swingSpeedScale) {
+    chassis.set_swing(RIGHT_SWING, theta, SWING_SPEED * swingSpeedScale);
+  }
+}
\ No newline at end of file