State machine improvements

README.md

@@ -2,4 +2,8 @@
 
 ![example workflow](https://github.com/ad101-lab/pronounce-this/actions/workflows/c-cpp.yml/badge.svg)
 
-This is our programming repository for 2021-2022 VEX Competition Tipping Point.  
+This is our programming repository for 2022-2023 VEX Competition Spin up.  
+
+## Libraries used
+
+pros-grafana-lib - [Github link](https://github.com/BWHS-Robotics/pros-grafana-cli)

firmware/squiggles.mk

@@ -0,0 +1 @@
+INCLUDE+=-iquote"$(ROOT)/include/okapi/squiggles"

firmware/v5-common.ld

@@ -21,7 +21,7 @@ SECTIONS
    *(.gnu.linkonce.t.*)
    *(.plt)
    *(.gnu_warning)
-   *(.gcc_execpt_table)
+   *(.gcc_except_table)
    *(.glue_7)
    *(.glue_7t)
    *(.vfp11_veneer)

include/api.h

@@ -8,7 +8,7 @@
  * This file should not be modified by users, since it gets replaced whenever
  * a kernel upgrade occurs.
  *
- * Copyright (c) 2017-2021, Purdue University ACM SIGBots.
+ * Copyright (c) 2017-2022, Purdue University ACM SIGBots.
  * All rights reserved.
  *
  * This Source Code Form is subject to the terms of the Mozilla Public
@@ -40,9 +40,9 @@
 #endif /* __cplusplus */
 
 #define PROS_VERSION_MAJOR 3
-#define PROS_VERSION_MINOR 5
-#define PROS_VERSION_PATCH 4
-#define PROS_VERSION_STRING "3.5.4"
+#define PROS_VERSION_MINOR 6
+#define PROS_VERSION_PATCH 0
+#define PROS_VERSION_STRING "3.6.0"
 
 #define PROS_ERR (INT32_MAX)
 #define PROS_ERR_F (INFINITY)
@@ -53,6 +53,7 @@
 #include "pros/ext_adi.h"
 #include "pros/gps.h"
 #include "pros/imu.h"
+#include "pros/link.h"
 #include "pros/llemu.h"
 #include "pros/misc.h"
 #include "pros/motors.h"
@@ -75,6 +76,7 @@
 #include "pros/rtos.hpp"
 #include "pros/screen.hpp"
 #include "pros/vision.hpp"
+#include "pros/link.hpp"
 #endif
 
 #endif  // _PROS_API_H_

include/display/lv_misc/lv_color.h

@@ -378,7 +378,11 @@ static inline uint8_t lv_color_brightness(lv_color_t color)
 #    define LV_COLOR_MAKE(r8, g8, b8) ((lv_color_t){{g8 >> 5, r8 >> 3, b8 >> 3, (g8 >> 2) & 0x7}})
 #  endif
 #elif LV_COLOR_DEPTH == 32
-#define LV_COLOR_MAKE(r8, g8, b8) ((lv_color_t){{b8, g8, r8, 0xff}})            /*Fix 0xff alpha*/
+#ifdef __cplusplus
+#  define LV_COLOR_MAKE(r8, g8, b8) lv_color_t{b8, g8, r8, 0xff}
+#else 
+#  define LV_COLOR_MAKE(r8, g8, b8) ((lv_color_t){{b8, g8, r8, 0xff}})            /*Fix 0xff alpha*/
+#endif
 #endif
 #else
 #if LV_COLOR_DEPTH == 1

include/feedbackControllers/feedbackController.hpp

@@ -0,0 +1,48 @@
+
+
+namespace Pronounce {
+	class FeedbackController {
+	private:
+	protected:
+		double target = 0.0;
+		double position = 0.0;
+		double power;
+		double maxPower = 1.0;
+	public:
+		FeedbackController(/* args */) {}
+
+		virtual double update(double input) { return 0; }
+
+		virtual void reset() {}
+
+		void setTarget(double target) {
+			this->target = target;
+		}
+
+		double getTarget() {
+			return target;
+		}
+
+		void setPosition(double target) {
+			this->target = target;
+		}
+
+		double getPosition() {
+			return position;
+		}
+
+		double getPower() {
+			return power;
+		}
+
+		void setMaxPower(double maxPower) {
+			this->maxPower = maxPower;
+		}
+
+		double getMaxPower() {
+			return maxPower;
+		}
+
+		~FeedbackController() {}
+	};
+} // namespace Pronounce

include/feedbackControllers/flywheelPID.hpp

@@ -0,0 +1,19 @@
+#pragma once
+
+#include "feedbackController.hpp"
+#include "pid.hpp"
+
+namespace Pronounce {
+	class FlywheelPID : public PID {
+	private:
+		double feedforwardMultiplier;
+	public:
+		FlywheelPID() {}
+
+		double update(double input) {
+			return calculatePidValues(input) + input * feedforwardMultiplier;
+		}
+
+		~FlywheelPID() {}
+	};
+} // namespace Pronounce

include/feedbackControllers/pid.hpp

@@ -2,130 +2,134 @@
 
 #include <cmath>
 #include "utils/utils.hpp"
+#include "feedbackController.hpp"
 
 namespace Pronounce {
-	
+
 	/**
 	 * @brief PID class
-	 * 
+	 *
 	 * @author ad101-lab
-	 * 
+	 *
 	 * Hopefully adding FF later
-	 * 
+	 *
 	 */
-    class PID {
-    private:
-        double kP;
-        double kI;
-        double kD;
-
-        double target = 0.0;
-        double position;
+	class PID : public FeedbackController {
+	private:
+		double kP;
+		double kI;
+		double kD;
 
-        double error;
-        double totalError = 0.0;
-        double prevError = 0.0;
-        double derivitive;
+		double error;
+		double totalError = 0.0;
+		double prevError = 0.0;
+		double derivitive;
 
-        double integralBound = 30.0;
-        double maxIntegral = 0.3;
+		double integralBound = 30.0;
+		double maxIntegral = 0.3;
 
-        double power;
+		double power;
 
 		bool turnPid = false;
-    public:
-        PID();
-        PID(double kP, double kI, double kD, double target = 0, double position = 0, bool turnPid = false);
-
-        double update();
-
-        void operator=(PID pid) {
-            this->kP = pid.getKP();
-            this->kI = pid.getKI();
-            this->kD = pid.getKD();
-            this->integralBound = pid.getIntegralBound();
-            this->maxIntegral = pid.getMaxIntegral();
-        }
-
-        void operator=(PID* pid) {
-            this->kP = pid->getKP();
-            this->kI = pid->getKI();
-            this->kD = pid->getKD();
-            this->integralBound = pid->getIntegralBound();
-            this->maxIntegral = pid->getMaxIntegral();
-        }
-
-        double getDerivitive() {
-            return derivitive;
-        }
-
-        double getError() {
-            return error;
-        }
-
-        double getPower() {
-            return power;
-        }
-
-        void setPower(double power) {
-            this->power = power;
-        }
-
-        double getPosition() {
-            return this->position;
-        }
-
-        void setPosition(double position) {
-            this->position = position;
-        }
-
-        double getTarget() {
-            return target;
-        }
-
-        void setTarget(double target) {
-            this->target = target;
-        }
-
-        double getKP() {
-            return kP;
-        }
-
-        void setKP(double kP) {
-            this->kP = kP;
-        }
-
-        double getKI() {
-            return kI;
-        }
-
-        void setKI(double kI) {
-            this->kI = kI;
-        }
-
-        double getKD() {
-            return kD;
-        }
-
-        void setKD(double kD) {
-            this->kD = kD;
-        }
-
-        double getIntegralBound() {
-            return this->integralBound;
-        }
-
-        void setIntegralBound(double integralBound) {
-            this->integralBound = integralBound;
-        }
-
-        double getMaxIntegral() {
-            return this->maxIntegral;
-        }
-
-        void setMaxIntegral(double maxIntegral) {
-            this->maxIntegral = maxIntegral;
-        }
+
+	protected:
+		double calculatePidValues(double input) {
+			position = input;
+
+			if (turnPid) {
+				this->error = angleDifference(target, position);
+			}
+			else {
+				this->error = target - position;
+			}
+
+			this->derivitive = error - prevError;
+
+			if (abs(error) < integralBound) {
+				totalError += error;
+			}
+			else {
+				totalError = 0;
+			}
+
+			totalError = abs(totalError) > maxIntegral ? signnum_c(totalError) * maxIntegral : totalError;
+
+			this->power = error * kP + derivitive * kD + totalError * kI;
+
+			prevError = error;
+
+			return this->power;
+		}
+
+	public:
+		PID();
+		PID(double kP, double kI, double kD, double target = 0, double position = 0, bool turnPid = false);
+
+		double update(double input);
+
+		void operator=(PID pid) {
+			this->kP = pid.getKP();
+			this->kI = pid.getKI();
+			this->kD = pid.getKD();
+			this->integralBound = pid.getIntegralBound();
+			this->maxIntegral = pid.getMaxIntegral();
+		}
+
+		void operator=(PID* pid) {
+			this->kP = pid->getKP();
+			this->kI = pid->getKI();
+			this->kD = pid->getKD();
+			this->integralBound = pid->getIntegralBound();
+			this->maxIntegral = pid->getMaxIntegral();
+		}
+
+		double getDerivitive() {
+			return derivitive;
+		}
+
+		double getError() {
+			return error;
+		}
+
+		double getKP() {
+			return kP;
+		}
+
+		void setKP(double kP) {
+			this->kP = kP;
+		}
+
+		double getKI() {
+			return kI;
+		}
+
+		void setKI(double kI) {
+			this->kI = kI;
+		}
+
+		double getKD() {
+			return kD;
+		}
+
+		void setKD(double kD) {
+			this->kD = kD;
+		}
+
+		double getIntegralBound() {
+			return this->integralBound;
+		}
+
+		void setIntegralBound(double integralBound) {
+			this->integralBound = integralBound;
+		}
+
+		double getMaxIntegral() {
+			return this->maxIntegral;
+		}
+
+		void setMaxIntegral(double maxIntegral) {
+			this->maxIntegral = maxIntegral;
+		}
 
 		bool getTurnPid() {
 			return turnPid;
@@ -141,8 +145,8 @@ namespace Pronounce {
 			this->derivitive = 0;
 		}
 
-        ~PID();
-    };
+		~PID();
+	};
 } // namespace Pronounce