Cleaning everything up

src/EZ-Template/drive/drive.cpp

@@ -199,7 +199,7 @@ void Drive::drive_defaults_set() {
   pid_heading_constants_set(11.0, 0.0, 20.0);
   pid_turn_constants_set(3.0, 0.05, 20.0, 15.0);
   pid_swing_constants_set(6.0, 0.0, 65.0);
-  pid_odom_angular_constants_set(5.5, 0.0, 62.5);
+  pid_odom_angular_constants_set(5.0, 0.0, 60.0);
   pid_odom_boomerang_constants_set(3.5, 0.0, 35.0);
   pid_turn_min_set(30);
   pid_swing_min_set(30);
@@ -213,8 +213,8 @@ void Drive::drive_defaults_set() {
   pid_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms);
   pid_swing_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms);
   pid_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 500_ms);
-  pid_odom_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 750_ms);
-  pid_odom_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 750_ms);
+  pid_odom_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 750_ms);
+  pid_odom_drive_exit_condition_set(90_ms, 1_in, 250_ms, 3_in, 500_ms, 750_ms);
 
   pid_turn_chain_constant_set(3_deg);
   pid_swing_chain_constant_set(5_deg);

src/EZ-Template/drive/pid_tasks.cpp

@@ -157,13 +157,10 @@ void Drive::swing_pid_task() {
 
 // Odom To Point Task
 void Drive::ptp_task() {
-  bool print_debug = false;
-
   // Compute slew
   slew_left.iterate(drive_sensor_left());
   slew_right.iterate(drive_sensor_right());
   double max_slew_out = fmax(slew_left.output(), slew_right.output());
-  // double new_max_slew_out = max_slew_out * 2.0;
 
   // Decide if we've past the target or not
   int dir = (current_drive_direction == REV ? -1 : 1);                                                       // If we're going backwards, add a -1
@@ -181,23 +178,14 @@ void Drive::ptp_task() {
   current_a_odomPID.compute_error(wrapped_a_target, odom_theta_get());
   // printf("shortest_a_target: %.2f      error: %.2f\n", a_target, wrapped_a_target);
 
-  // if (!print_debug) printf("kp: %.2f    ki: %.5f    kd: %.2f    start_i: %.2f        thratio: %.2f    hratio: %.2f    tratio: %.2f    angle err: %.2f\n", kp, ki, kd, kstart_i, turn_heading_ratio, heading_ratio, turning_ratio, current_a_odomPID.error);
-
-  double rawxyout = 0.0;
-  double rawaout = 0.0;
-
   // Prioritize turning by scaling xy_out down
   double xy_out = xyPID.output;
   xy_out = util::clamp(xy_out, max_slew_out);
-  if (print_debug) printf("xyt(%.2f, %.2f, %.2f)", odom_x_get(), odom_y_get(), odom_theta_get());
-  rawxyout = xy_out;
   // double scale = cos(util::to_rad(current_a_odomPID.error)) / odom_turn_bias_amount;
   double scale = 1.0 - ((1.0 - cos(util::to_rad(current_a_odomPID.error))) / odom_turn_bias_amount);  // 1 - ((1-0.7)/0.75)
   if (is_odom_turn_bias_enabled)
     xy_out *= scale;
-  if (print_debug) printf("   scaled xy out by %.2f: %.2f", scale, xy_out);
   double a_out = current_a_odomPID.output;
-  rawaout = a_out;
   // a_out = util::clamp(a_out, max_slew_out);
 
   // Scale xy_out and a_out to max speed
@@ -208,15 +196,10 @@ void Drive::ptp_task() {
     a_out *= (max_slew_out / faster_side);
   }
 
-  if (print_debug) printf("   raw xy/a(%.2f, %.2f)", rawxyout, rawaout);
-  if (print_debug) printf("   final xy/a(%.2f, %.2f)", xy_out, a_out);
-
   // Combine heading and drive
   double l_out = xy_out + a_out;
   double r_out = xy_out - a_out;
 
-  if (print_debug) printf("   raw lr(%.2f, %.2f)", l_out, r_out);
-
   // Vector scaling when combining drive and imu
   // this ensures no data is lost that would otherwise by lost in clamping
   faster_side = fmax(fabs(l_out), fabs(r_out));
@@ -225,25 +208,6 @@ void Drive::ptp_task() {
     r_out *= (max_slew_out / faster_side);
   }
 
-  /*
-  double forward = xy_out / max_slew_out;
-  double curve = a_out / max_slew_out;
-  double left_speed = forward + fabs(forward) * curve;
-  double right_speed = forward - fabs(forward) * curve;
-  // normalizes output
-  double fasterr_side = fmax(fabs(left_speed), fabs(right_speed));
-  if (fasterr_side > 1.0) {
-    left_speed /= fasterr_side;
-    right_speed /= fasterr_side;
-  }
-  l_out = left_speed * max_slew_out;
-  r_out = right_speed * max_slew_out;
-  */
-
-  if (print_debug) printf("   final lr(%.2f, %.2f)", l_out, r_out);
-
-  if (print_debug) printf("      a err: %.2f\n", current_a_odomPID.error);
-
   // printf("lr out (%.2f, %.2f)   fwd curveZ(%.2f, %.2f)   lr slew (%.2f, %.2f)\n", l_out, r_out, xy_out, a_out, slew_left.output(), slew_right.output());
   // printf("max_slew_out %.2f      headingerr: %.2f\n", max_slew_out, aPID.error);
   // printf("lr(%.2f, %.2f)   xy_raw: %.2f   xy_out: %.2f   heading_out: %.2f      max_slew_out: %.2f\n", l_out, r_out, xyPID.output, xy_out, current_a_odomPID.output, max_slew_out);

src/autons.cpp

@@ -20,16 +20,16 @@ void default_constants() {
   chassis.pid_heading_constants_set(11.0, 0.0, 20.0);        // Holds the robot straight while going forward without odom
   chassis.pid_turn_constants_set(3.0, 0.05, 20.0, 15.0);     // Turn in place constants
   chassis.pid_swing_constants_set(6.0, 0.0, 65.0);           // Swing constants
-  chassis.pid_odom_angular_constants_set(5.5, 0.0, 62.5);    // Angular control for odom motions // 5.5 62.5 // 6 72.5
-  chassis.pid_odom_boomerang_constants_set(3.5, 0.0, 35.0);  // Angular control for boomerang motions // 5.7 65.0 // 5.5 62.5 // 6 72.5
+  chassis.pid_odom_angular_constants_set(5.0, 0.0, 60.0);    // Angular control for odom motions
+  chassis.pid_odom_boomerang_constants_set(3.5, 0.0, 35.0);  // Angular control for boomerang motions
 
   // Exit conditions
   // https://ez-robotics.github.io/EZ-Template/tutorials/tuning_exit_conditions
   chassis.pid_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms);
   chassis.pid_swing_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms);
   chassis.pid_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 500_ms);
-  chassis.pid_odom_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 750_ms);
-  chassis.pid_odom_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 750_ms);
+  chassis.pid_odom_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 750_ms);
+  chassis.pid_odom_drive_exit_condition_set(90_ms, 1_in, 250_ms, 3_in, 500_ms, 750_ms);
   chassis.pid_turn_chain_constant_set(3_deg);
   chassis.pid_swing_chain_constant_set(5_deg);
   chassis.pid_drive_chain_constant_set(3_in);

src/main.cpp

@@ -22,7 +22,7 @@ ez::Drive chassis(
     {-5, -6, -7, -8},  // Left Chassis Ports (negative port will reverse it!)
     {11, 15, 16, 17},  // Right Chassis Ports (negative port will reverse it!)
 
-    21,      // IMU port
+    21,      // IMU Port
     4.125,   // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!)
     420.0);  // Wheel RPM = cartridge * (motor gear / wheel gear)
 
@@ -117,25 +117,32 @@ void autonomous() {
   chassis.pid_targets_reset();                 // Resets PID targets to 0
   chassis.drive_imu_reset();                   // Reset gyro position to 0
   chassis.drive_sensor_reset();                // Reset drive sensors to 0
-  chassis.odom_pose_set({0_in, 0_in, 0_deg});  // Reset the current position to 0
+  chassis.odom_pose_set({0_in, 0_in, 0_deg});  // Set the current position, you can start at a specific position with this
   chassis.drive_brake_set(MOTOR_BRAKE_HOLD);   // Set motors to hold.  This helps autonomous consistency
-  chassis.drive_width_set(12.0_in);
+  chassis.drive_width_set(12.0_in);            // You can measure this with a tape measure
 
-  int speed = 110;
+  // Drive to 0, 24
+  chassis.pid_odom_set({{0_in, 24_in}, fwd, 110},
+                       true);
+  chassis.pid_wait();
 
-  // chassis.pid_odom_set({{0_in, 24_in, 90_deg}, fwd, speed});
+  // Turn to face 24, 24
+  chassis.pid_turn_set({24_in, 24_in}, fwd, 110,
+                       true);
+  chassis.pid_wait();
 
-  chassis.pid_odom_set({{{0_in, 24_in}, fwd, speed},
-                        {{24_in, 24_in}, fwd, speed}},
+  // Drive to 24, 24
+  chassis.pid_odom_set({{24_in, 24_in}, fwd, 110},
                        true);
-  // chassis.pid_odom_set({{{16_in, 16_in}, fwd, speed}}, true);
   chassis.pid_wait();
-  pros::delay(250);
 
-  // chassis.pid_odom_set({{24_in, 24_in, 90_deg}, fwd, speed}, true);
-  // chassis.pid_wait();
-  // pros::delay(250);
+  // Drive backwards through 0, 24 and stop at 0, 0
+  chassis.pid_odom_set({{{0_in, 24_in}, rev, 110},
+                        {{0_in, 0_in}, rev, 110}},
+                       true);
+  chassis.pid_wait();
 
+  // Uncomment this to use the auton selector
   // ez::as::auton_selector.selected_auton_call();  // Calls selected auton from autonomous selector
 }