Complete Autonomous From Sig

include/globals.hpp

@@ -41,7 +41,7 @@ inline sylib::SpeedControllerInfo flywheel_speed_controller(
 );
 
 inline auto flywheel_motor =
-    std::make_shared<sylib::Motor>(5, 600, true, flywheel_speed_controller);
+    std::make_shared<sylib::Motor>(4, 600, true, flywheel_speed_controller);
 
 inline lib::Flywheel flywheel(flywheel_motor, flywheel_led);
 
@@ -59,7 +59,7 @@ inline lib::Intake intake(intake_motor);
 
 // Drivetrain
 inline pros::Motor_Group left_motors({-11, -19, 20});
-inline pros::Motor_Group right_motors({1, 8, -10});
+inline pros::Motor_Group right_motors({1, 8, -9});
 
 inline lemlib::Drivetrain drivetrain{
     &left_motors,   // left drivetrain motors

include/lib/utils/auton_selector.hpp

@@ -4,7 +4,7 @@
 
 // selector configuration
 #define HUE 120  // color of theme from 0-360
-#define DEFAULT 1
+#define DEFAULT 2
 #define AUTONS "Offensive", "Defensive", "Skills", "Nothing"
 
 namespace lib {

src/auton.cpp

@@ -7,7 +7,7 @@
 #include "lib/subsystems/intake.hpp"
 #include "lib/utils/auton_selector.hpp"
 #include "main.h"
-#include "pros/rtos.hpp"
+#include "pros/rtos.hpp" 
 
 /**
  * Runs the user autonomous code. This function will be started in its own task
@@ -28,22 +28,22 @@ void autonomous() {
     case 1:  // Offensive side
 
       hang.set_state(lib::HangState::Expanded); //Release Intake
-      pros::delay(2000);
       chassis.moveToPoint(0, 35, 2000); //Move In front Of goal
       chassis.waitUntilDone();
       chassis.turnTo(20, 45, 1000); //turn to goal
       intake.set_state(lib::IntakeState::Reversed); //outtake preload triball
       pros::delay(1000);
-      chassis.turnTo(-30,25, 1000); //turn to middle bar triball 
-      chassis.moveToPoint(-30, 23, 2000);
+      intake.set_state(lib::IntakeState::Idle);
+      chassis.turnTo(-30,29, 1000); //turn to middle bar triball 
+      chassis.moveToPoint(-30, 25, 2000);
       chassis.waitUntilDone();
       intake.set_state(lib::IntakeState::Running); //intake middle bar triball
       chassis.turnTo(0, 48, 2000); //turn towards goal
-      pros::delay(1000);
+      chassis.moveToPoint(-20, 40 ,1000);
       intake.set_state(lib::IntakeState::Reversed); //outake middle bar triball 
       pros::delay(1000);
       intake.set_state(lib::IntakeState::Idle);
-      chassis.moveToPoint(-40, 48,1000); //move to middle middle bar triball
+      chassis.moveToPoint(-40, 47,1000); //move to middle middle bar triball
       chassis.waitUntilDone();
       intake.set_state(lib::IntakeState::Running); //intake this triball
       pros::delay(2000);
@@ -52,8 +52,7 @@ void autonomous() {
       flaps.set_state(lib::FlapState::Expanded); //expand flaps
       intake.set_state(lib::IntakeState::Reversed);
       pros::delay(500);
-      chassis.moveToPoint(24, 48, 3000); //push in all four triballs 
-      chassis.moveToPoint(10, 48, 1000, false); //move backwards 
+      chassis.moveToPoint(14, 48, 2000); //push in all four triballs 
       chassis.waitUntilDone();
       flaps.set_state(lib::FlapState::Idle);
 
@@ -78,53 +77,67 @@ void autonomous() {
     //winpoint code 
       hang.set_state(lib::HangState::Expanded); //Release Intake
       pros::delay(2000);
+      chassis.turnTo(-45, 45, 1000);
+      chassis.moveToPoint(-10, 10, 1000); //unload triball from matchload zone
+      chassis.waitUntilDone();
       flaps.set_state(lib::FlapState::Expanded);
-      pros::delay(2000);
-      chassis.moveToPoint(0, 25, 1000); //unload triball from matchload zone
+      chassis.moveToPoint(8, 0, 2000, false); //move to starting points
       chassis.waitUntilDone();
       flaps.set_state(lib::FlapState::Idle);
-      chassis.moveToPoint(0, 0, 2000, false); //move to starting point
-      chassis.turnTo(36, -35, 1000);
+      chassis.moveToPoint(-10, -10, 1500);
+      chassis.waitUntilDone();
+      chassis.turnTo(36, -15, 1000);
       pros::delay(2000);
-      chassis.moveToPoint(36, -35, 390); //touch bar w ziptie
+      chassis.moveToPoint(30, -8, 1000); //touch bar w ziptie
 
 
 
       break;
     case 3:  // Skills
-
       hang.set_state(lib::HangState::Expanded);  
       chassis.moveToPoint(-20, 0, 1000);
       chassis.turnTo(24, 24, 1000);
-      chassis.moveToPoint(-20, 0, 1000, false); //back into zone for matchloading
+      chassis.moveToPoint(-17, 0, 1000, false); //back into zone for matchloading
       chassis.waitUntilDone();
       flywheel.set_state(lib::FlywheelState::Spinning);
       pros::delay(30000);
       chassis.waitUntilDone();
       hang.set_state(lib::HangState::Idle);
-      chassis.moveToPoint(0, 0, 2000);//move back to starting point
+      flywheel.set_state(lib::FlywheelState::Idle);
+      chassis.turnTo(0, -6, 1000);
+      chassis.moveToPoint(0, -6, 2000);//move back to starting point
       chassis.waitUntilDone();
       chassis.turnTo(60, 0, 1000);//turn towards other side
-      chassis.moveToPoint(70, 0, 4000);//move under bar to other side
-      chassis.turnTo(80, 45, 1000);//turn to goal
+      chassis.moveToPoint(75, 0, 4000);//move under bar to other side
+      chassis.turnTo(88, 45, 1000);//turn to goal
+      chassis.waitUntilDone(); 
+      chassis.moveToPose(91, 34, -90, 1500);//push into goal from side
       chassis.waitUntilDone();
-      flaps.set_state(lib::FlapState::Expanded);//open wings
-      chassis.moveToPoint(82, 30, 2000);//push into goal from side
-      chassis.waitUntilDone();
-      flaps.set_state(lib::FlapState::Idle);//close flaps
-      chassis.moveToPoint(80, 10, 2000, false);//move backwards
+      chassis.moveToPose(88, 5, 0, 2000, {.forwards=false});//move backwards
+      chassis.moveToPose(88, 50, 0, 2000);//push into goal from side
+      chassis.moveToPose(88, 0, 0, 2000, {.forwards=false});//move backwards
+      chassis.turnTo(55, 40, 1000);
       chassis.moveToPoint(55, 45, 3000); //move to middle bar 
       chassis.turnTo(85, 45, 1000);//face goal
       chassis.waitUntilDone();
       flaps.set_state(lib::FlapState::Expanded);//open wings
       chassis.moveToPoint(85, 45, 2000);//push into goal
-      chassis.moveToPoint(60, 45, 2000, false);//move back
+      chassis.moveToPoint(50, 45, 2000, false);//move back
       flaps.set_state(lib::FlapState::Idle);//close wings
-      chassis.moveToPoint(60, 50, 2000);//move left to other side of goal
-      chassis.turnTo(70, 48, 1000); //turn to goal
+      chassis.moveToPoint(60, 70, 2000);//move left to other side of goal
+      chassis.turnTo(70, 70, 1000); //turn to goal
       chassis.waitUntilDone();
       flaps.set_state(lib::FlapState::Expanded);//open wings
-      chassis.moveToPoint(85, 48, 2000);//push into goal
+      chassis.moveToPoint(75, 70, 2000);//push into goal
+      chassis.waitUntilDone();
+      chassis.moveToPoint(60, 65, 1500, false);
+
+      /*
+      chassis.moveToPoint(60, 55, 200
+      0, false);
+      flaps.set_state(lib::FlapState::Idle);
+      chassis.turnTo(55, 75, )
+      */
 
 
 

src/misc.cpp

@@ -32,9 +32,9 @@ void initialize() {
   flaps.start_task();
   hang.start_task();
 
-  logger.start_task();
-  logger.set_rate(10);
-  logger.set_state(lib::LoggerMode::FirstInFirstOut);
+  //logger.start_task();
+  // logger.set_rate(10);
+  // logger.set_state(lib::LoggerMode::FirstInFirstOut);
 
   led.gradient(0x8B3AFD, 0x7DFF29, 0, 0, false, true);
   led.cycle(*led, 1);

src/opcontrol.cpp

@@ -1,8 +1,10 @@
 #include <cmath>
 
 #include "globals.hpp"
+#include "lib/subsystems/flywheel.hpp"
 #include "lib/subsystems/intake.hpp"
 #include "main.h"
+#include "pros/misc.h"
 
 /**
  * Runs the operator control code. This function will be started in its own task
@@ -18,6 +20,8 @@
  * task, not resume it from where it left off.
  */
 void opcontrol() {
+  printf("opcontrol started\n");
+
   bool is_drive_reversed = false;
   hang.set_state(lib::HangState::Expanded);
   while (true) {
@@ -48,6 +52,12 @@ void opcontrol() {
       controller.rumble(".");
       flywheel.toggle();
     }
+
+    // Flywheel control
+    if (controller.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_LEFT)) {
+      controller.rumble(".");
+      flywheel.set_state(lib::FlywheelState::Reversed);
+    }
 
     /*
     if (controller.get_digital(pros::E_CONTROLLER_DIGITAL_LEFT)) {
@@ -74,4 +84,4 @@ void opcontrol() {
 
     pros::delay(20);
   }
-}
+}