Diff: bot/wheel_unit/wheel_unit.cpp

Revision:

23:37bb9afe9fdc

Child:

24:9a4ca5442717

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bot/wheel_unit/wheel_unit.cpp	Wed Sep 13 14:26:47 2017 +0900
@@ -0,0 +1,32 @@
+#include "wheel_unit.h"
+
+WheelUnit::WheelUnit(DigitalOut* RS485Controller, Serial* RS485) :
+	Omni(4),
+	wheels({
+		{RS485Controller, 0, 0, SM, RS485},
+		{RS485Controller, 0, 1, SM, RS485},
+		{RS485Controller, 0, 2, SM, RS485},
+		{RS485Controller, 0, 3, SM, RS485}
+	})
+{
+	Omni::setWheelRadian(PI / 2.0, 3 * PI / 2.0, 5 * PI / 2.0, 7 * PI / 2.0);
+	for(int i = 0; i < 4; i++) {
+		wheels[i].braking = true;
+	}
+}
+
+void WheelUnit::moveXY(float X, float Y, float moment)
+{
+	Omni::computeXY(X, Y, moment);
+	for(int i = 0; i < 4; i++) {
+		wheels[i].setSpeed(Omni::getOutput(i));
+	}
+}
+
+void WheelUnit::moveCircular(float r, float radian, float moment)
+{
+	Omni::computeCircular(r, radian, moment);
+	for(int i = 0; i < 4; i++) {
+		wheels[i].setSpeed(Omni::getOutput(i));
+	}
+}