2017_3_h.cpp

00001 #include "2017_3_h.h"
00002 #include "mbed.h"
00003 #include <math.h>
00004 
00005 Omni_3::Omni_3(PinName pin_pwm_F, PinName pin_dere_F, PinName pin_channelA_F, PinName pin_channelB_F,
00006                PinName pin_pwm_L, PinName pin_dere_L, PinName pin_channelA_L, PinName pin_channelB_L,
00007                PinName pin_pwm_R, PinName pin_dere_R, PinName pin_channelA_R, PinName pin_channelB_R,
00008                int arg_rev)
00009 {
00010     md_f = Create_MD_PID(pin_channelA_F, pin_channelB_F, NC, 500, QEI::X4_ENCODING,
00011                          0.095 * 0.125, 0, 0.000125 * 4, 500,
00012                          pin_pwm_F, pin_dere_F);
00013 
00014     md_l = Create_MD_PID(pin_channelA_L, pin_channelB_L, NC, 500, QEI::X4_ENCODING,
00015                          0.095 * 0.125, 0, 0.000125 * 4, 500,
00016                          pin_pwm_L, pin_dere_L);
00017 
00018     md_r = Create_MD_PID(pin_channelA_R, pin_channelB_R, NC, 500, QEI::X4_ENCODING,
00019                          0.095 * 0.125, 0, 0.000125 * 4, 500,
00020                          pin_pwm_R, pin_dere_R);
00021 
00022     rev = arg_rev;
00023 }
00024 
00025 void Omni_3::Drive( int arg_a, double arg_x, double arg_y, double arg_rota)
00026 {
00027     double dere, sp;
00028     double duty[3];
00029     double speed[3];
00030 
00031     dere = atan2(arg_y, arg_x);
00032     sp = pow(arg_x * arg_x + arg_y * arg_y, 0.60);
00033 
00034     speed[0] = cos(dere) * sp;   // vx
00035     speed[1] = sin(dere) * sp;   // vy
00036     speed[2] = arg_rota;         // omega
00037 
00038     Matrix(speed, duty);
00039     for(int i = 0; i < 3; i++){
00040         if(fabs(duty[i]) > 1.0){
00041             int inv = fabs(duty[i]);
00042             for(int j = 0; j < 3; j++){
00043                 duty[j] *= 1.0 / (double)inv;
00044             }
00045         }
00046     }
00047     //if(arg_a){
00048         md_f->Drive(arg_a, duty[0], 100);
00049         md_l->Drive(arg_a, duty[1], 100);
00050         md_r->Drive(arg_a, duty[2], 100);
00051     /*}else{
00052         md_f->Drive(arg_a, duty[0], 100);
00053         md_l->Drive(arg_a, duty[1], 100);
00054         md_r->Drive(arg_a, duty[2], 100);
00055     }*/
00056 }
00057 
00058 void Omni_3::Matrix(double speed[3], double duty[3])
00059 {
00060     double keisu[3][3];
00061     keisu[0][0] = 1.0;
00062     keisu[0][1] = 0.0;
00063     keisu[0][2] = 1.0;
00064     keisu[1][0] = -1.0 / 2.0;
00065     keisu[1][1] = -sqrt(3.0) / 2.0;
00066     keisu[1][2] = 1.0;
00067     keisu[2][0] = -1.0 / 2.0;
00068     keisu[2][1] = sqrt(3.0) / 2.0;
00069     keisu[2][2] = 1.0;
00070 
00071     for(int i = 0; i < 3; i++){
00072         duty[i] = 0.0;
00073         for(int j = 0; j < 3; j++){
00074             duty[i] += keisu[i][j] * speed[j] * rev;
00075         }
00076     }
00077 }