Slew for odom is actually fixed now

Fixed some stuff in the example project, fixed slew constants, and commented out unused variables in tracking

EZ-Template-Example-Project/src/main.cpp

@@ -96,8 +96,8 @@ void autonomous() {
   chassis.drive_sensor_reset();               // Reset drive sensors to 0
   chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency
   chassis.odom_pose_set({0, 0, 0});
-  chassis.drive_width_set(8.93);  // just use a tape measure
-  chassis.dlead = 0.375;
+  chassis.drive_width_set(11);  // just use a tape measure
+  chassis.dlead = 0.5;
   chassis.odometry_enabled = true;
 
   chassis.pid_odom_smooth_pp_set({{{0, 16, 45}, fwd, 110},
@@ -149,8 +149,8 @@ void opcontrol() {
       chassis.pid_tuner_iterate();  // Allow PID Tuner to iterate
     }
 
-    // chassis.opcontrol_tank();  // Tank control
-    chassis.opcontrol_arcade_standard(ez::SPLIT);  // Standard split arcade
+    chassis.opcontrol_tank();  // Tank control
+    // chassis.opcontrol_arcade_standard(ez::SPLIT);   // Standard split arcade
     // chassis.opcontrol_arcade_standard(ez::SINGLE);  // Standard single arcade
     // chassis.opcontrol_arcade_flipped(ez::SPLIT);    // Flipped split arcade
     // chassis.opcontrol_arcade_flipped(ez::SINGLE);   // Flipped single arcade

src/EZ-Template/drive/drive.cpp

@@ -164,8 +164,8 @@ void Drive::drive_defaults_set() {
 
   // Slew constants
   slew_turn_constants_set(3_deg, 70);
-  slew_drive_constants_set(3_in, 80);
-  slew_drive_constants_set(3_in, 80);
+  slew_drive_constants_set(3_in, 70);
+  slew_swing_constants_set(3_in, 80);
 
   // Exit condition constants
   pid_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms);

src/EZ-Template/drive/set_pid.cpp

@@ -357,7 +357,7 @@ void Drive::raw_pid_odom_ptp_set(odom imovement, bool slew_on) {
   PID::Constants pid_consts;
   slew::Constants slew_consts;
   PID::Constants angle_consts = headingPID.constants_get();
-  if (imovement.drive_direction == REV) {
+  if (current_drive_direction == REV) {
     pid_consts = backward_drivePID.constants_get();
     slew_consts = slew_backward.constants_get();
 
@@ -386,16 +386,10 @@ void Drive::raw_pid_odom_ptp_set(odom imovement, bool slew_on) {
   // Get the starting point for if we're positive or negative.  This is used to find if we've past target
   past_target = util::sgn(is_past_target(odom_target, odom_current));
 
-  // Initialize slew
-  int dir = current_drive_direction == REV ? -1 : 1;  // If we're going backwards, add a -1
-  double dist_to_target = util::distance_to_point(odom_target, odom_current) * dir;
-  dist_to_target = dist_to_target < slew_consts.distance_to_travel && mode == PURE_PURSUIT ? slew_consts.distance_to_travel : dist_to_target;
-  slew_left.initialize(slew_on, max_speed, dist_to_target + l_start, l_start);
-  slew_right.initialize(slew_on, max_speed, dist_to_target + r_start, r_start);
-
   // This is used for wait_until
-  leftPID.target_set(l_start + dist_to_target);
-  rightPID.target_set(l_start + dist_to_target);
+  int dir = current_drive_direction == REV ? -1 : 1;  // If we're going backwards, add a -1
+  leftPID.target_set(l_start + (LOOK_AHEAD * dir));
+  rightPID.target_set(l_start + (LOOK_AHEAD * dir));
   leftPID.exit = xyPID.exit;  // Switch over to xy pid exits
   rightPID.exit = xyPID.exit;
 }
@@ -408,11 +402,18 @@ void Drive::pid_odom_ptp_set(odom imovement, bool slew_on) {
   xyPID.timers_reset();
   aPID.timers_reset();
 
-  // This is used for wait_until
+  // This is used for wait_until and slew
   l_start = drive_sensor_left();
   r_start = drive_sensor_right();
 
   raw_pid_odom_ptp_set(imovement, slew_on);
+
+  // Initialize slew
+  int dir = current_drive_direction == REV ? -1 : 1;  // If we're going backwards, add a -1
+  double dist_to_target = util::distance_to_point(odom_target, odom_current) * dir;
+  slew_left.initialize(slew_on, max_speed, dist_to_target + l_start, l_start);
+  slew_right.initialize(slew_on, max_speed, dist_to_target + r_start, r_start);
+
   drive_mode_set(POINT_TO_POINT);
 }
 
@@ -432,10 +433,16 @@ void Drive::raw_pid_odom_pp_set(std::vector<odom> imovements, bool slew_on) {
 
   raw_pid_odom_ptp_set(pp_movements[pp_index], slew_on);
 
-  // This is used for wait_until
+  // This is used for wait_until and slew
   l_start = drive_sensor_left();
   r_start = drive_sensor_right();
 
+  // Initialize slew
+  int dir = current_drive_direction == REV ? -1 : 1;  // If we're going backwards, add a -1
+  double dist_to_target = util::distance_to_point(pp_movements.end()->target, odom_current) * dir;
+  slew_left.initialize(slew_on, max_speed, dist_to_target + l_start, l_start);
+  slew_right.initialize(slew_on, max_speed, dist_to_target + r_start, r_start);
+
   drive_mode_set(PURE_PURSUIT);
 }
 

src/EZ-Template/drive/tracking.cpp

@@ -45,15 +45,15 @@ void Drive::ez_tracking_task() {
   // while (true) {
   // printf("x: %.2f   y: %.2f   a: %.2f\n", odom_current.x, odom_current.y, odom_current.theta);
 
-  float l_current = drive_sensor_left();
+  // float l_current = drive_sensor_left();
   float r_current = drive_sensor_right();
   // c_current = get_raw_center();
 
-  float l_ = l_current - l_last;
+  // float l_ = l_current - l_last;
   float r_ = r_current - r_last;
   // float c = c_current - c_last;
 
-  l_last = l_current;
+  // l_last = l_current;
   r_last = r_current;
   // c_last = c_current;
 
@@ -63,7 +63,7 @@ void Drive::ez_tracking_task() {
   last_theta = current_global_theta;
 
   float beta = 0.0;
-  float h = l_;
+  float h = r_;
   if (theta != 0) {
     float radius_r = r_ / theta;
     beta = theta / 2.0;

src/autons.cpp

@@ -30,8 +30,8 @@ void default_constants() {
   chassis.pid_drive_chain_constant_set(3_in);
 
   chassis.slew_turn_constants_set(3_deg, 70);
-  chassis.slew_drive_constants_set(3_in, 80);
-  chassis.slew_drive_constants_set(3_in, 80);
+  chassis.slew_drive_constants_set(3_in, 70);
+  chassis.slew_swing_constants_set(3_in, 80);
 }
 
 ///