自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
Odmetry.cpp@7:a0b9b6e71e41, 2022-02-23 (annotated)
- Committer:
- kazumayamanaka
- Date:
- Wed Feb 23 03:03:36 2022 +0000
- Revision:
- 7:a0b9b6e71e41
- Parent:
- 2:e189d55ef292
automatic and manual control of wheels
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kazumayamanaka | 2:e189d55ef292 | 1 | #include "Odmetry.hpp" |
kazumayamanaka | 2:e189d55ef292 | 2 | |
kazumayamanaka | 2:e189d55ef292 | 3 | Odmetry::Odmetry(){ |
kazumayamanaka | 2:e189d55ef292 | 4 | dt = 0.05; |
kazumayamanaka | 2:e189d55ef292 | 5 | Radius = 395; |
kazumayamanaka | 2:e189d55ef292 | 6 | Wradius = 25; |
kazumayamanaka | 2:e189d55ef292 | 7 | XPos = 0; |
kazumayamanaka | 2:e189d55ef292 | 8 | YPos = 0; |
kazumayamanaka | 2:e189d55ef292 | 9 | } |
kazumayamanaka | 2:e189d55ef292 | 10 | |
kazumayamanaka | 2:e189d55ef292 | 11 | void Odmetry::pass_pulse(double PULSE1,double PULSE2,double PULSE3,double PULSE4){ |
kazumayamanaka | 2:e189d55ef292 | 12 | pulse_a[0] = PULSE1; |
kazumayamanaka | 2:e189d55ef292 | 13 | pulse_a[1] = PULSE2; |
kazumayamanaka | 2:e189d55ef292 | 14 | pulse_a[2] = PULSE3; |
kazumayamanaka | 2:e189d55ef292 | 15 | pulse_a[3] = PULSE4; |
kazumayamanaka | 2:e189d55ef292 | 16 | |
kazumayamanaka | 2:e189d55ef292 | 17 | if(PULSE1 == PULSE2 == PULSE3 == PULSE4){ |
kazumayamanaka | 2:e189d55ef292 | 18 | return; |
kazumayamanaka | 2:e189d55ef292 | 19 | } |
kazumayamanaka | 2:e189d55ef292 | 20 | else{ |
kazumayamanaka | 2:e189d55ef292 | 21 | CalcSpd(); |
kazumayamanaka | 2:e189d55ef292 | 22 | Angular(); |
kazumayamanaka | 2:e189d55ef292 | 23 | transform(); |
kazumayamanaka | 2:e189d55ef292 | 24 | CalcPosition(); |
kazumayamanaka | 2:e189d55ef292 | 25 | } |
kazumayamanaka | 2:e189d55ef292 | 26 | } |
kazumayamanaka | 2:e189d55ef292 | 27 | |
kazumayamanaka | 2:e189d55ef292 | 28 | void Odmetry::pass_angle(double THETA){ |
kazumayamanaka | 2:e189d55ef292 | 29 | Theta = THETA - PI / 4; |
kazumayamanaka | 2:e189d55ef292 | 30 | } |
kazumayamanaka | 2:e189d55ef292 | 31 | |
kazumayamanaka | 2:e189d55ef292 | 32 | |
kazumayamanaka | 2:e189d55ef292 | 33 | void Odmetry::CalcSpd(){ |
kazumayamanaka | 2:e189d55ef292 | 34 | for(int i = 0;i < 4;i ++){ |
kazumayamanaka | 2:e189d55ef292 | 35 | diff[i] = pulse_a[i] - pulse_b[i]; |
kazumayamanaka | 2:e189d55ef292 | 36 | spd[i] = Co * diff[i] / dt; |
kazumayamanaka | 2:e189d55ef292 | 37 | pulse_b[i] = pulse_a[i]; |
kazumayamanaka | 2:e189d55ef292 | 38 | } |
kazumayamanaka | 2:e189d55ef292 | 39 | } |
kazumayamanaka | 2:e189d55ef292 | 40 | |
kazumayamanaka | 2:e189d55ef292 | 41 | void Odmetry::Angular(){ |
kazumayamanaka | 2:e189d55ef292 | 42 | for(int i = 0;i < 4;i ++){ |
kazumayamanaka | 2:e189d55ef292 | 43 | Omega[i] = spd[i] / Radius; |
kazumayamanaka | 2:e189d55ef292 | 44 | } |
kazumayamanaka | 2:e189d55ef292 | 45 | } |
kazumayamanaka | 2:e189d55ef292 | 46 | |
kazumayamanaka | 2:e189d55ef292 | 47 | void Odmetry::transform(){ |
kazumayamanaka | 2:e189d55ef292 | 48 | Xpower = Wradius * (Omega[1] - Omega[3]); |
kazumayamanaka | 2:e189d55ef292 | 49 | Ypower = Wradius * (Omega[0] - Omega[2]); |
kazumayamanaka | 2:e189d55ef292 | 50 | } |
kazumayamanaka | 2:e189d55ef292 | 51 | |
kazumayamanaka | 2:e189d55ef292 | 52 | void Odmetry::CalcPosition(){ |
kazumayamanaka | 2:e189d55ef292 | 53 | Xspd = cos(Theta) * Xpower - sin(Theta) * Ypower; |
kazumayamanaka | 2:e189d55ef292 | 54 | Yspd = sin(Theta) * Xpower + cos(Theta) * Ypower; |
kazumayamanaka | 2:e189d55ef292 | 55 | |
kazumayamanaka | 2:e189d55ef292 | 56 | XPos += Xspd * dt; |
kazumayamanaka | 2:e189d55ef292 | 57 | YPos += Yspd * dt; |
kazumayamanaka | 2:e189d55ef292 | 58 | } |
kazumayamanaka | 2:e189d55ef292 | 59 | |
kazumayamanaka | 7:a0b9b6e71e41 | 60 | void Odmetry::print_Pos(){ printf("XPos:%lf YPos:%lf theta:%lf\n",XPos,YPos,Theta + PI / 2); } |
kazumayamanaka | 7:a0b9b6e71e41 | 61 | |
kazumayamanaka | 7:a0b9b6e71e41 | 62 | double Odmetry::obt_CurrentPosX(){ return XPos; } |
kazumayamanaka | 7:a0b9b6e71e41 | 63 | |
kazumayamanaka | 7:a0b9b6e71e41 | 64 | double Odmetry::obt_CurrentPosY(){ return YPos; } |
kazumayamanaka | 7:a0b9b6e71e41 | 65 | |
kazumayamanaka | 7:a0b9b6e71e41 | 66 | double Odmetry::obt_CurrentTheta(){ return Theta + PI / 2; } |