足回り用プログラム(修正版)
Dependencies: SBDBT arrc_mbed BNO055
Revision 5:e189d55ef292, committed 2022-01-28
- Comitter:
- kazumayamanaka
- Date:
- Fri Jan 28 15:12:04 2022 +0000
- Parent:
- 1:ea880e226e5a
- Commit message:
- program of wheel;
Changed in this revision
diff -r ea880e226e5a -r e189d55ef292 AutoMatic.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AutoMatic.cpp Fri Jan 28 15:12:04 2022 +0000 @@ -0,0 +1,28 @@ +#include "AutoMatic.hpp" + +double AutoMatic::CalcDiffX(){ return TargetPosX - PosX; } + +void AutoMatic::pass_TargetPosX(double X){ + TargetPosX = X; +} + +void AutoMatic::pass_PosX(double X){ + PosX = X; +} + +void AutoMatic::CalcPowerX(){ + if(CalcDiffX()>0){ + Xpower = 100; + } + else{ + Xpower = 0; + } +} + +double AutoMatic::obt_Xpower(){ + CalcPowerX(); + return Xpower; +} + + +
diff -r ea880e226e5a -r e189d55ef292 AutoMatic.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AutoMatic.hpp Fri Jan 28 15:12:04 2022 +0000 @@ -0,0 +1,23 @@ +#ifndef AUTO_MATIC_H +#define AUTO_MATIC_H + +#include "mbed.h" + +class AutoMatic{ + public: + void pass_TargetPosX(double X); + void pass_PosX(double X); + double obt_Xpower(); + + private: + double TargetPosX,TargetPosY; + double PosX,PosY; + double Xpower,Ypower; + double diffX,diffY; + + double CalcDiffX(); + void CalcPowerX(); + +}; + +#endif \ No newline at end of file
diff -r ea880e226e5a -r e189d55ef292 Odmetry.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Odmetry.cpp Fri Jan 28 15:12:04 2022 +0000 @@ -0,0 +1,60 @@ +#include "Odmetry.hpp" + +Odmetry::Odmetry(){ + dt = 0.05; + Radius = 395; + Wradius = 25; + XPos = 0; + YPos = 0; +} + +void Odmetry::pass_pulse(double PULSE1,double PULSE2,double PULSE3,double PULSE4){ + pulse_a[0] = PULSE1; + pulse_a[1] = PULSE2; + pulse_a[2] = PULSE3; + pulse_a[3] = PULSE4; + + if(PULSE1 == PULSE2 == PULSE3 == PULSE4){ + return; + } + else{ + CalcSpd(); + Angular(); + transform(); + CalcPosition(); + } +} + +void Odmetry::pass_angle(double THETA){ + Theta = THETA - PI / 4; +} + + +void Odmetry::CalcSpd(){ + for(int i = 0;i < 4;i ++){ + diff[i] = pulse_a[i] - pulse_b[i]; + spd[i] = Co * diff[i] / dt; + pulse_b[i] = pulse_a[i]; + } +} + +void Odmetry::Angular(){ + for(int i = 0;i < 4;i ++){ + Omega[i] = spd[i] / Radius; + } +} + +void Odmetry::transform(){ + Xpower = Wradius * (Omega[1] - Omega[3]); + Ypower = Wradius * (Omega[0] - Omega[2]); +} + +void Odmetry::CalcPosition(){ + Xspd = cos(Theta) * Xpower - sin(Theta) * Ypower; + Yspd = sin(Theta) * Xpower + cos(Theta) * Ypower; + + XPos += Xspd * dt; + YPos += Yspd * dt; +} + +void Odmetry::print_Pos(){ printf("XPos:%lf YPos:%lf theta:%lf\n",XPos,YPos,Theta); }
diff -r ea880e226e5a -r e189d55ef292 Odmetry.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Odmetry.hpp Fri Jan 28 15:12:04 2022 +0000 @@ -0,0 +1,33 @@ +#ifndef ODMETRY_H +#define ODMETRY_H + +#include "mbed.h" +#define Co (108 * 3.1415926535897) / 512 +#define PI 3.1415926535897 + +class Odmetry{ + public: + Odmetry(); + void pass_pulse(double PULSE1,double PULSE2,double PULSE3,double PULSE4); + void pass_angle(double THETA); + void print_Pos(); + + private: + void Angular(); + void CalcPosition(); + void CalcSpd(); + void transform(); + + double Radius; + double Wradius; + double Omega[4]; + double spd[4]; + double pulse_a[4],pulse_b[4],diff[4]; + double Xspd,Yspd; + double Xpower,Ypower; + double XPos,YPos,Theta; + double dt; +}; + +#endif + \ No newline at end of file
diff -r ea880e226e5a -r e189d55ef292 PIDco.cpp --- a/PIDco.cpp Thu Jan 20 07:27:53 2022 +0000 +++ b/PIDco.cpp Fri Jan 28 15:12:04 2022 +0000 @@ -1,7 +1,7 @@ #include "PIDco.hpp" PIDco::PIDco(){ - spd = 0.0; + spd = 0.0; Pwm = 0.0; dt = 0.05; Integral = 0.0; @@ -31,8 +31,8 @@ PwmOut v1p(PIN_A); PwmOut v1m(PIN_B); - v1p.period_us(2048); - v1m.period_us(2048); + v1p.period_us(1024); + v1m.period_us(1024); cal_spd(); cal_Error();
diff -r ea880e226e5a -r e189d55ef292 main.cpp --- a/main.cpp Thu Jan 20 07:27:53 2022 +0000 +++ b/main.cpp Fri Jan 28 15:12:04 2022 +0000 @@ -3,6 +3,7 @@ #include "PIDco.hpp" #include "TARGETco.hpp" #include "DUALSHOCKco.hpp" +#include "Odmetry.hpp" #include "sbdbt.hpp" #include "BNO055.h" @@ -17,6 +18,11 @@ RotaryInc data_3(PC_0,PC_1,0); RotaryInc data_4(PC_2,PC_3,0); +RotaryInc MW_1(PA_13,PC_4,0); +RotaryInc MW_2(PC_10,PC_11,0); +RotaryInc MW_3(PA_7,PA_6,0); +RotaryInc MW_4(PA_8,PA_9,0); + PIDco pid_1; PIDco pid_2; PIDco pid_3; @@ -25,17 +31,17 @@ TARGETco TG; BNO055 bno(SDA,SCL); DUALSHOCKco DS; - +Odmetry odmetry; sbdbt sb(PA_0,PA_1); Timer Time; double timer; double theta; -double X,Y; int main(){ Time.start(); + while(1){ timer = Time.read_us(); bno.setmode(OPERATION_MODE_IMUPLUS); @@ -47,6 +53,8 @@ theta = -(2 * PI - theta); } + theta = theta - PI / 4; + DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An()); if(DS.cal_input() == true){ @@ -66,7 +74,10 @@ pid_3.wheel_ctl(PB_5,PB_4); pid_4.wheel_ctl(PB_7,PB_6); - pc.printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",TG.obt_target1(),TG.obt_target2(),TG.obt_target3(),TG.obt_target4(),pid_1.obt_spd(),pid_2.obt_spd(),pid_3.obt_spd(),pid_4.obt_spd()); + odmetry.pass_pulse(MW_1.get(),MW_2.get(),MW_3.get(),MW_4.get()); + odmetry.pass_angle(theta); + odmetry.print_Pos(); + //pc.printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",TG.obt_target1(),TG.obt_target2(),TG.obt_target3(),TG.obt_target4(),pid_1.obt_spd(),pid_2.obt_spd(),pid_3.obt_spd(),pid_4.obt_spd()); while(Time.read_us() - timer <= 50 * 1000); } }