Merge pull request #65 from calhighrobotics/fix/issue-58/retune-pids

fix/issue 58/retune pids

Author: absozero

src/globals.cpp

@@ -37,7 +37,7 @@ pros::adi::Pneumatics HangControl('C', false);
 pros::adi::Pneumatics SweeperControl('B', false);
 
 pros::Rotation lateral_sensor(16);
-pros::Rotation horizontal_sensor(-17);
+pros::Rotation horizontal_sensor(17);
 
 pros::Imu inertial_sensor(14);
 
@@ -59,8 +59,8 @@ 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, lemlib::Omniwheel::NEW_2, -5);
-lemlib::TrackingWheel vertical_tracking_wheel(&lateral_sensor, lemlib::Omniwheel::NEW_2, -1.45);
+lemlib::TrackingWheel horizontal_tracking_wheel(&horizontal_sensor, 2, -2.75);
+lemlib::TrackingWheel vertical_tracking_wheel(&lateral_sensor, 2, -1);
 
 // Describes the lemlib objects that are used to control the autonomous
 // functions of the robot.
@@ -83,21 +83,21 @@ lemlib::OdomSensors sensors{
 
 // forward/backward PID
 // lateral PID controller
-lemlib::ControllerSettings lateral_controller(10,  // proportional gain (kP)
-                                              0,   // integral gain (kI)
-                                              4.5, // derivative gain (kD)
-                                              3,   // anti windup
-                                              1,   // small error range, in inches
+lemlib::ControllerSettings lateral_controller(5.25, // proportional gain (kP)
+                                              0, // integral gain (kI)
+                                            13, // derivative gain (kD)
+                                              3, // anti windup
+                                              1, // small error range, in inches
                                               100, // small error range timeout, in milliseconds
-                                              3,   // large error range, in inches
+                                              3, // large error range, in inches
                                               500, // large error range timeout, in milliseconds
-                                              25   // maximum acceleration (slew)
+                                              20 // maximum acceleration (slew)
 );
 
 // angular PID controller
-lemlib::ControllerSettings angular_controller(2,    // proportional gain (kP)
+lemlib::ControllerSettings angular_controller(1.73,    // proportional gain (kP)
                                               0,    // integral gain (kI)
-                                              10.5, // derivative gain (kD)
+                                              9, // derivative gain (kD)
                                               3,    // anti windup
                                               1,    // small error range, in degrees
                                               100,  // small error range timeout, in milliseconds

src/main.cpp

@@ -53,14 +53,15 @@ struct Electronics {
  * to keep execution time for this mode under a few seconds.
  */
 
-pros::rtos::Task MotorNotification(electronic.controllers.notify_motor_disconnect);
-pros::rtos::Task LadyBrownNotification(subsystem.ladybrown.edge_check);
+
 
 void initialize() {
    chassis.calibrate();
 
    chassis.setPose(0, 0, 0);
+   pros::rtos::Task MotorNotification(electronic.controllers.notify_motor_disconnect);
 
+   //pros::rtos::Task LadyBrownNotification(subsystem.ladybrown.edge_check);
    //screen.selector.selector();
    pros::lcd::initialize();
    pros::Task screen_task([&]() {
@@ -112,20 +113,27 @@ void competition_initialize() {}
 void autonomous() {
 
    pros::lcd::initialize();
-   pros::Task screen_task([&]() {
-      while (true) {
-         // print robot location to the brain screen
-         pros::lcd::print(0, "X: %f", chassis.getPose().x);         // x
-         pros::lcd::print(1, "Y: %f", chassis.getPose().y);         // y
-         pros::lcd::print(2, "Theta: %f", chassis.getPose().theta); // heading
-         // delay to save resources
-         pros::lcd::print(3, "Rotation Sensor: %i", lateral_sensor.get_position());
-         pros::lcd::print(4, "Rotation Sensor: %i", horizontal_sensor.get_position());
-         pros::delay(20);
-      }
-   });
+   // pros::Task screen_task([&]() {
+   //    while (true) {
+   //       // print robot location to the brain screen
+   //       pros::lcd::print(0, "X: %f", chassis.getPose().x);         // x
+   //       pros::lcd::print(1, "Y: %f", chassis.getPose().y);         // y
+   //       pros::lcd::print(2, "Theta: %f", chassis.getPose().theta); // heading
+   //       // delay to save resources
+   //       pros::lcd::print(3, "Rotation Sensor: %i", lateral_sensor.get_position());
+   //       pros::lcd::print(4, "Rotation Sensor: %i", horizontal_sensor.get_position());
+   //       pros::delay(20);
+   //    }
+   // });
 
    subsystem.autonomous.AutoDrive(subsystem.intake, subsystem.latch);
+
+   //chassis.turnToHeading(90, 100000);
+   // chassis.turnToHeading(180, 2000);
+   //chassis.moveToPoint(0, 24, 10000);
+
+
+
 }
 
 /**
@@ -163,16 +171,6 @@ void opcontrol() {
          // 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_X)) {
-         if (LadyBrownNotification.get_state() == 2) {
-            LadyBrownNotification.suspend();
-            controller.print(0, 0, "LB-Disabled");
-         } else {
-            LadyBrownMotor.set_zero_position(LadyBrownMotor.get_position());
-            LadyBrownNotification.resume();
-            controller.print(0, 0, "LB-Enabled");
-         }
-      }
 
       subsystem.drivetrain.run();
       subsystem.latch.run();