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