Diff: shared/Diff2/Diff2.cpp

Revision:

0:ee7e9405e1c7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/shared/Diff2/Diff2.cpp	Wed Apr 14 07:26:19 2021 +0000
@@ -0,0 +1,46 @@
+#include "Diff2.h"
+
+Diff2::Diff2(MD *md[NUM])
+{
+    wheel[L] = new Wheel( md[L],  M_PI / 2.0,  M_PI);
+    wheel[R] = new Wheel( md[R], -M_PI / 2.0,  0.0);
+}
+
+Diff2::Diff2(MD *md_L, MD *md_R)
+{
+    wheel[L] = new Wheel( md_L,  M_PI / 2.0,  M_PI);
+    wheel[R] = new Wheel( md_R, -M_PI / 2.0,  0.0);
+}
+
+Diff2::Diff2(Wheel *wheel[NUM])
+{
+    for(int i = 0; i < NUM; i++){
+        this->wheel[i] = wheel[i];
+    }
+}
+
+void Diff2::move(double x, double y, double yaw)
+{
+    double move_angle, move_radius;
+    double duty[NUM];
+    double max_duty, limit_duty = 1.0;
+    
+    move_angle = atan2(y, x);
+    move_radius = sqrt( y * y + x * x );
+    
+    for(int i = 0; i < NUM; i++)
+        duty[i] = wheel[i]->wheel_speed(move_angle, move_radius, yaw);
+
+    // 平行移動と回転のうち、大きい方をmax_dutyに格納
+    if(move_radius > fabs(yaw))
+    	max_duty = move_radius;
+    else
+    	max_duty = fabs(yaw);
+
+	up_limit_balance(duty, NUM, max_duty);
+	down_limit_balance(duty, NUM, limit_duty);
+    
+    for(int i = 0; i < NUM; i++){
+        wheel[i]->drive(duty[i]);
+    }
+}
\ No newline at end of file