Coding

1. #include "vex.h"

2. #include "libraries/include.h"

3.  

4. using namespace vex;

5. using signature = vision::signature;

6. using code = vision::code;

7.  

8. brain  Brain;

9.  

10. //? Other stuff

11. inertial Inertial(PORT1);

12. rotation AutonSelector(PORT12);

13. digital_out Claw(Brain.ThreeWirePort.A);

14.  

15. //? -----------

16.  

17. /* #region Controller Stuff */

18. controller ControllerPrimary(primary);

19. controller ControllerPartner(partner);

20.  

21. bool clawToggling = false;

22. bool hookToggling = false;

23. /* #endregion */

24.  

25. /* #region Drivetrain */

26. bool disableBrake = false;

27.  

28. motor RM1(PORT15, ratio6_1, false); // A

29. motor RM2(PORT16, ratio6_1, false); // B

30. motor RM3(PORT17, ratio6_1, false); // C

31.  

32. motor LM1(PORT18, ratio6_1, true); // D

33. motor LM2(PORT19, ratio6_1, true); // E

34. motor LM3(PORT20, ratio6_1, true); // F

35.  

36. motor_group RMG(RM1, RM2, RM3); // Right motor group

37. motor_group LMG(LM1, LM2, LM3); // Left motor group

38.  

39. smartdrive Drivetrain(LMG, RMG, Inertial, 12.59, 12.5, 10, distanceUnits::in, -0.4375);

40. /* #endregion */

41.  

42. /* #region Drive Controllers */

43. controller* DriveController = &ControllerPrimary;

44. controller* NonDriveController = &ControllerPartner;

45.  

46. double driveMultiplier = 50;

47. double driveMultiplierDecimal = driveMultiplier/100;

48.  

49. double driveAxis3 = 0; // Forward and backward

50. double driveAxis1 = 0; // Left and right

51. /* #endregion */

52.  

53. /* #region Intake */

54. motor Intake(PORT11, ratio18_1, true);

55. // For switching intake prority

56. controller* MainIntakeController = &ControllerPartner;

57. controller* SecondaryIntakeController = &ControllerPrimary;

58.  

59. double mainIntakeAxis2 = 0;

60. double secondaryIntakeAxis2 = 0;

61. /* #endregion */

62.  

63. /* #region Hook */

64. motor Hook(PORT2, ratio36_1, false);

65. PIDController hookPID(0.6, 0, 0);

66. double hookPID_dt = 0.1;

67.  

68. double hookDown = 136;

69. double hookUp = 30;

70. double hookMid = 70;

71.  

72. double hookTarget = hookUp;

73.  

74. bool doResetHook = false;

75. bool resetStarted = false;

76. /* #endregion */

77.  

78.  

79. void vexcodeInit( void ) {

80.     Inertial.startCalibration(3000);

81.     while (Inertial.isCalibrating())

82.     {

83.         wait(10, msec);

84.     }

85. }

1. #include "vex.h"

2. #include "include.h"

3.  

4. #include <iostream>

5. #include <map>

6.  

7.  

8. using namespace vex;

9.  

10. competition Competition;

11.  

12. int autonMode = 0;

13.  

14. void pre_auton(void) {

15.     double autonPosition = AutonSelector.angle(deg);

16.     autonPosition = 360 - autonPosition;

17.  

18.     autonMode = ceil(autonPosition / 60 - 0.5) + 1;

19.     autonMode = autonMode == 7 ? 1 : autonMode;

20.  

21.    

22.     std::cout << "Auton Mode Selected: " <<to_string(autonMode) << std::endl;

23. }

24.  

25. void autonomous(void) {

26.     pre_auton();

27.     disableBrake = true;

28.     switch (autonMode) {

29.         case 1:

30.             auton1();

31.             break;

32.         case 2:

33.             auton2();

34.             break;

35.         case 3:

36.             auton3();

37.             break;

38.         case 4:

39.             auton4();

40.             break;

41.         case 5:

42.             auton5();

43.             break;

44.         case 6:

45.             auton6();

46.             break;

47.     }

48.     disableBrake = false;

49. }

50.  

51.  

52. void usercontrol(void) {

53.     disableBrake = false;

54.     task controllerHandlerTask(controllerHandlerFunction);

55.     while (1) {

56.         this_thread::sleep_for(10);

57.     }

58. }

59.  

60. int main() {

61.     vexcodeInit();

62.    

63.     Competition.autonomous(autonomous);

64.     Competition.drivercontrol(usercontrol);

65.  

66.     pre_auton();

67.  

68.     while (true) {

69.         this_thread::sleep_for(20);

70.     }

71. }

72.  

1. #include "vex.h"

2. #include "robot-config.h"

3. #include "include.h"

4.  

5. int controllerHandlerFunction( void ) {

6.     doResetHook = true;

7.     while(1) {

8.         controllerDriveControlCheck();

9.         controllerDrive();

10.  

11.         intakeControl();

12.         hookControl();

13.  

14.         if (MainIntakeController->ButtonX.pressing() ||

15.         SecondaryIntakeController->ButtonX.pressing()) {

16.             if (!clawToggling) {

17.                 clawToggling = true;

18.                 Claw.set(!Claw.value());

19.             }

20.         } else clawToggling = false;

21.  

22.         if (ControllerPrimary.ButtonDown.pressing() ||

23.         ControllerPartner.ButtonDown.pressing()) {

24.             if (!hookToggling) {

25.                 hookToggling = true;

26.                 hookTarget = hookUp;

27.             }

28.         } else hookToggling = false;

29.  

30.         if (ControllerPrimary.ButtonUp.pressing() ||

31.         ControllerPartner.ButtonUp.pressing()) {

32.             if (!hookToggling) {

33.                 hookToggling = true;

34.                 hookTarget = hookDown;

35.             }

36.         } else hookToggling = false;

37.  

38.         if (ControllerPrimary.ButtonRight.pressing() ||

39.         ControllerPartner.ButtonRight.pressing()) {

40.             if (!hookToggling) {

41.                 hookToggling = true;

42.                 hookTarget = hookMid;

43.             }

44.         } else hookToggling = false;

45.  

46.         if (ControllerPrimary.ButtonY.pressing() ||

47.         ControllerPartner.ButtonY.pressing()) {

48.             alignToMogo();

49.         }

50.  

51.         this_thread::sleep_for(10);

52.     }

53. }

54.  

55. void controllerDriveControlCheck( void ) {

56.     if (ControllerPrimary.ButtonA.pressing() && DriveController != &ControllerPrimary) {

57.         DriveController->rumble("-");

58.         DriveController = &ControllerPrimary;

59.         NonDriveController = &ControllerPartner;

60.     }

61.     else if (ControllerPartner.ButtonA.pressing() && DriveController != &ControllerPartner) {

62.         DriveController->rumble("-");

63.         DriveController = &ControllerPartner;

64.         NonDriveController = &ControllerPrimary;

65.     }

66. }

67.  

68. void controllerDrive( void ) {

69.     // If the L1 button is pressed, set the drive multiplier to 100 otherwise set it to 50

70.     DriveController->ButtonL1.pressing() ? driveMultiplier = 100 : driveMultiplier = 50;

71.  

72.     // Convert the drive mutliplier decimal

73.     driveMultiplierDecimal = driveMultiplier/100;

74.  

75.     // Get the position for driving from the drive controller

76.     driveAxis3 = DriveController->Axis3.position(pct);

77.     driveAxis1 = DriveController->Axis1.position(pct);

78.  

79.     // Deadband (Small values are ignored to prevent stick drift)

80.     if (abs(driveAxis3) < 5) driveAxis3 = 0;

81.     if (abs(driveAxis1) < 5) driveAxis1 = 0;

82.  

83.     // Calculate the power for each side of the drivetrain

84.     double leftPower = (driveAxis3 + driveAxis1);

85.     double rightPower = (driveAxis3 - driveAxis1);

86.  

87.     // Multiply the power by the drive mutliplier decimal

88.     leftPower *= driveMultiplierDecimal;

89.     rightPower *= driveMultiplierDecimal;

90.  

91.     // Set the power for each side of the drivetrain

92.     if (leftPower != 0 || rightPower != 0) {

93.         RMG.spin(directionType::fwd, rightPower, velocityUnits::pct);

94.         LMG.spin(directionType::fwd, leftPower, velocityUnits::pct);

95.     } else {

96.         if (!disableBrake) {

97.             RMG.stop(brake);

98.             LMG.stop(brake);

99.         }

100.     }

101. }

102.  

1. #include "vex.h"

2. #include "robot-config.h"

3.  

4. bool intakeControl_checkValues( controller* targetController ) {

5.     bool targetControllerButtonL2Pressing = targetController->ButtonL2.pressing();

6.     bool targetControllerButtonR2Pressing = targetController->ButtonR2.pressing();

7.     bool targetControllerButtonR1Pressing = targetController->ButtonR1.pressing();

8.  

9.     double intakeAxis2 = targetController->Axis2.position(pct);

10.  

11.     if (abs(intakeAxis2) < 5) intakeAxis2 = 0;

12.  

13.     if (targetControllerButtonL2Pressing) {

14.         Intake.spin(directionType::rev, -intakeAxis2, velocityUnits::pct);

15.     } else {

16.         if (targetControllerButtonR1Pressing || targetControllerButtonR2Pressing) {

17.             directionType intakeDirection = directionType::fwd;

18.             if (targetControllerButtonR1Pressing) intakeDirection = directionType::fwd;

19.             if (targetControllerButtonR2Pressing) intakeDirection = directionType::rev;

20.             Intake.spin(intakeDirection, 100, velocityUnits::pct);

21.         } else {

22.             Intake.stop();

23.         }

24.     }

25.  

26.     if (targetControllerButtonR1Pressing ||

27.     targetControllerButtonR2Pressing ||

28.     intakeAxis2 != 0) {

29.         return false;

30.     } else {

31.         return true;

32.     }

33. }

34.  

35. void intakeControl( void ) {

36.     bool allowSecondary = intakeControl_checkValues(MainIntakeController);

37.  

38.     if (allowSecondary) {

39.         intakeControl_checkValues(SecondaryIntakeController);

40.     }

41. }

1. #include "vex.h"

2. #include "robot-config.h"

3.  

4. void resetHook( void ) {

5.     if (doResetHook) {

6.         Hook.spin(directionType::fwd, 60, velocityUnits::pct);

7.         if (Hook.torque(torqueUnits::Nm) >= 0.5) {

8.             resetStarted = true;

9.         }

10.         if (abs(Hook.velocity(velocityUnits::rpm)) <= 0 && resetStarted) {

11.             Hook.setPosition(0, rotationUnits::deg);

12.             Hook.stop(brake);

13.             doResetHook = false;

14.             resetStarted = false;

15.         }

16.     }

17. }

18.  

19. void hookControl( void ) {

20.     resetHook();

21.     if (doResetHook) return;

22.     double control = hookPID.calculate(hookTarget, -Hook.position(deg), hookPID_dt);

23.     control = abs(control) < 1 ? 0 : control;

24.     Hook.spin(directionType::rev, control, velocityUnits::pct);

25. }

1. #include "vex.h"

2. #include "robot-config.h"

3. #include "libraries/pid.h"

4.  

5. using namespace vex;

6. using signature = vision::signature;

7. using code = vision::code;

8.  

9. /* #region Vision Sensor */

10. signature VisionSensor__BLUE_DONUT(1, -3699, -3339, -3518, 8013, 8787, 8400, 3.9, 0);

11. signature VisionSensor__RED_DONUT(2, 10309, 11361, 10836, -1321, -767, -1044, 2, 0);

12. signature VisionSensor__MOBILE_GOAL(3, -49, 49, 0, -4033, -3635, -3834, 7.5, 0);

13. vision VisionSensor(PORT3, 50, VisionSensor__BLUE_DONUT, VisionSensor__RED_DONUT, VisionSensor__MOBILE_GOAL);

14.  

15. PIDController alignPID(0.2, 0, 0);

16. double alignPID_dt = 0.1;

17. /* #endregion */

18.  

19. void alignToMogo( void ) {

20.     VisionSensor.takeSnapshot(3);

21.     if (VisionSensor.largestObject.exists) {

22.         double measuredValue = 157.5 - VisionSensor.largestObject.centerX;

23.         double control = alignPID.calculate(157.5, measuredValue, alignPID_dt);

24.         LMG.spin(directionType::fwd, control, velocityUnits::pct);

25.         RMG.spin(directionType::rev, control, velocityUnits::pct);

26.     }

27. }

1. #include "vex.h"

2. #include "robot-config.h"

3. #include "controllers/include.h"

4. #include "libraries/include.h"

5.  

6. void auton1( void ) {

7.     Drivetrain.driveFor( directionType::rev, 18, distanceUnits::in );

8.     toggleAlignToMogo();

9.     Drivetrain.driveFor( directionType::rev, 17, distanceUnits::in );

10.     toggleClaw(true);

11.     Drivetrain.driveFor( directionType::fwd, 17, distanceUnits::in );

12.     Drivetrain.turnToHeading( 245, rotationUnits::deg );

13.     Intake.spin( directionType::fwd, 100, velocityUnits::pct );

14.     Drivetrain.driveFor( directionType::fwd, 28, distanceUnits::in, 50, velocityUnits::pct );

15.     Drivetrain.turnToHeading( 115, rotationUnits::deg );

16.     Intake.spin( directionType::rev, 100, velocityUnits::pct );

17.     Drivetrain.driveFor( directionType::fwd, 28, distanceUnits::in, 30, velocityUnits::pct );

18.     Drivetrain.turnFor(turnType::right, 30, rotationUnits::deg);

19.     Drivetrain.driveFor( directionType::fwd, 3, distanceUnits::in );

20. }

#pragma once

#include "lemlib/chassis/chassis.hpp"

#include "lemlib/pose.hpp"

namespace lemlib {

/**

 * @brief Set the sensors to be used for odometry

 *

 * @param sensors the sensors to be used

 * @param drivetrain drivetrain to be used

 */

void setSensors(lemlib::OdomSensors sensors, lemlib::Drivetrain drivetrain);

/**

 * @brief Get the pose of the robot

 *

 * @param radians true for theta in radians, false for degrees. False by default

 * @return Pose

 */

Pose getPose(bool radians = false);

/**

 * @brief Set the Pose of the robot

 *

 * @param pose the new pose

 * @param radians true if theta is in radians, false if in degrees. False by default

 */

void setPose(Pose pose, bool radians = false);

/**

 * @brief Get the speed of the robot

 *

 * @param radians true for theta in radians, false for degrees. False by default

 * @return lemlib::Pose

 */

Pose getSpeed(bool radians = false);

/**

 * @brief Get the local speed of the robot

 *

 * @param radians true for theta in radians, false for degrees. False by default

 * @return lemlib::Pose

 */

Pose getLocalSpeed(bool radians = false);

/**

 * @brief Estimate the pose of the robot after a certain amount of time

 *

 * @param time time in seconds

 * @param radians False for degrees, true for radians. False by default

 * @return lemlib::Pose

 */

Pose estimatePose(float time, bool radians = false);

/**

 * @brief Update the pose of the robot

 *

 */

void update();

/**

 * @brief Initialize the odometry system

 *

 */

void init();

} // namespace lemlib

#include "classes/clamp.h"

Clamp::Clamp(ControllerPair* pair)

    : piston(CLAMP_PORT, E_ADI_DIGITAL_OUT), 

    distanceSensor(CLAMP_DISTANCE_PORT),

    pair(pair)

{

}

void Clamp::toggle( bool override )

{

    bool clampState = piston.get_value();

    double mogoDistance = distanceSensor.get();

    if (!clampState && (mogoDistance < 70 || override)) {

        piston.set_value(true);

    } else {

        piston.set_value(false);

    }

}

void Clamp::set(bool state)

{

    piston.set_value(state);

}

void Clamp::loop() {

    if (pair->driver->btnX.isPressed_new()) {

        toggle();

    } else if (pair->support->btnX.isPressed_new()) {

        toggle();

    }

    if (autoClampEnabled) {

        if (distanceSensor.get() < 70) {

            set(true);

            autoClampEnabled = false;

        }

    }

}

Clamp::~Clamp()

{

}

#pragma once

#include "classes/controllerSolo.h"

#include "main.h"

class ControllerPair {

public:

    ControllerSolo* master;

    ControllerSolo* partner;

    ControllerSolo* driver;

    ControllerSolo* support;

    ControllerPair(ControllerSolo* masterController, ControllerSolo* partnerController)

        : master(masterController),

        partner(partnerController),

        driver(masterController),

        support(partnerController) {

    }

    void setDriver(ControllerSolo* newDriver) {

        if (newDriver == driver) return;

        ControllerSolo* temp = driver;

        driver = newDriver;

        support = temp;

    }

    void checkForSwitch() {

        if (master->btnA.isPressed()) {

            setDriver(master);

        } else if (partner->btnA.isPressed()) {

            setDriver(partner);

        }

    }   

    void loop() {

        master->buttonLoop();

        partner->buttonLoop();

        checkForSwitch();

    }

};

#include "classes/intake.h"

Intake::Intake(ControllerPair* pair)

    : motor(INTAKE_MOTOR_PORT, v5::MotorGear::blue),

    piston(INTAKE_RISER_PORT, E_ADI_DIGITAL_OUT),

    pair(pair) {

}

bool Intake::checkForInput(ControllerSolo* controller) {

    if (controller->btnR1.isPressed()) {

        motor.move(127);

        return true;

    } else if (controller->btnR2.isPressed()) {

        motor.move(-127);

        return true;

    }

    return false;

}

void Intake::handleMotor() {

    if (checkForInput(pair->driver)) return;

    if (checkForInput(pair->support)) return;

    if (motor.get_voltage() != 0) motor.move(0);

}

void Intake::toggleRiser() {

    piston.set_value(!piston.get_value());

}

void Intake::handleRiser() {

    if (pair->master->btnLeft.isPressed_new()) toggleRiser();

    else if (pair->partner->btnLeft.isPressed_new()) toggleRiser();

}

void Intake::loop() {

    handleMotor();

    handleRiser();

}

#pragma once

// #include "main.h"

#include "liblvgl/lvgl.h"

#include "other/fieldImg.h"

#include <string>

#include "classes/screen/tabs.h"

#include "classes/autonomous/main.h"

class Autonomous {

public:

  Autonomous(lv_obj_t *parent, AutonomousRoutines* autonRoutines);

  lv_obj_t *optionsLabel;

  // 0 is no auton, 1 is blue negative, 2 is blue positive, 3 is red positive, 4

  // is red negative

  int8_t auton_selection = 0;

  const char* routine_selection = "No Auton";

private:

  AutonomousRoutines* autonRoutines;

  lv_obj_t *btn_matrix;

  static void staticSelectAuton(lv_event_t *e) {

    Autonomous *instance = (Autonomous *)lv_event_get_user_data(e);

    instance->selectAuton(e);

  };

  static void staticSelectAutonRoutine(lv_event_t *e) {

    Autonomous *instance = (Autonomous *)lv_event_get_user_data(e);

    instance->selectAutonRoutine();

  };

  void selectAuton(lv_event_t *e);

  void selectAutonRoutine(int16_t check_id = -1);

  void setBtnMatrix(const char **map, size_t size);

};

void Autonomous::selectAuton(lv_event_t *e) {

  lv_event_code_t code = lv_event_get_code(e);

  if (code == LV_EVENT_CLICKED) {

    lv_obj_t *obj = lv_event_get_target(e);

    lv_obj_t *label = lv_obj_get_child(obj, 0);

    bool is_positive = std::string(lv_label_get_text(label)) == "+";

    lv_color32_t color = lv_obj_get_style_bg_color(obj, 0);

    uint32_t raw_color = color.full;

    if (raw_color == lv_color_make(255, 0, 0).full) {

      if (is_positive) {

        // Red Positive

        auton_selection = 3;

        setBtnMatrix((const char **)autonRoutines->redplus.routines,

                     autonRoutines->redplus.routinesSize);

        lv_label_set_text(optionsLabel, "Red Positive (3)");

      } else {

        // Red Negative

        auton_selection = 4;

        setBtnMatrix((const char **)autonRoutines->redminus.routines,

                     autonRoutines->redminus.routinesSize);

        lv_label_set_text(optionsLabel, "Red Negative (4)");

      }

    } else if (raw_color == lv_color_make(0, 119, 200).full) {

      if (is_positive) {

        // Blue Positive

        auton_selection = 2;

        setBtnMatrix((const char **)autonRoutines->blueplus.routines,

                     autonRoutines->blueplus.routinesSize);

        lv_label_set_text(optionsLabel, "Blue Positive (2)");

      } else {

        // Blue Negative

        auton_selection = 1;

        setBtnMatrix((const char **)autonRoutines->blueminus.routines,

                     autonRoutines->blueminus.routinesSize);

        lv_label_set_text(optionsLabel, "Blue Negative (1)");

      }

    } else if (raw_color == lv_color_make(100, 100, 100).full) {

      // No Auton

      auton_selection = 0;

      static constexpr const char* noAutonMap[] = {"No Auton", ""};

      setBtnMatrix((const char **)noAutonMap, sizeof(noAutonMap) / sizeof(noAutonMap[0]));

      lv_label_set_text(optionsLabel, "No Auton (0)");

    }

  }

}

Autonomous::Autonomous(lv_obj_t *parent, AutonomousRoutines* autonRoutines)

    : autonRoutines(autonRoutines) {

  lv_obj_t *fieldContainer = lv_obj_create(parent);

  lv_obj_set_size(fieldContainer, 240, 240);

  lv_obj_set_style_pad_all(fieldContainer, 0, 0);

  lv_obj_clear_flag(fieldContainer, LV_OBJ_FLAG_SCROLLABLE);

  lv_obj_t *fieldImg = lv_img_create(fieldContainer);

  lv_img_set_src(fieldImg, &field_image);

  lv_obj_align(fieldImg, LV_ALIGN_TOP_LEFT, 0, 0);

  /* #region Auton Buttons */

  lv_obj_t *redPlus = lv_btn_create(fieldContainer);

  lv_obj_set_size(redPlus, 25, 25);

  lv_obj_align(redPlus, LV_ALIGN_TOP_LEFT, 20, 20);

  lv_obj_set_style_bg_color(redPlus, lv_color_make(255, 0, 0), 0);

  lv_obj_add_event_cb(redPlus, staticSelectAuton, LV_EVENT_CLICKED, this);

  lv_obj_t *redPlusLabel = lv_label_create(redPlus);

  lv_obj_set_style_text_font(redPlusLabel, &lv_font_montserrat_24, 0);

  lv_label_set_text(redPlusLabel, "+");

  lv_obj_center(redPlusLabel);

  lv_obj_t *redMinus = lv_btn_create(fieldContainer);

  lv_obj_set_size(redMinus, 25, 25);

  lv_obj_align(redMinus, LV_ALIGN_TOP_RIGHT, -20, 20);

  lv_obj_set_style_bg_color(redMinus, lv_color_make(255, 0, 0), 0);

  lv_obj_add_event_cb(redMinus, staticSelectAuton, LV_EVENT_CLICKED, this);

  lv_obj_t *redMinusLabel = lv_label_create(redMinus);

  lv_obj_set_style_text_font(redMinusLabel, &lv_font_montserrat_24, 0);

  lv_label_set_text(redMinusLabel, "-");

  lv_obj_center(redMinusLabel);

  lv_obj_t *bluePlus = lv_btn_create(fieldContainer);

  lv_obj_set_size(bluePlus, 25, 25);

  lv_obj_align(bluePlus, LV_ALIGN_BOTTOM_LEFT, 20, -20);

  lv_obj_set_style_bg_color(bluePlus, lv_color_make(0, 119, 200), 0);

  lv_obj_add_event_cb(bluePlus, staticSelectAuton, LV_EVENT_CLICKED, this);

  lv_obj_t *bluePlusLabel = lv_label_create(bluePlus);

  lv_obj_set_style_text_font(bluePlusLabel, &lv_font_montserrat_24, 0);

  lv_label_set_text(bluePlusLabel, "+");

  lv_obj_center(bluePlusLabel);

  lv_obj_t *blueMinus = lv_btn_create(fieldContainer);

  lv_obj_set_size(blueMinus, 25, 25);

  lv_obj_align(blueMinus, LV_ALIGN_BOTTOM_RIGHT, -20, -20);

  lv_obj_set_style_bg_color(blueMinus, lv_color_make(0, 119, 200), 0);

  lv_obj_add_event_cb(blueMinus, staticSelectAuton, LV_EVENT_CLICKED, this);

  lv_obj_t *blueMinusLabel = lv_label_create(blueMinus);

  lv_obj_set_style_text_font(blueMinusLabel, &lv_font_montserrat_24, 0);

  lv_label_set_text(blueMinusLabel, "-");

  lv_obj_center(blueMinusLabel);

  lv_obj_t *noAuton = lv_btn_create(fieldContainer);

  lv_obj_set_size(noAuton, 25, 25);

  lv_obj_align(noAuton, LV_ALIGN_CENTER, 0, 0);

  lv_obj_set_style_bg_color(noAuton, lv_color_make(100, 100, 100), 0);

  lv_obj_add_event_cb(noAuton, staticSelectAuton, LV_EVENT_CLICKED, this);

  lv_obj_t *noAutonLabel = lv_label_create(noAuton);

  lv_obj_set_style_text_font(noAutonLabel, &lv_font_montserrat_24, 0);

  lv_label_set_text(noAutonLabel, "X");

  lv_obj_center(noAutonLabel);

  /* #endregion */

  lv_obj_t *optionsContainer = lv_obj_create(parent);

  lv_obj_set_size(optionsContainer, 240 - 35, 240);

  lv_obj_align(optionsContainer, LV_ALIGN_TOP_RIGHT, 0, 0);

  optionsLabel = lv_label_create(optionsContainer);

  lv_obj_set_style_text_font(optionsLabel, &lv_font_montserrat_16, 0);

  lv_label_set_text(optionsLabel, "No Auton (0)");

  lv_obj_set_style_border_side(optionsLabel, LV_BORDER_SIDE_BOTTOM, 0);

  lv_obj_set_style_border_width(optionsLabel, 3, 0);

  lv_obj_set_style_border_color(optionsLabel,

                                lv_palette_main(LV_PALETTE_GREEN), 0);

  lv_obj_align(optionsLabel, LV_ALIGN_TOP_MID, 0, 0);

  /* #region Options Btn Matrix */

  static lv_style_t style_bg;

  lv_style_init(&style_bg);

  lv_style_set_pad_all(&style_bg, 0);

  lv_style_set_pad_gap(&style_bg, 5);

  lv_style_set_clip_corner(&style_bg, true);

  lv_style_set_radius(&style_bg, 0);

  lv_style_set_border_width(&style_bg, 0);

  static lv_style_t style_btn;

  lv_style_init(&style_btn);

  lv_style_set_radius(&style_btn, 0);

  lv_style_set_border_width(&style_btn, 3);

  lv_style_set_border_opa(&style_btn, LV_OPA_50);

  lv_style_set_border_color(&style_btn, lv_palette_main(LV_PALETTE_GREY));

  lv_style_set_radius(&style_btn, LV_RADIUS_CIRCLE);

  static constexpr const char* map[] = {"No Autons", ""};

  btn_matrix = lv_btnmatrix_create(optionsContainer);

  lv_obj_add_style(btn_matrix, &style_bg, 0);

  lv_obj_add_style(btn_matrix, &style_btn, LV_PART_ITEMS);

  lv_obj_add_event_cb(btn_matrix, staticSelectAutonRoutine, LV_EVENT_VALUE_CHANGED, this);

  lv_obj_align(btn_matrix, LV_ALIGN_TOP_MID, 0, 35);

  setBtnMatrix((const char **)map, sizeof(map) / sizeof(map[0]));

  /* #endregion */

}

void Autonomous::setBtnMatrix(const char **map, size_t size) {

  lv_btnmatrix_set_map(btn_matrix, map);

  lv_obj_set_size(btn_matrix, 240 - 35 - 40, size * 20);

  lv_btnmatrix_set_btn_ctrl_all(btn_matrix, LV_BTNMATRIX_CTRL_CHECKABLE);

  lv_btnmatrix_set_one_checked(btn_matrix, true);

  lv_btnmatrix_set_btn_ctrl(btn_matrix, 0, LV_BTNMATRIX_CTRL_CHECKED);

  selectAutonRoutine(0);

}

void Autonomous::selectAutonRoutine(int16_t check_id) {

  if (check_id == -1) {

    check_id = lv_btnmatrix_get_selected_btn(btn_matrix);

  }

  const char* routineName = lv_btnmatrix_get_btn_text(btn_matrix, check_id);

  routine_selection = routineName;

  autonRoutines->autonSelection = auton_selection;

  autonRoutines->subAuton = routineName;

}

#include "classes/doinker.h"

Doinker::Doinker(ControllerPair* pair)

: motor(DOINKER_MOTOR_PORT, v5::MotorGear::red),

pair(pair),

pid(HOOK_KP, HOOK_KI, HOOK_KD) {

    motor.set_encoder_units(v5::MotorEncoderUnits::degrees);

}

void Doinker::resetLoop() {

    if (isResetting) {

        motor.move_velocity(40);

        if (motor.get_torque() >= 0.05) {

            resetStarted = true;

        }

        if (std::fabs(motor.get_actual_velocity()) <= 0 && resetStarted) {

            motor.tare_position();

            motor.move(0);

            isResetting = false;

            resetStarted = false;

        }

    }

}

void Doinker::reset() {

    isResetting = true;

}

void Doinker::controlCheck(ControllerSolo* controller) {

    if (controller->btnUp.isPressed_new()) {

        target = HOOK_DOWN;

    } else if (controller->btnRight.isReleased_new()) {

        target = HOOK_MID;

    } else if (controller->btnDown.isPressed_new()) {

        target = HOOK_UP;

    }

}

void Doinker::loop() {

    resetLoop();

    if (!isResetting) {

        double control = pid.calculate(target, -motor.get_position(), HOOK_DT);

        control = std::fabs(control) < 1 ? 0 : control;

        motor.move_velocity(-control);

    }

    controlCheck(pair->driver);

    controlCheck(pair->partner);

}

#include "main.h"

#include "lemlib/api.hpp"

#include "liblvgl/lvgl.h"

#include "pros/apix.h"

#include "fieldUtils.h"

// #include "logo.h"

#include "classes/include.h"

ControllerSolo master(E_CONTROLLER_MASTER);

ControllerSolo partner(E_CONTROLLER_PARTNER);

ControllerPair pair(&master, &partner);

DriveBase drivebase;

Clamp clamp(&pair);

Intake intake(&pair);

Doinker doinker(&pair);

BumperSwitch odomReset('H');

// ASSET(test_txt);

AutonomousRoutines autonomousRoutines(

    &drivebase, 

    &intake, 

    &clamp, 

    &doinker

);

Screen screenClass(&autonomousRoutines, &drivebase);

static bool isAuton = false;

void initialize() {

    lv_init();

    lv_theme_default_init(

        NULL,

        lv_palette_main(LV_PALETTE_GREEN),

        lv_palette_main(LV_PALETTE_GREY),

        true,

        LV_FONT_DEFAULT

    );

    drivebase.chassis.calibrate();

    doinker.reset();

    // Create a task for updating sensor data

    Task screen_task([&]() {

        static lv_obj_t* text_x = lv_label_create(screenClass.tabs.odometryTab);

        static lv_obj_t* text_y = lv_label_create(screenClass.tabs.odometryTab);

        static lv_obj_t* text_theta = lv_label_create(screenClass.tabs.odometryTab);

        // Position them once

        lv_obj_align(text_x, LV_ALIGN_TOP_LEFT, 10, 10);

        lv_obj_align(text_y, LV_ALIGN_TOP_LEFT, 10, 30);

        lv_obj_align(text_theta, LV_ALIGN_TOP_LEFT, 10, 50);

        while (true) {

            //TODO: Disable some of this during autonomous

            pair.loop();

            clamp.loop();

            if (!isAuton) intake.loop();

            doinker.loop();

            screenClass.motors.loop();

            if (odomReset.pressed()) {

                printf("Resetting Odom to Auton Start\n");

                autonomousRoutines.runAuton(true);

            }

        //     lv_task_handler();

            // Only update text content

            char buf[32];

            sprintf(buf, "X: %.2f", drivebase.chassis.getPose().x);

            lv_label_set_text(text_x, buf);

            sprintf(buf, "Y: %.2f", drivebase.chassis.getPose().y);

            lv_label_set_text(text_y, buf);

            sprintf(buf, "Theta: %.2f", drivebase.chassis.getPose().theta);

            lv_label_set_text(text_theta, buf);

            delay(20);

        }

        // Create text objects once

        // static lv_obj_t* text_x = lv_label_create(lv_scr_act());

        // static lv_obj_t* text_y = lv_label_create(lv_scr_act());

        // static lv_obj_t* text_theta = lv_label_create(lv_scr_act());

        // // Position them once

        // lv_obj_align(text_x, LV_ALIGN_TOP_LEFT, 10, 10);

        // lv_obj_align(text_y, LV_ALIGN_TOP_LEFT, 10, 30);

        // lv_obj_align(text_theta, LV_ALIGN_TOP_LEFT, 10, 50);

        // while (true) {

        //     lv_task_handler();

        //     // Only update text content

        //     char buf[32];

        //     sprintf(buf, "X: %.2f", drivebase.chassis.getPose().x);

        //     lv_label_set_text(text_x, buf);

        //     sprintf(buf, "Y: %.2f", drivebase.chassis.getPose().y);

        //     lv_label_set_text(text_y, buf);