自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
AUTOmatics.cpp@8:f2407caf81ba, 2022-03-05 (annotated)
- Committer:
- kazumayamanaka
- Date:
- Sat Mar 05 01:00:42 2022 +0000
- Revision:
- 8:f2407caf81ba
program of motor
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kazumayamanaka | 8:f2407caf81ba | 1 | #include "AUTOmatics.hpp" |
kazumayamanaka | 8:f2407caf81ba | 2 | |
kazumayamanaka | 8:f2407caf81ba | 3 | void automatics::getTarget(double TARGETX,double TARGETY,double TARGETT){ |
kazumayamanaka | 8:f2407caf81ba | 4 | targetX = TARGETX, targetY = TARGETY, targetT = TARGETT; |
kazumayamanaka | 8:f2407caf81ba | 5 | } |
kazumayamanaka | 8:f2407caf81ba | 6 | |
kazumayamanaka | 8:f2407caf81ba | 7 | void automatics::getCurrent(double CURRENTX,double CURRENTY,double CURRENTT){ |
kazumayamanaka | 8:f2407caf81ba | 8 | currentX = CURRENTX, currentY = CURRENTY, currentT = CURRENTT; |
kazumayamanaka | 8:f2407caf81ba | 9 | } |
kazumayamanaka | 8:f2407caf81ba | 10 | |
kazumayamanaka | 8:f2407caf81ba | 11 | void automatics::Calc_Cp(){ |
kazumayamanaka | 8:f2407caf81ba | 12 | xCp = MAXSPEED / targetX, yCp = MAXSPEED / targetY, tCp = MAXSPEED / targetT; |
kazumayamanaka | 8:f2407caf81ba | 13 | } |
kazumayamanaka | 8:f2407caf81ba | 14 | |
kazumayamanaka | 8:f2407caf81ba | 15 | void automatics::Calc_diff(){ |
kazumayamanaka | 8:f2407caf81ba | 16 | diffX = targetX - currentX, diffY = targetY - currentY , diffT = targetT - currentT; |
kazumayamanaka | 8:f2407caf81ba | 17 | } |
kazumayamanaka | 8:f2407caf81ba | 18 | |
kazumayamanaka | 8:f2407caf81ba | 19 | void automatics::Calc_Pval(){ |
kazumayamanaka | 8:f2407caf81ba | 20 | xP = diffX * xCp, yP = diffY * yCp, tP = diffT * tCp; |
kazumayamanaka | 8:f2407caf81ba | 21 | } |
kazumayamanaka | 8:f2407caf81ba | 22 | |
kazumayamanaka | 8:f2407caf81ba | 23 | void automatics::Calc_PWM(){ |
kazumayamanaka | 8:f2407caf81ba | 24 | xPWM += xP, yPWM += yP, tPWM += tP; |
kazumayamanaka | 8:f2407caf81ba | 25 | } |
kazumayamanaka | 8:f2407caf81ba | 26 | |
kazumayamanaka | 8:f2407caf81ba | 27 | double automatics::get_xPWM(){ return xPWM; } |
kazumayamanaka | 8:f2407caf81ba | 28 | |
kazumayamanaka | 8:f2407caf81ba | 29 | double automatics::get_yPWM(){ return yPWM; } |
kazumayamanaka | 8:f2407caf81ba | 30 | |
kazumayamanaka | 8:f2407caf81ba | 31 | double automatics::get_tPWM(){ return tPWM; } |
kazumayamanaka | 8:f2407caf81ba | 32 | |
kazumayamanaka | 8:f2407caf81ba | 33 |