広田 勇斗 / Mbed 2 deprecated GAMMA_A

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   DataPool MD_PID mbed

2017_3_b/2017_3_b.cpp@0:6ed41770a6b7, 2017-08-22 (annotated)

Committer:
hirotayamato
Date:
Tue Aug 22 04:55:34 2017 +0000
Revision:
0:6ed41770a6b7
Child:
2:158fea945e51
GAMMA_A;

Who changed what in which revision?

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