From f027da53a2f392d0510153b1bfc38791ca1c0fcb Mon Sep 17 00:00:00 2001
From: ary <cjansah11@gmail.com>
Date: Sat, 9 Sep 2023 13:07:51 -0400
Subject: [PATCH] do more stuff on scrim bot

this is terrible, bad, terrible, and abominable code
(it has odom implemented)
---
 RELENTLESS-Lite/include/autons.hpp         |  8 ++++
 RELENTLESS-Lite/include/globals.hpp        | 24 +++++++++++
 RELENTLESS-Lite/include/superstructure.hpp |  3 ++
 RELENTLESS-Lite/src/autons.cpp             | 46 ++++++++++++++++++++++
 RELENTLESS-Lite/src/globals.cpp            | 43 ++++++++++++++++++++
 RELENTLESS-Lite/src/superstructure.cpp     |  0
 6 files changed, 124 insertions(+)
 create mode 100644 RELENTLESS-Lite/include/autons.hpp
 create mode 100644 RELENTLESS-Lite/include/globals.hpp
 create mode 100644 RELENTLESS-Lite/include/superstructure.hpp
 create mode 100644 RELENTLESS-Lite/src/autons.cpp
 create mode 100644 RELENTLESS-Lite/src/globals.cpp
 create mode 100644 RELENTLESS-Lite/src/superstructure.cpp

diff --git a/RELENTLESS-Lite/include/autons.hpp b/RELENTLESS-Lite/include/autons.hpp
new file mode 100644
index 0000000..5b293fa
--- /dev/null
+++ b/RELENTLESS-Lite/include/autons.hpp
@@ -0,0 +1,8 @@
+#pragma once
+
+#include "arylib/drive/drive.hpp"
+
+void test_auton();
+
+void default_constants();
+void exit_condition_defaults();
\ No newline at end of file
diff --git a/RELENTLESS-Lite/include/globals.hpp b/RELENTLESS-Lite/include/globals.hpp
new file mode 100644
index 0000000..6253e11
--- /dev/null
+++ b/RELENTLESS-Lite/include/globals.hpp
@@ -0,0 +1,24 @@
+#include "main.h"
+
+#define TRACK_WIDTH 11.5;
+
+namespace globals {
+    extern pros::Controller master;
+
+    extern pros::Motor motor_fl;
+    extern pros::Motor motor_ml;
+    extern pros::Motor motor_bl;
+    extern pros::Motor motor_fr;
+    extern pros::Motor motor_mr;
+    extern pros::Motor motor_br;
+
+    extern pros::Motor_Group left_drive;
+    extern pros::Motor_Group right_drive;
+
+    extern lemlib::Drivetrain_t chassis_odom;
+    extern Drive chassis;
+
+    extern pros::Rotation enc_left;
+    extern pros::Rotation enc_right;
+    extern pros::Rotation enc_theta;
+}
\ No newline at end of file
diff --git a/RELENTLESS-Lite/include/superstructure.hpp b/RELENTLESS-Lite/include/superstructure.hpp
new file mode 100644
index 0000000..eea21d0
--- /dev/null
+++ b/RELENTLESS-Lite/include/superstructure.hpp
@@ -0,0 +1,3 @@
+namespace superstruct {
+    
+}
\ No newline at end of file
diff --git a/RELENTLESS-Lite/src/autons.cpp b/RELENTLESS-Lite/src/autons.cpp
new file mode 100644
index 0000000..c56f461
--- /dev/null
+++ b/RELENTLESS-Lite/src/autons.cpp
@@ -0,0 +1,46 @@
+#include <stdlib.h>
+
+#include "main.h"
+#include "globals.hpp"
+#include "superstructure.hpp"
+
+using namespace globals;
+using namespace superstruct;
+
+// **CONSTANTS** //
+const int DRIVE_SPEED = 110;
+const int TURN_SPEED  = 90;
+const int SWING_SPEED = 90;
+
+void default_constants() {
+  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 exit_condition_defaults() {
+  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);
+}
+
+// ** other stufff** //
+
+// ** AUTONS ** //
+
+void test_auton() {
+    chassis.set_drive_pid(5, DRIVE_SPEED, false, true);
+    chassis.set_swing_pid(e_swing::LEFT_SWING, 50, TURN_SPEED);
+    chassis.set_drive_pid(36, DRIVE_SPEED, true, true);
+    chassis.wait_until(24);
+    /* RUN THE INTAKE */
+    pros::delay(500);
+    chassis.set_drive_pid(-2, DRIVE_SPEED, false, false);
+    chassis.set_turn_pid(130, TURN_SPEED);
+
+  //pros::task_t intakeTask(run_intake_for, (void*) malloc(sizeof(double)), TASK_PRIORITY_DEFAULT, TASK_STACK_DEPTH_DEFAULT, "runIntakeFor");
+}
\ No newline at end of file
diff --git a/RELENTLESS-Lite/src/globals.cpp b/RELENTLESS-Lite/src/globals.cpp
new file mode 100644
index 0000000..e57da87
--- /dev/null
+++ b/RELENTLESS-Lite/src/globals.cpp
@@ -0,0 +1,43 @@
+#include "globals.hpp"
+
+using namespace ary;
+namespace globals {
+    // Chassis
+    pros::Controller master(CONTROLLER_MASTER);
+
+    pros::Motor motor_fl(4, MOTOR_GEARSET_6, true);
+    pros::Motor motor_ml(4, MOTOR_GEARSET_6, true);
+    pros::Motor motor_bl(4, MOTOR_GEARSET_6, true);
+    pros::Motor motor_fr(4, MOTOR_GEARSET_6, true);
+    pros::Motor motor_mr(4, MOTOR_GEARSET_6, true);
+    pros::Motor motor_br(4, MOTOR_GEARSET_6, true);
+
+    pros::Motor_Group left_drive({ motor_fl, motor_ml, motor_bl });
+    pros::Motor_Group right_drive({ motor_fr, motor_mr, motor_br });
+
+    lemlib::Drivetrain_t chassis_odom {
+        &left_drive,
+        &right_drive,
+        11.5,
+        2.75, 
+        450
+    };
+
+    Drive chassis(
+        {-4,-4,-4,-4},
+        {1,1,1,1},
+        16,
+        3.125,
+        600,
+        0.6
+    ); 
+
+    
+    // Electronics / Pneumatics / Sensors
+    // pros::Rotation enc_left();
+    // pros::Rotation enc_right();
+    // pros::Rotation enc_theta();
+
+    // Misc
+
+}
\ No newline at end of file
diff --git a/RELENTLESS-Lite/src/superstructure.cpp b/RELENTLESS-Lite/src/superstructure.cpp
new file mode 100644
index 0000000..e69de29