Diff: shared/Wheel/Wheel.cpp

Revision:

0:ee7e9405e1c7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Wheel/Wheel.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,48 @@
+#include "Wheel.h"
+
+Wheel::Wheel(MD *md, double wheel_angle, double position_angle)
+{
+    this->md = md;
+    this->wheel_angle = wheel_angle;
+    this->wheel_radius = 0;
+    
+    this->position_angle = position_angle;
+    this->position_radius = 1;
+}
+
+Wheel::Wheel(MD *md, double wheel_angle, double position_x, double position_y, double wheel_radius)
+{
+    this->md = md;
+    this->wheel_angle = wheel_angle;
+    this->wheel_radius = wheel_radius;
+    
+    set_position(position_x, position_y);
+}
+    
+void Wheel::set_position(double position_x, double position_y)
+{
+    position_angle = atan2(position_y, position_x);
+    position_radius = sqrt( position_y * position_y + position_x * position_x );
+}
+    
+double Wheel::wheel_speed(double move_angle, double move_radius, double yaw)
+{
+    double speed;
+    double moving, rotating;
+    
+    while(move_angle > M_PI)
+        move_angle -= 2.0 * M_PI;
+    
+    moving = cos(wheel_angle - move_angle) * move_radius;
+    
+    rotating = sin(wheel_angle - position_angle) * yaw * position_radius;
+    
+    speed = moving + rotating;
+    
+    return speed;
+}
+
+void Wheel::drive(double duty)
+{
+    md->drive(duty);
+}