Merge pull request #165 from alexDickhans/ad-odometry

Change units
alexDickhans · Jul 26, 2022 · e979bea · e979bea
2 parents 2a4dba5 + a05256c
commit e979bea
Show file tree

Hide file tree

Showing 221 changed files with 1,961 additions and 40,885 deletions.
diff --git a/firmware/okapilib.a b/firmware/okapilib.a
diff --git a/firmware/pros-grafana-lib.a b/firmware/pros-grafana-lib.a
diff --git a/firmware/squiggles.mk b/firmware/squiggles.mk
diff --git a/include/auton/autonSelector.hpp b/include/auton/autonSelector.hpp
diff --git a/include/chassis/abstractHolonomicDrivetrain.hpp b/include/chassis/abstractHolonomicDrivetrain.hpp
@@ -11,14 +11,14 @@ namespace Pronounce
     class AbstractHolonomicDrivetrain : public AbstractDrivetrain {
     private:
     public:
-        AbstractHolonomicDrivetrain();
+        AbstractHolonomicDrivetrain() {}
 
-		virtual double getSpeed() { return 0; }
+		virtual QSpeed getSpeed() { return 0.0; }
 
         virtual void setDriveVectorVelocity(Vector vector) {}
         virtual void setDriveVectorVelocity(Vector vector, double rotation) {}
 
-        ~AbstractHolonomicDrivetrain();
+        ~AbstractHolonomicDrivetrain() {}
     };
 
 } // namespace Pronounce
diff --git a/include/chassis/abstractTankDrivetrain.hpp b/include/chassis/abstractTankDrivetrain.hpp
@@ -2,6 +2,7 @@
 
 #include "abstractDrivetrain.hpp"
 #include "utils/utils.hpp"
+#include "units/units.hpp"
 #include <iostream>
 
 namespace Pronounce {
@@ -12,7 +13,7 @@ namespace Pronounce {
 		AbstractTankDrivetrain();
 		AbstractTankDrivetrain(double trackWidth);
 
-		virtual double getSpeed() { return 0; }
+		virtual QSpeed getSpeed() { return 0.0; }
 
 		double getTrackWidth() {
 			return trackWidth;

diff --git a/include/chassis/drivetrain.hpp b/include/chassis/drivetrain.hpp
@@ -25,9 +25,7 @@ namespace Pronounce {
          */
         virtual double getSpeed() { return 0; }
 
-		~Drivetrain() {
-
-		}
+		~Drivetrain() {}
     };
 } // namespace Pronounce
 
diff --git a/include/chassis/mecanumDrivetrain.hpp b/include/chassis/mecanumDrivetrain.hpp
diff --git a/include/chassis/simDrivetrain.hpp b/include/chassis/simDrivetrain.hpp
diff --git a/include/chassis/simTankDrivetrain.hpp b/include/chassis/simTankDrivetrain.hpp
diff --git a/include/chassis/xdrive.hpp b/include/chassis/xdrive.hpp
@@ -6,7 +6,7 @@
 #include "utils/vector.hpp"
 
 namespace Pronounce {
-	class XDrive : public Drivetrain, AbstractHolonomicDrivetrain {
+	class XDrive : public Drivetrain, public AbstractHolonomicDrivetrain {
 	private:
 
 		pros::Motor* frontLeftMotor;
@@ -43,10 +43,29 @@ namespace Pronounce {
 		}
 
 		void setDriveVectorVelocity(Vector vector, double rotation) {
-			this->frontLeftMotor->move_velocity(vector.getCartesian().getY() + vector.getCartesian().getX() + rotation);
-			this->frontRightMotor->move_velocity(vector.getCartesian().getY() - vector.getCartesian().getX() - rotation);
-			this->backLeftMotor->move_velocity(vector.getCartesian().getY() - vector.getCartesian().getX() + rotation);
-			this->backRightMotor->move_velocity(vector.getCartesian().getY() + vector.getCartesian().getX() - rotation);
+			double frontLeftVelocity = (vector.getCartesian().getY() + vector.getCartesian().getX()).getValue() + rotation;
+			double frontRightVelocity = (vector.getCartesian().getY() - vector.getCartesian().getX()).getValue() - rotation;
+			double backLeftVelocity = (vector.getCartesian().getY() - vector.getCartesian().getX()).getValue() + rotation;
+			double backRightVelocity = (vector.getCartesian().getY() + vector.getCartesian().getX()).getValue() - rotation;
+
+			double maxVal = abs(frontLeftVelocity);
+			maxVal = maxVal > abs(frontRightVelocity) ? maxVal : abs(frontRightVelocity);
+			maxVal = maxVal > abs(backLeftVelocity) ? maxVal : abs(backLeftVelocity);
+			maxVal = maxVal > abs(backRightVelocity) ? maxVal : abs(backRightVelocity);
+
+			if (maxVal > 200.0) {
+				double adjustment = maxVal/200.0;
+
+				frontLeftVelocity *= adjustment;
+				frontRightVelocity *= adjustment;
+				backLeftVelocity *= adjustment;
+				backRightVelocity *= adjustment;
+			} 
+
+			this->frontLeftMotor->move_velocity(frontLeftVelocity);
+			this->frontRightMotor->move_velocity(frontRightVelocity);
+			this->backLeftMotor->move_velocity(backLeftVelocity);
+			this->backRightMotor->move_velocity(backRightVelocity);
 		}
 
 		void setDriveVectorVelocity(Vector vector) {
@@ -66,5 +85,7 @@ namespace Pronounce {
 			return total / 4;
 		}
 
+		~XDrive() {}
+
 	};
 }