Diff: DifferentialDriveKinematics.h

Revision:

2:baa00f631c7e

Child:

3:7b00b653cbce

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DifferentialDriveKinematics.h	Tue Apr 14 19:30:37 2020 +0000
@@ -0,0 +1,120 @@
+#ifndef DIFFERENTIAL_DRIVE_KINEMATICS_H
+#define DIFFERENTIAL_DRIVE_KINEMATICS_H
+
+#include<sstream>
+#include<string>
+
+#include "Matrix.h"
+#include "Motor.h"
+#include "HallEffectEncoder.h"
+
+
+
+class DifferentialDriveKinematics {
+public:
+    DifferentialDriveKinematics(float x, float y, float theta, Motor left_motor, Motor right_motor, 
+                                // PinName pin_enc_l, PinName pin_enc_r, float encoder_speed_period ,
+                                HallEffectEncoder& enc_l_, HallEffectEncoder& enc_r_,
+                                float Ts, float controller_precision);
+
+    
+    
+    // Passive functions, used to get speed, angle, and other stuff
+    void updateStates(float speed_left, float speed_right);
+    
+    float getAngleFromTarget(float x_target, float y_target);
+    
+    float getSpeedFromTarget(float x_target, float y_target);
+    
+    float controlAngle(float angle, float target_angle);
+    
+    bool getWheelSpeedFromLinearAngularSpeed(float linear_speed, float angular_speed, float* wheel_speeds);
+    
+    float getPWMFromSpeedRight(float speed);
+    float getPWMFromSpeedLeft(float speed);
+    
+    
+    //Logger
+    void init_logger(RawSerial* pc_);
+    void log();
+    std::string getLog();
+    
+    // active functions, like follow path and stuff
+    bool goToAngle(float angle);
+    
+    bool followPath(float path[100][2], int length_array, RawSerial* pc);
+    
+    bool goToStart(float start_x, float start_y, float next_target_x, float next_target_y);
+    
+    void kalmanPredictUpdate(float speed_left, float speed_right); // the fuction will only rely the attributes of the class
+    
+    float getPwmFromMotorSpeed(float speed_motor);
+    
+    
+    float getX();
+    float getY();
+    float getTheta();
+    
+    float getR();
+    float getL();
+
+
+private:
+
+    //Logger
+    Ticker log_ticker;
+    // RawSerial* pc;
+
+
+    float _x;
+    float _y;
+    float _theta;
+    float Ts;
+    
+    // the motors
+    Motor m_r;
+    Motor m_l;
+    
+    // the wheel encoders
+    HallEffectEncoder& enc_l;
+    HallEffectEncoder& enc_r;
+    
+    float R; // Radius of the wheel
+    float L; // length between the wheels
+    
+    // gains used to control the angle of the robot
+    float Kp;
+    float Ki;
+    float Kd;
+    
+    // errors used to control the angle of the robot
+    float previous_error;
+    float steady_error;
+    
+    // used for the controller
+    float precision;
+    
+    
+    // this is used to remove the accoup at the beginning
+    bool isItFirstExecControlAngle;
+    
+    // those are used for Kalman
+    Matrix Pk;
+    Matrix Fk;
+    Matrix Hk; // Hk is I33
+    Matrix Qk;
+    Matrix Rk;
+    
+    
+    
+    // const for the motor profiles
+    const float A_L = 80.95987857;
+    const float B_L = -4.598798777003484;
+    const float A_R = 68.53560796;
+    const float B_R = -3.283267993031359;
+    
+};
+
+
+
+#endif
\ No newline at end of file