Diff: 2017_3/2017_3_z.cpp

Revision:

2:82c337a18500

Parent:

1:fa8227369eb0

--- a/2017_3/2017_3_z.cpp	Sat Aug 05 02:36:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,87 +0,0 @@
-#include "2017_3_z.h"
-#include "mbed.h"
-#include <math.h>
-
-Omni_3::Omni_3(PinName pin_pwm_F1, PinName pin_pwm_F2, PinName pin_dere_F1, PinName pin_dere_F2, PinName pin_channelA_F, PinName pin_channelB_F,
-               PinName pin_pwm_L1, PinName pin_pwm_L2, PinName pin_dere_L1, PinName pin_dere_L2, PinName pin_channelA_L, PinName pin_channelB_L,
-               PinName pin_pwm_R1, PinName pin_pwm_R2, PinName pin_dere_R1, PinName pin_dere_R2, PinName pin_channelA_R, PinName pin_channelB_R,
-               int arg_rev)
-{
-    md_f1 = Create_MD_PID(pin_channelA_F, pin_channelB_F, NC, 500, QEI::X4_ENCODING, 
-                         0.095 * 2, 0, 0.002 * 4, 500, 
-                         pin_pwm_F1, pin_dere_F1);
-                         
-    md_f2 = Create_MD_PID(pin_channelA_F, pin_channelB_F, NC, 500, QEI::X4_ENCODING,
-    					 0.095 * 2, 0, 0.002 * 4, 500,
-    					 pin_pwm_F2, pin_dere_F2);
-    
-    md_l1 = Create_MD_PID(pin_channelA_L, pin_channelB_L, NC, 500, QEI::X4_ENCODING,
-    					 0.095 * 2, 0, 0.002 * 4, 500,
-    					 pin_pwm_L1, pin_dere_L1);
-    
-    md_l2 = Create_MD_PID(pin_channelA_L, pin_channelB_L, NC, 500, QEI::X4_ENCODING,
-    					 0.095 * 2, 0, 0.002 * 4, 500,
-    					 pin_pwm_L2, pin_dere_L2);
-    
-    md_r1 = Create_MD_PID(pin_channelA_R, pin_channelB_R, NC, 500, QEI::X4_ENCODING,
-    					 0.095 * 2, 0, 0.002 * 4, 500,
-    					 pin_pwm_R1, pin_dere_R1);
-    					 
-    md_r2 = Create_MD_PID(pin_channelA_R, pin_channelB_R, NC, 500, QEI::X4_ENCODING,
-    					 0.095 * 2, 0, 0.002 * 4, 500,
-    					 pin_pwm_R2, pin_dere_R2);
-    
-    rev = arg_rev;
-}
-
-void Omni_3::Drive( double arg_x, double arg_y, double arg_rota,
-					double F_Gain, double L_Gain, double R_Gain)
-{
-    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_f1->Drive(duty[0] * F_Gain, 100);
-    md_f2->Drive(duty[0] * F_Gain, 100);
-    md_l1->Drive(duty[1] * L_Gain, 100);
-    md_l2->Drive(duty[1] * L_Gain, 100);
-    md_r1->Drive(duty[2] * R_Gain, 100);
-    md_r2->Drive(duty[2] * R_Gain, 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