Diff: mechanism.cpp

Revision:

3:ff9bb9f8fabe

Parent:

1:58b20a731e15

--- a/mechanism.cpp	Fri Sep 13 03:01:24 2019 +0000
+++ b/mechanism.cpp	Fri Oct 04 08:15:54 2019 +0000
@@ -8,7 +8,10 @@
     md = new ikarashiMDC*[12];
     for(int i = 0; i < 4; i++) md[i] = new ikarashiMDC(1,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));
+    omni.wheel[0].setRadian(PI / 4.0 * 1.0);
+    omni.wheel[1].setRadian(PI / 4.0 * 3.0);
+    omni.wheel[2].setRadian(PI / 4.0 * 5.0);
+    omni.wheel[3].setRadian(PI / 4.0 * 7.0);
     for(int i = 4; i < 8; i++)md[i] = new ikarashiMDC(2,i-4,SM,serial);
     for(int i = 8; i < 11; i++)md[i] = new ikarashiMDC(3,i-7,SM,serial);
     for(int i = 4; i < 12; i++)md[i]-> braking = true;
@@ -160,9 +163,10 @@
     brake = trigger / 255.0;
 }
 
-void Mechanism::move(float x,float y,float turnPower)
+void Mechanism::move(float x,float y,float turnPower,float nowAngle, float norm)
 {
-    omni.computeXY(x,y,turnPower);
+    rad = atan2(y,x) + (nowAngle * (PI / 180.0));
+    omni.computeXY(norm * cos(rad),norm * sin(rad),turnPower);
     for (int i = 0; i < 4; ++i) wheel[i] = omni.wheel[i];
     for (int i = 0; i < 4; ++i) md[i]->setSpeed(wheel[i] * brake);
 }