Diff: 2017_4/2017_4.cpp

Revision:

0:e77e7d7bbae0

Child:

1:3082e5f990cd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/2017_4/2017_4.cpp	Wed Aug 30 05:23:13 2017 +0000
@@ -0,0 +1,81 @@
+#include "2017_4.h"
+#include "mbed.h"
+#include <math.h>
+
+Mekanamu_4::Mekanamu_4(PinName pin_pwm_FR, PinName pin_dere_FR, PinName pin_channelA_FR, PinName pin_channelB_FR,
+               		   PinName pin_pwm_FL, PinName pin_dere_FL, PinName pin_channelA_FL, PinName pin_channelB_FL,
+                       PinName pin_pwm_BL, PinName pin_dere_BL, PinName pin_channelA_BL, PinName pin_channelB_BL,
+                       PinName pin_pwm_BR, PinName pin_dere_BR, PinName pin_channelA_BR, PinName pin_channelB_BR,
+                       int arg_rev)
+{
+    md_fr = Create_MD_PID(pin_channelA_FR, pin_channelB_FR, NC, 500, QEI::X4_ENCODING, 
+                         0.095 * 2, 0, 0.002 * 4, 500, 
+                         pin_pwm_FR, pin_dere_FR);
+    
+    md_fl = Create_MD_PID(pin_channelA_FL, pin_channelB_FL, NC, 500, QEI::X4_ENCODING,
+    					 0.095 * 2, 0, 0.002 * 4, 500,
+    					 pin_pwm_FL, pin_dere_FL);
+    
+    md_bl = Create_MD_PID(pin_channelA_BL, pin_channelB_BL, NC, 500, QEI::X4_ENCODING,
+    					 0.095 * 2, 0, 0.002 * 4, 500,
+    					 pin_pwm_BL, pin_dere_BL);
+    					 
+    md_br = Create_MD_PID(pin_channelA_BR, pin_channelB_BR, NC, 500, QEI::X4_ENCODING,
+    					 0.095 * 2, 0, 0.002 * 4, 500,
+    					 pin_pwm_BR, pin_dere_BR);
+    					 
+    rev = arg_rev;
+}
+
+void Mekanamu_4::Drive( double arg_x, double arg_y, double arg_rota,
+						double F_Gain, double B_Gain, double R_Gain, double L_Gain)
+{
+    double dere, sp;
+    double duty[4];
+    double speed[3];
+    
+    dere = atan2(arg_y, arg_x);
+    sp = sqrt(arg_x * arg_x + arg_y * arg_y);
+        
+    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 < 4; 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_fr->Drive(duty[0] * F_Gain, 100);
+    md_fl->Drive(duty[1] * B_Gain, 100);
+    md_bl->Drive(duty[2] * R_Gain, 100);
+    md_br->Drive(duty[3] * L_Gain, 100);
+}
+
+void Mekanamu_4::Matrix(double speed[3], double duty[4])
+{
+    double keisu[4][3];
+    keisu[0][0] = sqrt(2.0) / 2.0;
+    keisu[0][1] = -sqrt(2.0) / 2.0;
+    keisu[0][2] = 1.0;
+    keisu[1][0] = sqrt(2.0) / 2.0;
+    keisu[1][1] = sqrt(2.0) / 2.0;
+    keisu[1][2] = 1.0;
+    keisu[2][0] = -sqrt(2.0) / 2.0;
+    keisu[2][1] = sqrt(2.0) / 2.0;
+    keisu[2][2] = 1.0;
+    keisu[3][0] = -sqrt(2.0) / 2.0;
+    keisu[3][1] = -sqrt(2.0) / 2.0;
+    keisu[3][2] = 1.0;
+    
+    for(int i = 0; i < 4; i ++){
+        duty[i] = 0;
+        for(int j = 0; j < 3; j ++){
+            duty[i] += keisu[i][j] * speed[j] * rev;
+        }
+    }
+}
\ No newline at end of file