Diff: Robot_Library/robot.h

Branch:

a-star

Revision:

61:9b1fc2bc7172

Parent:

51:884fca7f02de

Parent:

60:85a40e69ced6

Child:

63:4a9836ef3be0

--- a/Robot_Library/robot.h	Thu May 26 13:31:40 2022 +0200
+++ b/Robot_Library/robot.h	Mon May 30 09:37:28 2022 +0200
@@ -12,6 +12,7 @@
 
     public:
         Robot();
+        ~Robot();
         void Update();
 
     private:
@@ -30,9 +31,24 @@
         // Finite state machine functions
         void Initial();
         void Idle();
-        void FollowingLine();
-        void LeftTurn();
-        void RightTurn();
+        void FollowingLine(); // takes in rotational velocity?
+        void LeftTurn_90();
+        void RightTurn_90();
+
+        //PID
+
+        void PID_Move(std::uint8_t s_binary);
+        void PID_Delay(int ms);
+        bool IsSharpTurn(int b);
+
+        const float k_prop = 0.5f;
+        const float t_int = 0.5f;
+        const float t_deriv = 0.5f;
+        const float error_angles[4] = {0, 1, 2, 3};
+        const float error_scale = 10;
+
+        float previous_error_value;
+        float integral_error;
 
         //bool motors_enabled;
 
@@ -51,7 +67,7 @@
         const float COUNTS_PER_TURN = 64.0f * 19.0f;    // define counts per turn at gearbox end: counts/turn * gearratio
         const float KN = 530.0f / 12.0f;      
 
-        Eigen::Matrix2f wheel_to_robot;// Transoformation matrix
+        Eigen::Matrix2f wheel_to_robot;// Transformation matrix
         Eigen::Matrix2f robot_to_wheel;
         /* HARDWARE SETUP */
 
@@ -74,7 +90,7 @@
         EncoderCounter encoder_M1; // create encoder objects to read in the encoder counter values
         EncoderCounter encoder_M2;
 
-        Motion* trajectoryPlanners[2]; // two directory planners, 1 for each motor
+        Motion* trajectoryPlanners[2]; // two trajectory planners, 1 for each motor
         SpeedController* speedControllers[2];
 
         Eigen::Vector2f robot_speed_desired;    // Robot speed vector [x_dt, alpha_dt] in [m/s] and [rad/s]
@@ -88,7 +104,7 @@
         SDFileSystem sd;
         bool sd_on;
 
- // TODO, pas in motors as seperate objects n stuff ? not sure
+ // TODO, pass in motors as separate objects and stuff? not sure
 
 };