BETA_A
Dependencies: DataPool MD_PID mbed
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 * 2, 0, 0.002 * 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 * 2, 0, 0.002 * 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 * 2, 0, 0.002 * 4, 500, 00020 pin_pwm_R, pin_dere_R); 00021 00022 rev = arg_rev; 00023 } 00024 00025 void Omni_3::Drive( 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 md_f->Drive(duty[0], 100); 00048 md_l->Drive(duty[1], 100); 00049 md_r->Drive(duty[2], 100); 00050 } 00051 00052 void Omni_3::Matrix(double speed[3], double duty[3]) 00053 { 00054 double keisu[3][3]; 00055 keisu[0][0] = 1.0; 00056 keisu[0][1] = 0.0; 00057 keisu[0][2] = 1.0; 00058 keisu[1][0] = -1.0 / 2.0; 00059 keisu[1][1] = sqrt(3.0) / 2.0; 00060 keisu[1][2] = 1.0; 00061 keisu[2][0] = -1.0 / 2.0; 00062 keisu[2][1] = -sqrt(3.0) / 2.0; 00063 keisu[2][2] = 1.0; 00064 00065 for(int i = 0; i < 3; i++){ 00066 duty[i] = 0.0; 00067 for(int j = 0; j < 3; j++){ 00068 duty[i] += keisu[i][j] * speed[j] * rev; 00069 } 00070 } 00071 }
Generated on Mon Jul 18 2022 21:22:12 by 1.7.2