new odom changes

include/libSTRAITIS/drivetrain/odom.hpp

@@ -0,0 +1,11 @@
+#include "main.h"
+
+namespace strait
+{
+	class Position{
+        public:
+            double x;
+            double y;
+            double theta;
+    };
+}

include/libSTRAITIS/strait.hpp

@@ -8,6 +8,7 @@
 #include "util/selector.hpp"
 
 #include "drivetrain/chassis.hpp"
+#include "drivetrain/odom.hpp"
 #include "drivetrain/opcontrol.hpp"
 #include "drivetrain/PID.hpp"
 

src/init/declaration.cpp

@@ -1,3 +1,4 @@
+#include "lemlib/chassis/trackingWheel.hpp"
 #include "main.h"
 #include "pros/motors.hpp"
 
@@ -31,9 +32,9 @@ lemlib::TrackingWheel vertical(&verticalEnc, lemlib::Omniwheel::NEW_275, -3.7);
 // drivetrain settings
 lemlib::Drivetrain drivetrain(&leftDrive, // left motor group
                               &rightDrive, // right motor group
-                              13.5, // 10 inch track width
-                              lemlib::Omniwheel::OLD_275, // using new 3.25" omnis
-                              450, // drivetrain rpm is 360
+                              12.5, // 25 hole track width
+                              lemlib::Omniwheel::NEW_325,
+                              450, // drivetrain rpm
                               2 // chase power is 2. If we had traction wheels, it would have been 8
 );
 // lateral motion controller

src/libSTRAITIS/drivetrain/odom.cpp

@@ -2,47 +2,19 @@
 
 using namespace strait;
 
-// void strait::update_odom_position(float ForwardTracker_position, float SidewaysTracker_position, float orientation_deg){
-//   // this-> always refers to the old version of the variable, so subtracting this->x from x gives delta x.
-//   float Forward_delta = ForwardTracker_position-this->ForwardTracker_position;
-//   float Sideways_delta = SidewaysTracker_position-this->SideWaysTracker_position;
-//   this->ForwardTracker_position=ForwardTracker_position;
-//   this->SideWaysTracker_position=SidewaysTracker_position;
-//   float orientation_rad = to_rad(orientation_deg);
-//   float prev_orientation_rad = to_rad(this->orientation_deg);
-//   float orientation_delta_rad = orientation_rad-prev_orientation_rad;
-//   this->orientation_deg=orientation_deg;
+double    global_robot_x; // global X
+double    global_robot_y; // global Y
+double    globalTheta;    // global theta
 
-//   float local_X_position;
-//   float local_Y_position;
+Position odom_pos;
 
-//   // All of the following lines are pretty well documented in 5225A's Into to Position Tracking 
-//   // Document at http://thepilons.ca/wp-content/uploads/2018/10/Tracking.pdf 
+double getMotorDistanceTraveled() { 
+		double avgPosition = (leftDrive.at(1).get_position() + rightDrive.at(1).get_position()) / 2;
+		return (float(avgPosition) * 3.25 * M_PI / 360); 
+	}
 
-//   if (orientation_delta_rad == 0) {
-//     local_X_position = Sideways_delta;
-//     local_Y_position = Forward_delta;
-//   } else {
-//     local_X_position = (2*sin(orientation_delta_rad/2))*((Sideways_delta/orientation_delta_rad)+SidewaysTracker_center_distance); 
-//     local_Y_position = (2*sin(orientation_delta_rad/2))*((Forward_delta/orientation_delta_rad)+ForwardTracker_center_distance);
-//   }
+int current_robot_heading() {
+  globalTheta = (inertial.get_heading());
+  return globalTheta; 
+}
 
-//   float local_polar_angle;
-//   float local_polar_length;
-
-//   if (local_X_position == 0 && local_Y_position == 0){
-//     local_polar_angle = 0;
-//     local_polar_length = 0;
-//   } else {
-//     local_polar_angle = atan2(local_Y_position, local_X_position); 
-//     local_polar_length = sqrt(pow(local_X_position, 2) + pow(local_Y_position, 2)); 
-//   }
-
-//   float global_polar_angle = local_polar_angle - prev_orientation_rad - (orientation_delta_rad/2);
-
-//   float X_position_delta = local_polar_length*cos(global_polar_angle); 
-//   float Y_position_delta = local_polar_length*sin(global_polar_angle);
-
-//   X_position+=X_position_delta;
-//   Y_position+=Y_position_delta;
-// }

src/libSTRAITIS/drivetrain/pid.cpp

@@ -7,8 +7,6 @@ double global_timeOut = 0;
 
 using namespace strait;
 
-//strait::LateralPID pid;
-
 strait::LateralPID::LateralPID() {
 	pid.kp = global_kp;
 	pid.ki = global_ki;
@@ -34,7 +32,7 @@ void strait::LateralPID::move_lateral_pid(double target, double maxSpeed, double
 	double local_timer = 0;
 
 	while (true) {
-		double error = target - leftDrive.get_positions().at(0);
+		double error = target - ((leftDrive.get_positions().at(0) + rightDrive.get_positions().at(0)) / 2);
 		integral = (integral + error);
 		derivative = (error - prevError);
 

src/libSTRAITIS/util/utilities.cpp

@@ -61,10 +61,6 @@ namespace strait
     	return pros::competition::is_connected() && !pros::competition::is_autonomous() && !pros::competition::is_disabled();
 	}
 
-	double getMotorDistanceTraveled() { 
-		return (float(leftDrive.at(1).get_position()) * 2.75 * M_PI / 360); 
-	}
-
 	float reduce_0_to_360(float angle) {
 	  while(!(angle >= 0 && angle < 360)) {
 	    if( angle < 0 ) { angle += 360; }