広田 勇斗 / Mbed 2 deprecated Omni_2017_a

Omni_2017_a

Dependencies:   mbed

Fork of Omni_2017_z by 広田 勇斗

2017_3/2017_3_z.cpp@0:fd933ea5f19d, 2017-08-05 (annotated)

Committer:
hirotayamato
Date:
Sat Aug 05 02:29:14 2017 +0000
Revision:
0:fd933ea5f19d
Omni_2017_z

;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hirotayamato 0:fd933ea5f19d 1 #include "2017_3_z.h"
hirotayamato 0:fd933ea5f19d 2 #include "mbed.h"
hirotayamato 0:fd933ea5f19d 3 #include <math.h>
hirotayamato 0:fd933ea5f19d 4
hirotayamato 0:fd933ea5f19d 5 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,
hirotayamato 0:fd933ea5f19d 6 PinName pin_pwm_L1, PinName pin_pwm_L2, PinName pin_dere_L1, PinName pin_dere_L2, PinName pin_channelA_L, PinName pin_channelB_L,
hirotayamato 0:fd933ea5f19d 7 PinName pin_pwm_R1, PinName pin_pwm_R2, PinName pin_dere_R1, PinName pin_dere_R2, PinName pin_channelA_R, PinName pin_channelB_R,
hirotayamato 0:fd933ea5f19d 8 int arg_rev)
hirotayamato 0:fd933ea5f19d 9 {
hirotayamato 0:fd933ea5f19d 10 md_f1 = Create_MD_PID(pin_channelA_F, pin_channelB_F, NC, 500, QEI::X4_ENCODING,
hirotayamato 0:fd933ea5f19d 11 0.095 * 2, 0, 0.002 * 4, 500,
hirotayamato 0:fd933ea5f19d 12 pin_pwm_F1, pin_dere_F1);
hirotayamato 0:fd933ea5f19d 13
hirotayamato 0:fd933ea5f19d 14 md_f2 = Create_MD_PID(pin_channelA_F, pin_channelB_F, NC, 500, QEI::X4_ENCODING,
hirotayamato 0:fd933ea5f19d 15 0.095 * 2, 0, 0.002 * 4, 500,
hirotayamato 0:fd933ea5f19d 16 pin_pwm_F2, pin_dere_F2);
hirotayamato 0:fd933ea5f19d 17
hirotayamato 0:fd933ea5f19d 18 md_l1 = Create_MD_PID(pin_channelA_L, pin_channelB_L, NC, 500, QEI::X4_ENCODING,
hirotayamato 0:fd933ea5f19d 19 0.095 * 2, 0, 0.002 * 4, 500,
hirotayamato 0:fd933ea5f19d 20 pin_pwm_L1, pin_dere_L1);
hirotayamato 0:fd933ea5f19d 21
hirotayamato 0:fd933ea5f19d 22 md_l2 = Create_MD_PID(pin_channelA_L, pin_channelB_L, NC, 500, QEI::X4_ENCODING,
hirotayamato 0:fd933ea5f19d 23 0.095 * 2, 0, 0.002 * 4, 500,
hirotayamato 0:fd933ea5f19d 24 pin_pwm_L2, pin_dere_L2);
hirotayamato 0:fd933ea5f19d 25
hirotayamato 0:fd933ea5f19d 26 md_r1 = Create_MD_PID(pin_channelA_R, pin_channelB_R, NC, 500, QEI::X4_ENCODING,
hirotayamato 0:fd933ea5f19d 27 0.095 * 2, 0, 0.002 * 4, 500,
hirotayamato 0:fd933ea5f19d 28 pin_pwm_R1, pin_dere_R1);
hirotayamato 0:fd933ea5f19d 29
hirotayamato 0:fd933ea5f19d 30 md_r2 = Create_MD_PID(pin_channelA_R, pin_channelB_R, NC, 500, QEI::X4_ENCODING,
hirotayamato 0:fd933ea5f19d 31 0.095 * 2, 0, 0.002 * 4, 500,
hirotayamato 0:fd933ea5f19d 32 pin_pwm_R2, pin_dere_R2);
hirotayamato 0:fd933ea5f19d 33
hirotayamato 0:fd933ea5f19d 34 rev = arg_rev;
hirotayamato 0:fd933ea5f19d 35 }
hirotayamato 0:fd933ea5f19d 36
hirotayamato 0:fd933ea5f19d 37 void Omni_3::Drive( double arg_x, double arg_y, double arg_rota,
hirotayamato 0:fd933ea5f19d 38 double F_Gain, double L_Gain, double R_Gain)
hirotayamato 0:fd933ea5f19d 39 {
hirotayamato 0:fd933ea5f19d 40 double dere, sp;
hirotayamato 0:fd933ea5f19d 41 double duty[3];
hirotayamato 0:fd933ea5f19d 42 double speed[3];
hirotayamato 0:fd933ea5f19d 43
hirotayamato 0:fd933ea5f19d 44 dere = atan2(arg_y, arg_x);
hirotayamato 0:fd933ea5f19d 45 sp = sqrt(pow(arg_x, 2.0) + pow(arg_y, 2.0));
hirotayamato 0:fd933ea5f19d 46
hirotayamato 0:fd933ea5f19d 47 speed[0] = cos(dere) * sp; // vx
hirotayamato 0:fd933ea5f19d 48 speed[1] = sin(dere) * sp; // vy
hirotayamato 0:fd933ea5f19d 49 speed[2] = arg_rota; // omega
hirotayamato 0:fd933ea5f19d 50
hirotayamato 0:fd933ea5f19d 51 Matrix(speed, duty);
hirotayamato 0:fd933ea5f19d 52 for(int i = 0; i < 3; i++){
hirotayamato 0:fd933ea5f19d 53 if(fabs(duty[i]) > 1.0){
hirotayamato 0:fd933ea5f19d 54 int inv = fabs(duty[i]);
hirotayamato 0:fd933ea5f19d 55 for(int j = 0; j < 3; j++){
hirotayamato 0:fd933ea5f19d 56 duty[j] *= 1.0 / (double)inv;
hirotayamato 0:fd933ea5f19d 57 }
hirotayamato 0:fd933ea5f19d 58 }
hirotayamato 0:fd933ea5f19d 59 }
hirotayamato 0:fd933ea5f19d 60 md_f1->Drive(duty[0] * F_Gain, 100);
hirotayamato 0:fd933ea5f19d 61 md_f2->Drive(duty[0] * F_Gain, 100);
hirotayamato 0:fd933ea5f19d 62 md_l1->Drive(duty[1] * L_Gain, 100);
hirotayamato 0:fd933ea5f19d 63 md_l2->Drive(duty[1] * L_Gain, 100);
hirotayamato 0:fd933ea5f19d 64 md_r1->Drive(duty[2] * R_Gain, 100);
hirotayamato 0:fd933ea5f19d 65 md_r2->Drive(duty[2] * R_Gain, 100);
hirotayamato 0:fd933ea5f19d 66 }
hirotayamato 0:fd933ea5f19d 67
hirotayamato 0:fd933ea5f19d 68 void Omni_3::Matrix(double speed[3], double duty[3])
hirotayamato 0:fd933ea5f19d 69 {
hirotayamato 0:fd933ea5f19d 70 double keisu[3][3];
hirotayamato 0:fd933ea5f19d 71 keisu[0][0] = -1.0;
hirotayamato 0:fd933ea5f19d 72 keisu[0][1] = 0.0;
hirotayamato 0:fd933ea5f19d 73 keisu[0][2] = 1.0;
hirotayamato 0:fd933ea5f19d 74 keisu[1][0] = 1.0 / 2.0;
hirotayamato 0:fd933ea5f19d 75 keisu[1][1] = -sqrt(3.0) / 2.0;
hirotayamato 0:fd933ea5f19d 76 keisu[1][2] = 1.0;
hirotayamato 0:fd933ea5f19d 77 keisu[2][0] = 1.0 / 2.0;
hirotayamato 0:fd933ea5f19d 78 keisu[2][1] = sqrt(3.0) / 2.0;
hirotayamato 0:fd933ea5f19d 79 keisu[2][2] = 1.0;
hirotayamato 0:fd933ea5f19d 80
hirotayamato 0:fd933ea5f19d 81 for(int i = 0; i < 3; i++){
hirotayamato 0:fd933ea5f19d 82 duty[i] = 0.0;
hirotayamato 0:fd933ea5f19d 83 for(int j = 0; j < 3; j++){
hirotayamato 0:fd933ea5f19d 84 duty[i] += keisu[i][j] * speed[j] * rev;
hirotayamato 0:fd933ea5f19d 85 }
hirotayamato 0:fd933ea5f19d 86 }
hirotayamato 0:fd933ea5f19d 87 }