Diff: 2017_3_h/2017_3_h.cpp

Revision:

0:b8fa7a019f5d

Child:

5:93abe8105833

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/2017_3_h/2017_3_h.cpp	Fri Aug 25 05:10:49 2017 +0000
@@ -0,0 +1,71 @@
+#include "2017_3_h.h"
+#include "mbed.h"
+#include <math.h>
+
+Omni_3::Omni_3(PinName pin_pwm_F, PinName pin_dere_F, PinName pin_channelA_F, PinName pin_channelB_F,
+               PinName pin_pwm_L, PinName pin_dere_L, PinName pin_channelA_L, PinName pin_channelB_L,
+               PinName pin_pwm_R, PinName pin_dere_R, PinName pin_channelA_R, PinName pin_channelB_R,
+               int arg_rev)
+{
+    md_f = Create_MD_PID(pin_channelA_F, pin_channelB_F, NC, 500, QEI::X4_ENCODING, 
+                         0.095 * 2, 0, 0.002 * 4, 500, 
+                         pin_pwm_F, pin_dere_F);
+
+    md_l = Create_MD_PID(pin_channelA_L, pin_channelB_L, NC, 500, QEI::X4_ENCODING,
+    					 0.095 * 2, 0, 0.002 * 4, 500,
+    					 pin_pwm_L, pin_dere_L);
+
+    md_r = Create_MD_PID(pin_channelA_R, pin_channelB_R, NC, 500, QEI::X4_ENCODING,
+    					 0.095 * 2, 0, 0.002 * 4, 500,
+    					 pin_pwm_R, pin_dere_R);
+
+    rev = arg_rev;
+}
+
+void Omni_3::Drive( double arg_x, double arg_y, double arg_rota)
+{
+    double dere, sp;
+    double duty[3];
+    double speed[3];
+
+    dere = atan2(arg_y, arg_x);
+    sp = sqrt(pow(arg_x, 2.0) + pow(arg_y, 2.0));
+
+    speed[0] = cos(dere) * sp;   // vx
+    speed[1] = sin(dere) * sp;   // vy
+    speed[2] = arg_rota;         // omega
+
+    Matrix(speed, duty);
+    for(int i = 0; i < 3; i++){
+        if(fabs(duty[i]) > 1.0){
+            int inv = fabs(duty[i]);
+            for(int j = 0; j < 3; j++){
+                duty[j] *= 1.0 / (double)inv;
+            }
+        }
+    }
+    md_f->Drive(duty[0], 100);
+    md_l->Drive(duty[1], 100);
+    md_r->Drive(duty[2], 100);
+}
+
+void Omni_3::Matrix(double speed[3], double duty[3])
+{
+    double keisu[3][3];
+    keisu[0][0] = 1.0;
+    keisu[0][1] = 0.0;
+    keisu[0][2] = 1.0;
+    keisu[1][0] = 1.0 / 2.0;
+    keisu[1][1] = sqrt(3.0) / 2.0;
+    keisu[1][2] = 1.0;
+    keisu[2][0] = 1.0 / 2.0;
+    keisu[2][1] = -sqrt(3.0) / 2.0;
+    keisu[2][2] = 1.0;
+
+    for(int i = 0; i < 3; i++){
+        duty[i] = 0.0;
+        for(int j = 0; j < 3; j++){
+            duty[i] += keisu[i][j] * speed[j] * rev;
+        }
+    }
+}
\ No newline at end of file