Diff: mecanum2017_2.cpp

Revision:

1:b92b7b117776

Parent:

0:7d6d030c6a1b

--- a/mecanum2017_2.cpp	Fri Sep 29 08:53:02 2017 +0000
+++ b/mecanum2017_2.cpp	Wed Oct 25 09:35:35 2017 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "T_motor.h"
+#include "P_motor.h"
 #include "mecanum2017_2.h"
 
 double mecanum2017_2::get_angle(short xdata, short ydata)
@@ -10,15 +11,15 @@
 
 short mecanum2017_2::get_magnitude(short xdata, short ydata, short max)
 {
-    short magnitude = (short)(sqrt((double)xdata * xdata + ydata * ydata));
+    short magnitude = (short)(sqrt((double)xdata * xdata + ydata * ydata));  
     if(magnitude > max)  return max;
     else  return magnitude;
 }
 
 short mecanum2017_2::map(short value, short fromLow, short fromHigh, short toLow, short toHigh)
 {
-    if(value < fromLow) return fromLow;
-    else if(value > fromHigh) return fromHigh;
+    if(value < fromLow) return toLow;
+    else if(value > fromHigh) return toHigh;
     return (value - fromLow) * (toHigh - toLow) / (fromHigh - fromLow) + toLow;
 }
 
@@ -26,33 +27,39 @@
     : m1(i2c_, addr[0]), m2(i2c_, addr[1]), m3(i2c_, addr[2]), m4(i2c_, addr[3])
 {
     para = para_;
-    rate = rate_;
     alpha = PI / 4;
     beta = phase * (PI / 2); //phaseは初期角度の指定 左に90°で-1, 右に90°で+1
-    for(short i = 0; i < 4; i++)
-        para[i] *= rate;
+    set_rate(rate_);
+    //speed = 1;
 } //paraはモーターの回転方向を±1で表すよ いつもは全部+1だよ
 
 /* うごけー */
 void mecanum2017_2::move(short xdata_L, short ydata_L, short xdata_R)
 {
-    xdata_L = map(xdata_L, -64, 64, -100, 100);
-    ydata_L = map(ydata_L, -64, 64, -100, 100);
     xdata_R = map(xdata_R, -64, 64, -100, 100);
-    float magnitude = get_magnitude(xdata_L, ydata_L, 100) / 100.0f;
+    float magnitude = get_magnitude(xdata_L, ydata_L, 64);
+    magnitude = map(magnitude, 8, 64, 0, 100) / 100.0f;
     double angle = get_angle(xdata_L, ydata_L);
     float move_component[2] = {magnitude * (float)sin(angle + beta - alpha),
                                magnitude * (float)sin(angle + beta + alpha)
                               };
-    float roll_component[2] = {-xdata_R * 0.9f / 100.0f, xdata_R * 0.9f / 100.0f};
-
-    if(magnitude || xdata_R)  kasoku();
-    else timer.reset();
+    float roll_component[2] = {-xdata_R * 0.5f / 100.0f, xdata_R * 0.5f / 100.0f};
+    if(xdata_R)
+    {
+        move_component[0] *= 0.5f;
+        move_component[1] *= 0.5f;
+    }
+    //if(magnitude || xdata_R)  acceleration();
+    //else speed_reset();
 
-    m1 = speed * para[0] * (move_component[0] + roll_component[0]);
-    m2 = speed * para[1] * (move_component[1] + roll_component[1]);
-    m3 = speed * para[2] * (move_component[0] + roll_component[1]);
-    m4 = speed * para[3] * (move_component[1] + roll_component[0]);
+    m1 = para[0] * (move_component[0] + roll_component[0]);
+    m2 = para[1] * (move_component[1] + roll_component[1]);
+    m3 = para[2] * (move_component[0] + roll_component[1]);
+    m4 = para[3] * (move_component[1] + roll_component[0]);
+    m1.run();
+    m2.run();
+    m3.run();
+    m4.run();
 }
 
 void mecanum2017_2::anglemove(double angle)
@@ -60,11 +67,15 @@
     float move_component[2] = {(float)sin(angle + beta - alpha),
                                (float)sin(angle + beta + alpha)
                               };
-    kasoku();
-    m1 = speed * para[0] * move_component[0];
-    m2 = speed * para[1] * move_component[1];
-    m3 = speed * para[2] * move_component[0];
-    m4 = speed * para[3] * move_component[1];
+    //acceleration();
+    m1 = para[0] * move_component[0];
+    m2 = para[1] * move_component[1];
+    m3 = para[2] * move_component[0];
+    m4 = para[3] * move_component[1];
+    m1.run();
+    m2.run();
+    m3.run();
+    m4.run();
 }
 
 /* とまれー */
@@ -74,20 +85,43 @@
     m2.stop();
     m3.stop();
     m4.stop();
-    timer.reset();
+    //speed_reset();
+}
+
+void mecanum2017_2::set_rate(double rate_)
+{
+    for(short i = 0; i < 4; i++)
+    {
+        if(para[i] > 0)
+            para[i] = rate_;
+        else if(para[i] < 0)
+            para[i] = -rate_;
+    }
 }
 
-void mecanum2017_2::kasoku()
-{
-    int time = timer.read_ms();
-    if(time < 1000)
-    {
-        if(time == 0)
-        {
-            timer.start();
-            time = 10;
-        }
-        if(time >= 1000) timer.stop();
-        speed = time / 1000.0f; //ms * 1000 / 10;
-    }
-}
\ No newline at end of file
+// void mecanum2017_2::acceleration()
+// {
+//     int time = timer.read_ms();
+//     if(time < 1000)
+//     {
+//         if(time == 0)
+//         {
+//             timer.start();
+//             time = 10;
+//         }
+//         //if(time >= 1000) timer.stop();
+//         speed = time / 1000.0f;
+//     }
+//     else
+//     {
+//         timer.stop();
+//         speed = 1.0;
+//     }
+// }
+
+// void mecanum2017_2::speed_reset()
+// {
+//     speed = 0;
+//     timer.stop();
+//     timer.reset();
+// }
\ No newline at end of file