Add berthoud prog

include/driver/motorButton.hpp

@@ -10,19 +10,35 @@ namespace Pronounce {
     private:
         bool goToImmediately = false;
 
-        bool autonomousButton = false;
+        bool autonomousPosition = false;
+
+		bool dejam = false;
+		double dejamTime = 0;
+		double dejamSpeed = 0;
+		double dejamAuthority = 0;
+		double dejamStartTime = 0;
+		double jamSpeed = 50;
+
+		bool jammed = false;
 
         int positiveAuthority = 0;
         int neutralAuthority = 0;
         int negativeAuthority = 0;
 
         int autonomousAuthority = 0;
 
+		bool goToHeight = false;
+		double height;
+		double length;
+
+		double gearRatio;
+
         int min = 0;
         int max = 0;
         pros::Motor* motor;
 
     public:
+        MotorButton();
         MotorButton(pros::Controller* controller, pros::Motor* motor,
         pros::controller_digital_e_t positiveButton = pros::E_CONTROLLER_DIGITAL_L1,
         pros::controller_digital_e_t negativeButton = pros::E_CONTROLLER_DIGITAL_L2,
@@ -31,10 +47,13 @@ namespace Pronounce {
         int negativeAuthority = 0,
         int min = 0,
         int max = 0);
-        MotorButton();
 
         void updateActuator();
 
+		void resetPosition(double currentPosition) {
+			this->motor->set_zero_position(-currentPosition);
+		}
+
         bool getGoToImmediately() {
             return this->goToImmediately;
         }
@@ -51,16 +70,91 @@ namespace Pronounce {
             this->autonomousAuthority = autonomousAuthority;
         }
 
-        bool getAutonomousButton() {
-            return this->autonomousButton;
+        bool getAutonomousPosition() {
+            return this->autonomousPosition;
         }
 
-        void setAutonomousButton(bool autonomousButton) {
-            this->autonomousButton = autonomousButton;
+        void setAutonomousPosition(bool autonomousPosition) {
+            this->autonomousPosition = autonomousPosition;
         }
 
+		bool getGoToHeight() {
+			return this->goToHeight;
+		}
+
+		void setGoToHeight(bool goToHeight) {
+			this->goToHeight = goToHeight;
+		}
+
+		double getLength(double length) {
+			return this->length;
+		}
+
+		void setLength(double length) {
+			this->length = length;
+		}
+
+		double getHeight() {
+			return height;
+		}
+
+		void setHeight(double height) {
+			this->height = height;
+		}
+
+		double getGearRatio() {
+			return gearRatio;
+		}
+
+		void setGearRatio(double gearRatio) {
+			this->gearRatio = gearRatio;
+		}
+
+		bool getJammed() {
+			return this->jammed;
+		}
+
+		void setJammed(bool jammed) {
+			this->jammed = jammed;
+		}
+
+		bool getDejam() {
+			return this->dejam;
+		}
+
+		void setDejam(bool dejam) {
+			this->dejam = dejam;
+		}
+
+		double getDejamTime() {
+			return this->dejamTime;
+		}
+
+		void setDejamTime(double dejamTime) {
+			this->dejamTime = dejamTime;
+		}
+
+		double getDejamAuthority() {
+			return this->dejamAuthority;
+		}
+
+		void setDejamAuthority(double dejamAuthority) {
+			this->dejamAuthority = dejamAuthority;
+		}
+
+		double getDejamSpeed() {
+			return this->dejamSpeed;
+		}
+
+		void setDejamSpeed(double dejamSpeed) {
+			this->dejamSpeed = dejamSpeed;
+		}
+
+		double getDejamStartTime() {
+			return this->dejamStartTime;
+		}
+
         ~MotorButton();
     };
 
 } // namespace Pronounce
-

include/feedbackControllers/bangBang.hpp

@@ -0,0 +1,65 @@
+#pragma once
+
+#include "utils/utils.hpp"
+
+namespace Pronounce {
+	class BangBang {
+	private:
+		double minimumDifference;
+		bool reversable;
+		double outputStrength;
+		double setPoint = 0;
+		double lastInput = 0;
+	public:
+		BangBang();
+		BangBang(double minimumDifference);
+		BangBang(double minimumDifference, bool reversable);
+		BangBang(double minimumDifference, bool reversable, double outputStrength);
+
+		/**
+		 * @brief Updates the controller, input the current value, and it will return the output
+		 * 
+		 * @param input The current state
+		 * @return double The output at that state
+		 */
+		double update(double input);
+
+		double getSetPoint() {
+			return setPoint;
+		}
+
+		void setSetPoint(double setPoint) {
+			this->setPoint = setPoint;
+		}
+
+		double getMinimumDifference() {
+			return minimumDifference;
+		}
+
+		void setMinimumDifference(double minimumDifference) {
+			this->minimumDifference = minimumDifference;
+		}
+
+		bool isReversable() {
+			return reversable;
+		}
+
+		void setReversable(bool reversable) {
+			this->reversable = reversable;
+		}
+
+		double getOutputStrength() {
+			return outputStrength;
+		}
+
+		void setOutputStrength(double outputStrength) {
+			this->outputStrength = outputStrength;
+		}
+
+		bool isInRange() {
+			return abs(this->setPoint - this->lastInput) > minimumDifference;
+		}
+
+		~BangBang();
+	};
+} // namespace Pronounce

include/main.h

@@ -68,7 +68,12 @@
 #include "driver/motorButton.hpp"
 #include "driver/solenoidButton.hpp"
 
+// FeedbackControllers
+#include "feedbackControllers/bangBang.hpp"
+#include "feedbackControllers/pid.hpp"
+
 // Motion control
+#include "motionControl/balance.hpp"
 #include "motionControl/purePursuit.hpp"
 #include "motionControl/omniPurePursuit.hpp"
 #include "motionControl/tankPurePursuit.hpp"
@@ -79,9 +84,6 @@
 #include "odometry/threeWheelOdom.hpp"
 #include "odometry/tankOdom.hpp"
 
-// PID
-#include "pid/pid.hpp"
-
 // Position
 #include "position/motorOdom.hpp"
 #include "position/odomWheel.hpp"

include/motionControl/balance.hpp

@@ -0,0 +1,81 @@
+#pragma once
+
+#include "feedbackControllers/bangBang.hpp"	
+#include "feedbackControllers/pid.hpp"
+#include "chassis/abstractTankDrivetrain.hpp"
+#include "api.h"
+
+namespace Pronounce {
+	class Balance
+	{
+	private:
+		BangBang* linearController;
+		PID* orientationController;
+
+		bool enabled = false;
+
+		AbstractTankDrivetrain* drivetrain;
+		pros::Imu* imu;
+	public:
+		Balance();
+		Balance(AbstractTankDrivetrain* drivetrain);
+		Balance(AbstractTankDrivetrain* drivetrain, pros::Imu* imu);
+		Balance(AbstractTankDrivetrain* drivetrain, pros::Imu* imu, BangBang* linearController, PID* orientationController);
+
+		void update();
+
+		void balance(double waitTime) {
+			while(!this->isBalanced()) {
+				this->update();
+				pros::Task::delay(waitTime);
+			}
+		}
+
+		bool isBalanced() {
+			return linearController->isInRange();
+		}
+
+		BangBang* getLinearController() {
+			return this->linearController;
+		}
+
+		void setLinearController(BangBang* linearController) {
+			this->linearController = linearController;
+		}
+
+		PID* getOrientationController() {
+			return this->orientationController;
+		}
+
+		void setOrientationController(PID* orientationController) {
+			this->orientationController = orientationController;
+		}
+
+		bool isEnabled() {
+			return this->enabled;
+		}
+
+		void setEnabled(bool enabled) {
+			this->enabled = enabled;
+		}
+
+		AbstractTankDrivetrain* getDrivetrain() {
+			return this->drivetrain;
+		}
+
+		void setDrivetrain(AbstractTankDrivetrain* drivetrain) {
+			this->drivetrain = drivetrain;
+		}
+
+		pros::Imu* getImu() {
+			return this->imu;
+		}
+
+		void setImu(pros::Imu* imu) {
+			this->imu = imu;
+		}
+
+
+		~Balance();
+	};
+} // namespace Pronounce

include/motionControl/purePursuit.hpp

@@ -4,7 +4,7 @@
 #include <math.h>
 #include <algorithm>
 #include <iostream>
-#include "pid/pid.hpp"
+#include "feedbackControllers/pid.hpp"
 #include "utils/path.hpp"
 #include "utils/position.hpp"
 #include "utils/utils.hpp"

include/utils/purePursuitProfile.hpp

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "pid/pid.hpp"
+#include "feedbackControllers/pid.hpp"
 
 namespace Pronounce {
     class PurePursuitProfile {