広田 勇斗 / Mbed 2 deprecated ALPHA_A

ALPHA_A

Dependencies:   DataPool MD_PID mbed

2017_4/2017_4.cpp@3:42fdfd41f96a, 2017-09-07 (annotated)

Committer:
hirotayamato
Date:
Thu Sep 07 00:37:39 2017 +0000
Revision:
3:42fdfd41f96a
Parent:
1:3082e5f990cd
Child:
4:169a7b6c0138
ALPHA_A

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hirotayamato 0:e77e7d7bbae0 1 #include "2017_4.h"
hirotayamato 0:e77e7d7bbae0 2 #include "mbed.h"
hirotayamato 0:e77e7d7bbae0 3 #include <math.h>
hirotayamato 0:e77e7d7bbae0 4
hirotayamato 0:e77e7d7bbae0 5 Mekanamu_4::Mekanamu_4(PinName pin_pwm_FR, PinName pin_dere_FR, PinName pin_channelA_FR, PinName pin_channelB_FR,
hirotayamato 0:e77e7d7bbae0 6 PinName pin_pwm_FL, PinName pin_dere_FL, PinName pin_channelA_FL, PinName pin_channelB_FL,
hirotayamato 0:e77e7d7bbae0 7 PinName pin_pwm_BL, PinName pin_dere_BL, PinName pin_channelA_BL, PinName pin_channelB_BL,
hirotayamato 0:e77e7d7bbae0 8 PinName pin_pwm_BR, PinName pin_dere_BR, PinName pin_channelA_BR, PinName pin_channelB_BR,
hirotayamato 0:e77e7d7bbae0 9 int arg_rev)
hirotayamato 0:e77e7d7bbae0 10 {
hirotayamato 3:42fdfd41f96a 11 md_fr = Create_MD_PID(pin_channelA_FR, pin_channelB_FR, NC, 500, QEI::X4_ENCODING,
hirotayamato 3:42fdfd41f96a 12 0.095 * 2, 0, 0.002 * 4, 500,
hirotayamato 0:e77e7d7bbae0 13 pin_pwm_FR, pin_dere_FR);
hirotayamato 3:42fdfd41f96a 14
hirotayamato 0:e77e7d7bbae0 15 md_fl = Create_MD_PID(pin_channelA_FL, pin_channelB_FL, NC, 500, QEI::X4_ENCODING,
hirotayamato 0:e77e7d7bbae0 16 0.095 * 2, 0, 0.002 * 4, 500,
hirotayamato 0:e77e7d7bbae0 17 pin_pwm_FL, pin_dere_FL);
hirotayamato 3:42fdfd41f96a 18
hirotayamato 0:e77e7d7bbae0 19 md_bl = Create_MD_PID(pin_channelA_BL, pin_channelB_BL, NC, 500, QEI::X4_ENCODING,
hirotayamato 0:e77e7d7bbae0 20 0.095 * 2, 0, 0.002 * 4, 500,
hirotayamato 0:e77e7d7bbae0 21 pin_pwm_BL, pin_dere_BL);
hirotayamato 3:42fdfd41f96a 22
hirotayamato 0:e77e7d7bbae0 23 md_br = Create_MD_PID(pin_channelA_BR, pin_channelB_BR, NC, 500, QEI::X4_ENCODING,
hirotayamato 0:e77e7d7bbae0 24 0.095 * 2, 0, 0.002 * 4, 500,
hirotayamato 0:e77e7d7bbae0 25 pin_pwm_BR, pin_dere_BR);
hirotayamato 3:42fdfd41f96a 26
hirotayamato 0:e77e7d7bbae0 27 rev = arg_rev;
hirotayamato 0:e77e7d7bbae0 28 }
hirotayamato 0:e77e7d7bbae0 29
hirotayamato 0:e77e7d7bbae0 30 void Mekanamu_4::Drive( double arg_x, double arg_y, double arg_rota,
hirotayamato 0:e77e7d7bbae0 31 double F_Gain, double B_Gain, double R_Gain, double L_Gain)
hirotayamato 0:e77e7d7bbae0 32 {
hirotayamato 0:e77e7d7bbae0 33 double dere, sp;
hirotayamato 0:e77e7d7bbae0 34 double duty[4];
hirotayamato 0:e77e7d7bbae0 35 double speed[3];
hirotayamato 3:42fdfd41f96a 36
hirotayamato 0:e77e7d7bbae0 37 dere = atan2(arg_y, arg_x);
hirotayamato 1:3082e5f990cd 38 sp = pow(arg_x * arg_x + arg_y * arg_y, 0.60);
hirotayamato 3:42fdfd41f96a 39
hirotayamato 0:e77e7d7bbae0 40 speed[0] = cos(dere) * sp; // vx
hirotayamato 0:e77e7d7bbae0 41 speed[1] = sin(dere) * sp; // vy
hirotayamato 3:42fdfd41f96a 42 speed[2] = arg_rota; // omega
hirotayamato 3:42fdfd41f96a 43
hirotayamato 0:e77e7d7bbae0 44 Matrix(speed, duty);
hirotayamato 0:e77e7d7bbae0 45 for(int i = 0; i < 4; i ++){
hirotayamato 0:e77e7d7bbae0 46 if(fabs(duty[i]) > 1.0){
hirotayamato 0:e77e7d7bbae0 47 int inv = fabs(duty[i]);
hirotayamato 0:e77e7d7bbae0 48 for(int j = 0; j < 3; j ++){
hirotayamato 0:e77e7d7bbae0 49 duty[j] *= 1.0 / (double)inv;
hirotayamato 0:e77e7d7bbae0 50 }
hirotayamato 0:e77e7d7bbae0 51 }
hirotayamato 0:e77e7d7bbae0 52 }
hirotayamato 0:e77e7d7bbae0 53 md_fr->Drive(duty[0] * F_Gain, 100);
hirotayamato 0:e77e7d7bbae0 54 md_fl->Drive(duty[1] * B_Gain, 100);
hirotayamato 0:e77e7d7bbae0 55 md_bl->Drive(duty[2] * R_Gain, 100);
hirotayamato 0:e77e7d7bbae0 56 md_br->Drive(duty[3] * L_Gain, 100);
hirotayamato 0:e77e7d7bbae0 57 }
hirotayamato 0:e77e7d7bbae0 58
hirotayamato 0:e77e7d7bbae0 59 void Mekanamu_4::Matrix(double speed[3], double duty[4])
hirotayamato 0:e77e7d7bbae0 60 {
hirotayamato 0:e77e7d7bbae0 61 double keisu[4][3];
hirotayamato 0:e77e7d7bbae0 62 keisu[0][0] = sqrt(2.0) / 2.0;
hirotayamato 0:e77e7d7bbae0 63 keisu[0][1] = -sqrt(2.0) / 2.0;
hirotayamato 0:e77e7d7bbae0 64 keisu[0][2] = 1.0;
hirotayamato 0:e77e7d7bbae0 65 keisu[1][0] = sqrt(2.0) / 2.0;
hirotayamato 0:e77e7d7bbae0 66 keisu[1][1] = sqrt(2.0) / 2.0;
hirotayamato 0:e77e7d7bbae0 67 keisu[1][2] = 1.0;
hirotayamato 0:e77e7d7bbae0 68 keisu[2][0] = -sqrt(2.0) / 2.0;
hirotayamato 0:e77e7d7bbae0 69 keisu[2][1] = sqrt(2.0) / 2.0;
hirotayamato 0:e77e7d7bbae0 70 keisu[2][2] = 1.0;
hirotayamato 0:e77e7d7bbae0 71 keisu[3][0] = -sqrt(2.0) / 2.0;
hirotayamato 0:e77e7d7bbae0 72 keisu[3][1] = -sqrt(2.0) / 2.0;
hirotayamato 0:e77e7d7bbae0 73 keisu[3][2] = 1.0;
hirotayamato 3:42fdfd41f96a 74
hirotayamato 0:e77e7d7bbae0 75 for(int i = 0; i < 4; i ++){
hirotayamato 0:e77e7d7bbae0 76 duty[i] = 0;
hirotayamato 0:e77e7d7bbae0 77 for(int j = 0; j < 3; j ++){
hirotayamato 0:e77e7d7bbae0 78 duty[i] += keisu[i][j] * speed[j] * rev;
hirotayamato 0:e77e7d7bbae0 79 }
hirotayamato 0:e77e7d7bbae0 80 }
hirotayamato 0:e77e7d7bbae0 81 }