Merge pull request #75 from calhighrobotics/improvements/issue-72/skills-auton

Added some skills fixes for competition field

Author: aswsub

include/globals.h

@@ -75,6 +75,8 @@ extern pros::Rotation lateral_sensor;
 extern pros::Rotation horizontal_sensor;
 
 extern pros::Rotation LadyBrownRotation;
+extern pros::Rotation HookRotation;
+
 
 // forward/backward PID
 extern lemlib::ControllerSettings lateral_controller;
@@ -92,11 +94,15 @@ extern lemlib::ExpoDriveCurve throttle_curve;
 
 extern lemlib::ExpoDriveCurve steer_curve;
 
+extern lemlib::ExpoDriveCurve arcade_turn_curve;
+
 extern pros::adi::DigitalIn drivetrainToggleSwitch;
 extern pros::adi::DigitalIn autonToggleSwitch;
 
 extern pros::adi::Pneumatics LatchControl;
 extern pros::adi::Pneumatics SweeperControl;
+extern pros::adi::Pneumatics IntakeLift;
+extern pros::adi::DigitalIn HookSwitch;
 
 } // namespace Globals
 } // namespace Robot

include/robot/auton.h

@@ -64,7 +64,7 @@ class Autonomous {
     * strategy. It contains the specific actions and movements required for this
     * strategy.
     */
-   void RedNeg(Intake &intake, Latch &latch, DistanceSensor &distance);
+   void RedNeg(Intake &intake, Latch &latch, DistanceSensor &distance, LadyBrown &ladybrown);
 
    /**
     * @brief Runs the autonomous path for the near side offensive game strategy.
@@ -102,7 +102,7 @@ class Autonomous {
     * @todo Make the autonomous more fleshed out, building it properly for the
     * competition
     */
-   void BlueNeg(Intake &intake, Latch &latch, DistanceSensor &distance);
+   void BlueNeg(Intake &intake, Latch &latch, DistanceSensor &distance, LadyBrown &ladybrown);
 
    /**
     * Executes the Skills challenge autonomous.

include/robot/ladybrown.h

@@ -9,7 +9,7 @@ namespace Robot {
 class LadyBrown {
 public:
    // The ENUM associates with each location of the rotation sensor for the LadyBrown to move.
-   enum LADYBROWN_STATE { BASE_STATE = 1, LOAD_STATE = 2, ATTACK_STATE = 3 };
+   enum LADYBROWN_STATE { BASE_STATE = 1, LOAD_STATE = 2, ATTACK_STATE = 3, SIZECHECK_STATE = 4 };
 
    void run(bool async = true, int timeout = 1000);
 
@@ -22,10 +22,16 @@ class LadyBrown {
 private:
    lemlib::PID MoveToPointPID;
 
+   lemlib::PID ActiveBrakePID;
+
+   int brake_target;
+
    static LADYBROWN_STATE current_state;
 
    int target;
 
    bool isPIDRunning;
+
+   bool manualControl;
 };
 } // namespace Robot

src/globals.cpp

@@ -1,10 +1,10 @@
 #include "globals.h"
 #include "pros/abstract_motor.hpp"
+#include "pros/adi.hpp"
 #include "pros/misc.hpp"
 #include "pros/motors.hpp"
 #include "pros/optical.hpp"
 #include "pros/vision.h"
-#include "pros/vision.hpp"
 
 /*
  * Although the following constants belong in their own seperate
@@ -23,29 +23,35 @@ namespace Globals {
 pros::Controller controller(pros::E_CONTROLLER_MASTER);
 pros::Controller partner(pros::E_CONTROLLER_PARTNER);
 
-pros::Motor RightFront(13, pros::v5::MotorGears::blue, pros::v5::MotorUnits::degrees);
-pros::Motor LeftFront(-19, pros::v5::MotorGears::blue, pros::v5::MotorUnits::degrees);
-pros::Motor LeftBack(-18, pros::v5::MotorGears::blue, pros::v5::MotorUnits::degrees);
-pros::Motor RightBack(12, pros::v5::MotorGears::blue, pros::v5::MotorUnits::degrees);
-pros::Motor LeftMid(20, pros::v5::MotorGears::blue, pros::v5::MotorUnits::degrees);
-pros::Motor RightMid(-11, pros::v5::MotorGears::blue, pros::v5::MotorUnits::degrees);
-pros::Motor IntakeMotor(-9, pros::v5::MotorGears::blue, pros::v5::MotorUnits::degrees);
-pros::Motor HookMotor(7, pros::v5::MotorGears::green, pros::v5::MotorUnits::degrees);
-pros::Motor LadyBrownMotor(-3, pros::v5::MotorGears::green, pros::v5::MotorUnits::degrees);
+pros::Motor RightFront(16, pros::v5::MotorGears::blue, pros::v5::MotorUnits::degrees);
+pros::Motor LeftFront(-13, pros::v5::MotorGears::blue, pros::v5::MotorUnits::degrees);
+pros::Motor LeftBack(-11, pros::v5::MotorGears::blue, pros::v5::MotorUnits::degrees);
+pros::Motor RightBack(14, pros::v5::MotorGears::blue, pros::v5::MotorUnits::degrees);
+pros::Motor LeftMid(12, pros::v5::MotorGears::blue, pros::v5::MotorUnits::degrees);
+pros::Motor RightMid(-15, pros::v5::MotorGears::blue, pros::v5::MotorUnits::degrees);
+pros::Motor IntakeMotor(-4, pros::v5::MotorGears::blue, pros::v5::MotorUnits::degrees);
+pros::Motor HookMotor(3, pros::v5::MotorGears::green, pros::v5::MotorUnits::degrees);
+pros::Motor LadyBrownMotor(-10, pros::v5::MotorGears::green, pros::v5::MotorUnits::degrees);
 
 pros::adi::Pneumatics LatchControl('A', false);
 pros::adi::Pneumatics SweeperControl('B', false);
+pros::adi::Pneumatics IntakeLift('D', false);
 
-pros::Distance distance_sensor(10);
-pros::Rotation lateral_sensor(16);
-pros::Rotation horizontal_sensor(17);
+pros::adi::DigitalIn HookSwitch('C');
 
-pros::Rotation LadyBrownRotation(8);
 
-pros::Imu inertial_sensor(15);
+pros::Distance distance_sensor(6);
+pros::Rotation lateral_sensor(-19);
+pros::Rotation horizontal_sensor(-20);
+
+pros::Rotation LadyBrownRotation(9);
+pros::Rotation HookRotation(2);
+
+
+pros::Imu inertial_sensor(17);
 
 // Vision sensor configuration
-pros::Optical colorSensor(4);
+pros::Optical colorSensor(5);
 
 pros::vision_signature_s_t RED_SIG =
     pros::c::vision_signature_from_utility(1, -4653, -3619, -4136, 9831, 11725, 10778, 2.5, 0);
@@ -62,15 +68,15 @@ pros::MotorGroup drive_({LeftFront.get_port(), RightFront.get_port(), LeftMid.ge
                          LeftBack.get_port(), RightBack.get_port()});
 
 // Lemlib objects - Used by lemlib drive and odometry functions
-lemlib::TrackingWheel horizontal_tracking_wheel(&horizontal_sensor, 2, -2.75);
-lemlib::TrackingWheel vertical_tracking_wheel(&lateral_sensor, 2, -1);
+lemlib::TrackingWheel horizontal_tracking_wheel(&horizontal_sensor, 2, 0.25);
+lemlib::TrackingWheel vertical_tracking_wheel(&lateral_sensor, 2, -0.625);
 
 // Describes the lemlib objects that are used to control the autonomous
 // functions of the robot.
 lemlib::Drivetrain drivetrain{
     &drive_left,  // left drivetrain motors
     &drive_right, // right drivetrain motors
-    12.25,        // track width
+    10.8,        // track width
     lemlib::Omniwheel::OLD_325,
     450, // drivetrain rpm is 450
     2    // horizontal drift is 2
@@ -86,38 +92,56 @@ lemlib::OdomSensors sensors{
 
 // forward/backward PID
 // lateral PID controller
-lemlib::ControllerSettings lateral_controller(6.5, // proportional gain (kP)
+lemlib::ControllerSettings lateral_controller(6.4, // proportional gain (kP)
                                               0,   // integral gain (kI)
                                               35,  // derivative gain (kD)
-                                              0,   // anti windup
+                                              3,   // anti windup
                                               0.5, // small error range, in inches
                                               100, // small error range timeout, in milliseconds
-                                              3,   // large error range, in inches
+                                              2,   // large error range, in inches
                                               500, // large error range timeout, in milliseconds
-                                              20   // maximum acceleration (slew)
+                                              115   // maximum acceleration (slew)
 );
 
-// angular PID controller
-lemlib::ControllerSettings angular_controller(2.1, // proportional gain (kP)
+// 0,   // anti windup
+// 0.5, // small error range, in inches
+// 100, // small error range timeout, in milliseconds
+// 3,   // large error range, in inches
+// 500, // large error range timeout, in milliseconds
+// 20  
+
+// angular PID controller 2.2 17 Last test: kp 13 kd 75, kp was just before full oscilation, kd was just before jittering
+lemlib::ControllerSettings angular_controller(3.27, // proportional gain (kP)
                                               0,   // integral gain (kI)
-                                              14,  // derivative gain (kD)
-                                              0,   // anti windup
-                                              0.5, // small error range, in degrees
-                                              100, // small error range timeout, in milliseconds
-                                              2,   // large error range, in degrees
+                                              27.5,  // derivative gain (kD)
+                                              3,   // anti windup
+                                              2, // small error range, in degrees
+                                              90, // small error range timeout, in milliseconds
+                                              3,   // large error range, in degrees
                                               500, // large error range timeout, in milliseconds
-                                              0    // maximum acceleration (slew)
+                                              0     // maximum acceleration (slew)
 );
 
-lemlib::ExpoDriveCurve throttle_curve(3,    // joystick deadband out of 127
-                                      50,   // minimum output where drivetrain will move out of 127
-                                      0.992 // expo curve gain
+// 2, // small error range, in degrees
+// 80, // small error range timeout, in milliseconds
+// 3,   // large error range, in degrees
+// 500, // large error range timeout, in milliseconds
+// 0
+
+lemlib::ExpoDriveCurve throttle_curve(1,    // joystick deadband out of 127
+                                      35,   // minimum output where drivetrain will move out of 127
+                                      1.002 // expo curve gain
 );
 
 // input curve for steer input during driver control
-lemlib::ExpoDriveCurve steer_curve(3,   // joystick deadband out of 127
-                                   17,  // minimum output where drivetrain will move out of 127
-                                   1 // expo curve gain
+lemlib::ExpoDriveCurve steer_curve(1,   // joystick deadband out of 127
+                                   16,  // minimum output where drivetrain will move out of 127
+                                   1.0084 // expo curve gain
+);
+
+lemlib::ExpoDriveCurve arcade_turn_curve(1,   // joystick deadband out of 127
+                                   8,  // minimum output where drivetrain will move out of 127
+                                   1.016 // expo curve gain
 );
 
 lemlib::Chassis chassis(drivetrain, lateral_controller, angular_controller, sensors, &throttle_curve, &steer_curve);

src/main.cpp

@@ -5,6 +5,7 @@
 #include "lemlib/chassis/chassis.hpp"
 #include "liblvgl/llemu.hpp"
 #include "pros/misc.h"
+#include "lemlib/timer.hpp"
 #include "pros/misc.hpp"
 #include "pros/motors.hpp"
 #include "pros/rtos.hpp"
@@ -78,11 +79,19 @@ void initialize() {
          // delay to save resources
          pros::lcd::print(3, "Lateral Sensor: %i", lateral_sensor.get_position());
          pros::lcd::print(4, "Horizontal Sensor: %i", horizontal_sensor.get_position());
-         pros::lcd::print(5, "Lady Brown Sensor: %i", LadyBrownRotation.get_position());
+         pros::lcd::print(5, "Lady Brown Sensor: %i", HookMotor.get_voltage());
          pros::lcd::print(6, "Autonomous: %s", subsystem.autonomous.autonName);
-         pros::lcd::print(7, "Distance Position: %i", distance_sensor.get_distance());
+         pros::lcd::print(7, "Distance Position: %i", HookRotation.get_position());
 
-         pros::delay(20);
+         double total_wattage = LeftBack.get_power() + RightBack.get_power() + LeftFront.get_power() +
+                                RightFront.get_power() + LeftMid.get_power() + RightMid.get_power() +
+                                IntakeMotor.get_power() + HookMotor.get_power() + LadyBrownMotor.get_power();
+
+         //std::cout << "Total Wattage: " << total_wattage << std::endl;
+         //std::cout << chassis.getPose().theta << std::endl;
+         //std::cout << chassis.getPose().x << ", " << chassis.getPose().y << std::endl;
+
+         pros::delay(100);
       }
    });
 
@@ -138,7 +147,7 @@ void disabled() {}
  * starts.<asd></asd>
  */
 void competition_initialize() {
-   screen.selector.selector();
+   //screen.selector.selector();
 
 }
 
@@ -155,11 +164,15 @@ void competition_initialize() {
  */
 
 void autonomous() {
-   Autonomous::auton = Autonomous::BLUE_POS_LATE_RUSH;
+   Autonomous::auton = Autonomous::BLUE_NEG;
    subsystem.autonomous.AutoDrive(subsystem.intake, subsystem.latch, subsystem.sweeper, electronic.distance_sensor,
-                                  subsystem.ladybrown);
-}
+                                subsystem.ladybrown);
+
+   // chassis.setPose(0, 0, 0);
+   // chassis.moveToPoint(0, -3.5, 1000, {.forwards = false});
 
+
+}
 /**
  * Runs the operator control code. This function will be started in its own
  * task with the default priority and stack size whenever the robot is enabled
@@ -177,34 +190,18 @@ void opcontrol() {
 
    while (true) {
 
-      // Calls to event handling functions.
-      if (controller.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_UP)) {
-         autonomous();
-      }
       // Toggles the drivetrain orientation - can be forward or backward
-      if (controller.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_RIGHT)) {
+      if (controller.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_LEFT)) {
          std::string name = subsystem.drivetrain.toggleDrive();
          // Output the current drive mode to the controller screen
          controller.print(0, 0, name.c_str());
       }
-      // Checks for drivetrain reversal - Changes conditions in a value handler function in the drivetrain class
-      if (controller.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_DOWN)) {
-         // isReversed is static, it is changed for the global state.
-         Drivetrain::isReversed = !Drivetrain::isReversed;
-
-         // Output the current drive mode to the controller screen
-         controller.print(0, 0, "reversal: %d", Drivetrain::isReversed);
-      }
-
-      if (controller.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_A)) {
-         pros::Task move([&]() { subsystem.ladybrown.MoveToPoint(LadyBrown::ATTACK_STATE); }, "LadyBrownMove");
-      }
 
       if (controller.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_X)) {
-         IntakeMotor.move_velocity(600);   
+         pros::Task move([&]() { subsystem.ladybrown.MoveToPoint(LadyBrown::ATTACK_STATE); }, "LadyBrownMove");
       }
 
-
+      
 
       subsystem.drivetrain.run();
       subsystem.latch.run();