Diff: bot/wheel_unit/wheel_unit.cpp

Revision:

26:7258d5ad0bff

Parent:

25:d199d621ecca

--- a/bot/wheel_unit/wheel_unit.cpp	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/wheel_unit/wheel_unit.cpp	Wed Oct 25 03:25:06 2017 +0900
@@ -1,16 +1,18 @@
 #include "wheel_unit.h"
 
 WheelUnit::WheelUnit(DigitalOut* RS485Controller, Serial* RS485) :
-	Omni(4),
+	omni(4),
 	wheels({
-		{RS485Controller, 0, 1, SM, RS485},
-		{RS485Controller, 0, 2, SM, RS485},
-		{RS485Controller, 0, 3, SM, RS485},
-		{RS485Controller, 0, 0, SM, RS485}
+		ikarashiMDC({RS485Controller, 0, 1, SM, RS485}),
+		ikarashiMDC({RS485Controller, 0, 2, SM, RS485}),
+		ikarashiMDC({RS485Controller, 0, 3, SM, RS485}),
+		ikarashiMDC({RS485Controller, 0, 0, SM, RS485})
 	})
 {
-	Omni::setWheelRadian(PI / 4.0, 3 * PI / 4.0, 5 * PI / 4.0, 7 * PI / 4.0);
-	//Omni::setWheelRadian(PI / 4.0, PI / 4.0, PI / 4.0, PI / 4.0);
+	omni.wheel[0].setRadian(PI / 4.0 * 5.0);
+	omni.wheel[1].setRadian(PI / 4.0 * 7.0);
+	omni.wheel[2].setRadian(PI / 4.0 * 1.0);
+	omni.wheel[3].setRadian(PI / 4.0 * 3.0);
 	for(int i = 0; i < 4; i++) {
 		wheels[i].braking = true;
 	}
@@ -18,16 +20,32 @@
 
 void WheelUnit::moveXY(float X, float Y, float moment)
 {
-	Omni::computeXY(X, Y, moment);
+	omni.computeXY(X, Y, moment);
 	for(int i = 0; i < 4; i++) {
-		wheels[i].setSpeed(Omni::getOutput(i));
+		wheels[i].setSpeed(omni.wheel[i]);
+	}
+}
+
+void WheelUnit::moveXY(float X, float Y, float gX, float gY, float moment)
+{
+	omni.computeXY(X, Y, gX, gY, moment);
+	for(int i = 0; i < 4; i++) {
+		wheels[i].setSpeed(omni.wheel[i]);
 	}
 }
 
 void WheelUnit::moveCircular(float r, float radian, float moment)
 {
-	Omni::computeCircular(r, radian, moment);
+	omni.computeCircular(r, radian, moment);
 	for(int i = 0; i < 4; i++) {
-		wheels[i].setSpeed(Omni::getOutput(i));
+		wheels[i].setSpeed(omni.wheel[i]);
 	}
 }
+
+void WheelUnit::moveCircular(float r, float radian, float gX, float gY, float moment)
+{
+	omni.computeCircular(r, radian, gX, gY, moment);
+	for(int i = 0; i < 4; i++) {
+		wheels[i].setSpeed(omni.wheel[i]);
+	}
+}