Diff: mecanum2017.cpp

Revision:

3:063d7878f0d1

Parent:

2:678b5128a4d6

Child:

4:d4c2e7addc77

--- a/mecanum2017.cpp	Fri Aug 18 02:10:23 2017 +0000
+++ b/mecanum2017.cpp	Fri Aug 18 07:16:50 2017 +0000
@@ -2,24 +2,46 @@
 #include "T_motor.h"
 #include "mecanum2017.h"
 
-mecanum2017::mecanum2017(I2C& i2c_, char addr1, char addr2, char addr3, char addr4, int phase)
+mecanum2017::mecanum2017(I2C& i2c_, short para1_, char addr1, short para2_, char addr2,
+        short para3_, char addr3, short para4_, char addr4, int phase) //paraはモーターの回転方向を±1で表すよ いつもは全部+1だよ
     : m1(i2c_, addr1), m2(i2c_, addr2), m3(i2c_, addr3), m4(i2c_, addr4)
 {
+    para1 = para1_;
+    para2 = para2_;
+    para3 = para3_;
+    para4 = para4_;
     oldangle = 0.0;
-    alpha = PI / 4 + phase * (PI / 2); //phaseは初期角度の指定 右に90°で-1, 左に90°で+1
+    alpha = PI / 4;
+    beta = phase * (PI / 2); //phaseは初期角度の指定 左に90°で-1, 右に90°で+1
 }
 
 /* うごけー */
 void mecanum2017::move(int xdate, int ydate) //xdate, ydateは -64 ~ +64
 {
     if(xdate == 0 && ydate == 0) free();
-    double angle = atan2(double(ydate), double(xdate)); //入力の角度
+    double angle = atan2(double(ydate), double(xdate)) + beta; //入力の角度
     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)));
+    else
+    {
+        m1.control(float(para1 * sin(angle - alpha)));
+        m2.control(float(para2 * sin(angle + alpha)));
+        m3.control(float(para3 * sin(angle - alpha)));
+        m4.control(float(para4 * sin(angle + alpha)));
+    }
+    oldangle = angle; // 今回の角度を記憶
+}
+
+/*かくどー*/
+void mecanum2017::anglemove(double angle)
+{
+    angle += beta;
+    if(angle == oldangle); //前回と角度が同じだったらそのまま
+    else
+    {
+        m1.control(float(para1 * sin(angle - alpha)));
+        m2.control(float(para2 * sin(angle + alpha)));
+        m3.control(float(para3 * sin(angle - alpha)));
+        m4.control(float(para4 * sin(angle + alpha)));
     }
     oldangle = angle; // 今回の角度を記憶
 }
@@ -27,10 +49,10 @@
 /* まわれー */
 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);
+    m1.control(para1 * -rxdate * 0.015625); // 1 / 64 = 0.015625
+    m2.control(para2 *  rxdate * 0.015625);
+    m3.control(para3 *  rxdate * 0.015625);
+    m4.control(para4 * -rxdate * 0.015625);
 }
 
 /* とまれー */