Diff: MyOdometer.h

Revision:

0:c177452db984

Child:

4:40167450cdf0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MyOdometer.h	Mon Nov 02 09:00:16 2020 +0000
@@ -0,0 +1,92 @@
+#ifndef MY_ODOMETER_H
+#define MY_ODOMETER_H
+
+#include "JY901.h"
+#include "define.h"
+
+class CountWheel
+{
+    public:
+    CountWheel(Timer *t)
+    {
+        _t = t;
+        _t -> start();
+    }
+    float getRadian(float frequency)
+    {
+        last_time = current_time;
+        current_time = _t -> read();
+        float dt = current_time - last_time;
+        float delta_rad = deg_per_pulse * frequency * dt * DEG_TO_RAD;
+        return delta_rad;
+    }
+    
+    private:
+    Timer *_t;
+    float last_time;
+    float current_time;
+};
+
+
+Timer tmp_t;
+CountWheel counter[4] = {CountWheel(&tmp_t), CountWheel(&tmp_t), CountWheel(&tmp_t), CountWheel(&tmp_t)};
+
+class MyOdometer
+{
+public:
+    float x;
+    float y;
+    float theta;
+    
+    float vel_x;
+    float vel_y;
+    float vel_theta;
+    
+    float wheel_frequency[4];
+
+    MyOdometer(JY901 *jy901)
+    {
+        x = 0;
+        y = 0;
+        theta = 0;
+        vel_x = 0;
+        vel_y = 0;
+        vel_theta = 0;
+        jy901_ = jy901;
+        vel_timer.start();
+    }
+    void update()
+    {
+        theta = jy901_ -> calculateAngleOnlyGyro() * DEG_TO_RAD;
+        //theta = jy901_ -> getZaxisAngle() * DEG_TO_RAD;
+        for(int i = 0; i < MOTOR_NUM; i++)
+        {
+            wheel_rad[i] = counter[i].getRadian(wheel_frequency[i]);
+        }
+        float dx = (-wheel_rad[0] + wheel_rad[1] + wheel_rad[2] - wheel_rad[3])/4 * wheel_r;//*0.3535534 * 0.67;
+        float dy = (-wheel_rad[0] - wheel_rad[1] + wheel_rad[2] + wheel_rad[3])/4 * wheel_r;//*0.3535534 * 0.67;
+        
+        x += dx * cos(theta) + dy * sin(-theta);
+        y += dy * cos(theta) + dx * sin(theta);
+        
+        vel_x = (-wheel_frequency[0] + wheel_frequency[1] + wheel_frequency[2] - wheel_frequency[3])/4.0 * deg_per_pulse * DEG_TO_RAD * wheel_r;//*0.3535534 * 0.67;
+                    
+        vel_y = (-wheel_frequency[0] - wheel_frequency[1] + wheel_frequency[2] + wheel_frequency[3])/4.0 * deg_per_pulse * DEG_TO_RAD * wheel_r;//*0.3535534 * 0.67;
+        
+        float current_time = vel_timer.read();
+        float dt = (current_time - last_time);
+        vel_theta = (theta - last_theta) / dt;
+        last_theta = theta; 
+        last_time = current_time;
+    }
+    
+private:
+    JY901 *jy901_;
+    Timer vel_timer;
+    float wheel_rad[MOTOR_NUM];
+    float last_time;
+    float last_theta;
+}; 
+
+#endif
+