Diff: wheelUnit.cpp

Revision:

0:0d52f303ac08

Child:

1:3c9eaf598f21

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wheelUnit.cpp	Mon Jul 08 08:45:38 2019 +0000
@@ -0,0 +1,25 @@
+#include "wheelUnit.h"
+
+WheelUnit::WheelUnit(Serial* serial) :
+    omni(4),
+    pc(USBTX, USBRX, 115200)
+{
+    md = new ikarashiMDC*[4];
+    for(int i = 0; i < 4; ++i) md[i] = new ikarashiMDC(2,i,SM,serial);
+    for(int i = 0; i < 4; ++i) md[i]->braking = true;
+    for(int i = 0; i < 4; i++) omni.wheel[i].setRadian(PI / 4.0 * (2.0*i+1.0));
+}
+
+void WheelUnit::move(float x,float y,float turnPower)
+{
+    omni.computeXY(x,y,turnPower);
+    for (int i = 0; i < 4; ++i) md[i]->setSpeed(omni.wheel[i]);
+    print = omni.wheel[0];
+    pc.printf("%f\n\r",print);
+}
+
+void WheelUnit::stop()
+{
+    for (int i = 0; i < 4; ++i) md[i]->setSpeed(0);
+    pc.printf("0\n\r");
+}