Diff: mecanum2017.cpp

Revision:

0:d4e9f39c6086

Child:

1:cc63373e84ec

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mecanum2017.cpp	Thu Aug 17 03:47:38 2017 +0000
@@ -0,0 +1,43 @@
+#include "mbed.h"
+#include "T_motor.h"
+#include "mecanum2017.h"
+
+mecanum2017::mecanum2017(I2C& i2c_, char addr1, char addr2, char addr3, char addr4, int phase)
+    : m1(i2c_, addr1), m2(i2c_, addr2), m3(i2c_, addr3), m4(i2c_, addr4)
+{
+    oldangle = 0.0;
+    alpha = PI / 4 + phase * (PI / 2); //phaseは初期角度の指定 右に90°で-1, 左に90°で+1
+}
+
+/* うごけー */
+void mecanum2017::move(int xdate, int ydate) //xdate, ydateは -64 ~ +64
+{
+    if(xdate == 0 && ydate == 0) stop();
+    double angle = atan2(double(ydate), double(xdate)); //入力の角度
+    if(angle == oldangle); //前回と角度が同じだったらそのまま
+    else {
+        m1.control(float(sin(angle - alpha)));
+        m2.control(float(sin(angle + alpha)));
+        m3.control(float(sin(angle - alpha)));
+        m4.control(float(sin(angle + alpha)));
+    }
+    oldangle = angle; // 今回の角度を記憶
+}
+
+/* まわれー */
+void mecanum2017::rotation(int rxdate)
+{
+    m1.control(-rxdate * 0.015625); // 1 / 64 = 0.015625
+    m2.control( rxdate * 0.015625);
+    m3.control( rxdate * 0.015625);
+    m4.control(-rxdate * 0.015625);
+}
+
+/* とまれー */
+void mecanum2017::stop()
+{
+    m1.stop();
+    m2.stop();
+    m3.stop();
+    m4.stop();
+}
\ No newline at end of file