広田 勇斗 / Mbed 2 deprecated 2017_Bteam_beta_master_ashi

2017_Bteam_beta_master_ashi

Dependencies:   Alpha_Movements BoolProcess DataCaller_beta MD_PID mbed

Fork of 2017_Bteam_beta_master by taiyou komazawa

2017_3_h/2017_3_h.cpp@15:c78ef3aa8f5d, 2017-09-29 (annotated)

Committer:
hirotayamato
Date:
Fri Sep 29 00:33:48 2017 +0000
Revision:
15:c78ef3aa8f5d
Parent:
12:dbff655e398e 
2017_Bteam_beta_master_ashi

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hirotayamato 1:ebb68f50a49d 1 #include "2017_3_h.h"
hirotayamato 1:ebb68f50a49d 2 #include "mbed.h"
hirotayamato 1:ebb68f50a49d 3 #include <math.h>
hirotayamato 1:ebb68f50a49d 4
hirotayamato 1:ebb68f50a49d 5 Omni_3::Omni_3(PinName pin_pwm_F, PinName pin_dere_F, PinName pin_channelA_F, PinName pin_channelB_F,
hirotayamato 1:ebb68f50a49d 6 PinName pin_pwm_L, PinName pin_dere_L, PinName pin_channelA_L, PinName pin_channelB_L,
hirotayamato 1:ebb68f50a49d 7 PinName pin_pwm_R, PinName pin_dere_R, PinName pin_channelA_R, PinName pin_channelB_R,
hirotayamato 1:ebb68f50a49d 8 int arg_rev)
hirotayamato 1:ebb68f50a49d 9 {
hirotayamato 1:ebb68f50a49d 10 md_f = Create_MD_PID(pin_channelA_F, pin_channelB_F, NC, 500, QEI::X4_ENCODING,
hirotayamato 10:68987291c3b4 11 0.095 * 0.125, 0, 0.000125 * 4, 500,
hirotayamato 1:ebb68f50a49d 12 pin_pwm_F, pin_dere_F);
hirotayamato 1:ebb68f50a49d 13
hirotayamato 1:ebb68f50a49d 14 md_l = Create_MD_PID(pin_channelA_L, pin_channelB_L, NC, 500, QEI::X4_ENCODING,
hirotayamato 10:68987291c3b4 15 0.095 * 0.125, 0, 0.000125 * 4, 500,
hirotayamato 1:ebb68f50a49d 16 pin_pwm_L, pin_dere_L);
hirotayamato 1:ebb68f50a49d 17
hirotayamato 1:ebb68f50a49d 18 md_r = Create_MD_PID(pin_channelA_R, pin_channelB_R, NC, 500, QEI::X4_ENCODING,
hirotayamato 10:68987291c3b4 19 0.095 * 0.125, 0, 0.000125 * 4, 500,
hirotayamato 1:ebb68f50a49d 20 pin_pwm_R, pin_dere_R);
hirotayamato 1:ebb68f50a49d 21
hirotayamato 1:ebb68f50a49d 22 rev = arg_rev;
hirotayamato 1:ebb68f50a49d 23 }
hirotayamato 1:ebb68f50a49d 24
hirotayamato 12:dbff655e398e 25 void Omni_3::Drive( int arg_a, double arg_x, double arg_y, double arg_rota)
hirotayamato 1:ebb68f50a49d 26 {
hirotayamato 1:ebb68f50a49d 27 double dere, sp;
hirotayamato 1:ebb68f50a49d 28 double duty[3];
hirotayamato 1:ebb68f50a49d 29 double speed[3];
hirotayamato 1:ebb68f50a49d 30
hirotayamato 1:ebb68f50a49d 31 dere = atan2(arg_y, arg_x);
hirotayamato 1:ebb68f50a49d 32 sp = pow(arg_x * arg_x + arg_y * arg_y, 0.60);
hirotayamato 1:ebb68f50a49d 33
hirotayamato 1:ebb68f50a49d 34 speed[0] = cos(dere) * sp; // vx
hirotayamato 1:ebb68f50a49d 35 speed[1] = sin(dere) * sp; // vy
hirotayamato 1:ebb68f50a49d 36 speed[2] = arg_rota; // omega
hirotayamato 1:ebb68f50a49d 37
hirotayamato 1:ebb68f50a49d 38 Matrix(speed, duty);
hirotayamato 1:ebb68f50a49d 39 for(int i = 0; i < 3; i++){
hirotayamato 1:ebb68f50a49d 40 if(fabs(duty[i]) > 1.0){
hirotayamato 1:ebb68f50a49d 41 int inv = fabs(duty[i]);
hirotayamato 1:ebb68f50a49d 42 for(int j = 0; j < 3; j++){
hirotayamato 1:ebb68f50a49d 43 duty[j] *= 1.0 / (double)inv;
hirotayamato 1:ebb68f50a49d 44 }
hirotayamato 1:ebb68f50a49d 45 }
hirotayamato 1:ebb68f50a49d 46 }
hirotayamato 12:dbff655e398e 47 //if(arg_a){
hirotayamato 12:dbff655e398e 48 md_f->Drive(arg_a, duty[0], 100);
hirotayamato 12:dbff655e398e 49 md_l->Drive(arg_a, duty[1], 100);
hirotayamato 12:dbff655e398e 50 md_r->Drive(arg_a, duty[2], 100);
hirotayamato 12:dbff655e398e 51 /*}else{
hirotayamato 12:dbff655e398e 52 md_f->Drive(arg_a, duty[0], 100);
hirotayamato 12:dbff655e398e 53 md_l->Drive(arg_a, duty[1], 100);
hirotayamato 12:dbff655e398e 54 md_r->Drive(arg_a, duty[2], 100);
hirotayamato 12:dbff655e398e 55 }*/
hirotayamato 1:ebb68f50a49d 56 }
hirotayamato 1:ebb68f50a49d 57
hirotayamato 1:ebb68f50a49d 58 void Omni_3::Matrix(double speed[3], double duty[3])
hirotayamato 1:ebb68f50a49d 59 {
hirotayamato 1:ebb68f50a49d 60 double keisu[3][3];
hirotayamato 1:ebb68f50a49d 61 keisu[0][0] = 1.0;
hirotayamato 1:ebb68f50a49d 62 keisu[0][1] = 0.0;
hirotayamato 1:ebb68f50a49d 63 keisu[0][2] = 1.0;
hirotayamato 3:d7ce443fe117 64 keisu[1][0] = -1.0 / 2.0;
hirotayamato 15:c78ef3aa8f5d 65 keisu[1][1] = -sqrt(3.0) / 2.0;
hirotayamato 1:ebb68f50a49d 66 keisu[1][2] = 1.0;
hirotayamato 3:d7ce443fe117 67 keisu[2][0] = -1.0 / 2.0;
hirotayamato 15:c78ef3aa8f5d 68 keisu[2][1] = sqrt(3.0) / 2.0;
hirotayamato 1:ebb68f50a49d 69 keisu[2][2] = 1.0;
hirotayamato 1:ebb68f50a49d 70
hirotayamato 1:ebb68f50a49d 71 for(int i = 0; i < 3; i++){
hirotayamato 1:ebb68f50a49d 72 duty[i] = 0.0;
hirotayamato 1:ebb68f50a49d 73 for(int j = 0; j < 3; j++){
hirotayamato 1:ebb68f50a49d 74 duty[i] += keisu[i][j] * speed[j] * rev;
hirotayamato 1:ebb68f50a49d 75 }
hirotayamato 1:ebb68f50a49d 76 }
hirotayamato 1:ebb68f50a49d 77 }