自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
Diff: AUTOmatics.cpp
- Revision:
- 8:f2407caf81ba
diff -r a0b9b6e71e41 -r f2407caf81ba AUTOmatics.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AUTOmatics.cpp Sat Mar 05 01:00:42 2022 +0000 @@ -0,0 +1,33 @@ +#include "AUTOmatics.hpp" + +void automatics::getTarget(double TARGETX,double TARGETY,double TARGETT){ + targetX = TARGETX, targetY = TARGETY, targetT = TARGETT; +} + +void automatics::getCurrent(double CURRENTX,double CURRENTY,double CURRENTT){ + currentX = CURRENTX, currentY = CURRENTY, currentT = CURRENTT; +} + +void automatics::Calc_Cp(){ + xCp = MAXSPEED / targetX, yCp = MAXSPEED / targetY, tCp = MAXSPEED / targetT; +} + +void automatics::Calc_diff(){ + diffX = targetX - currentX, diffY = targetY - currentY , diffT = targetT - currentT; +} + +void automatics::Calc_Pval(){ + xP = diffX * xCp, yP = diffY * yCp, tP = diffT * tCp; +} + +void automatics::Calc_PWM(){ + xPWM += xP, yPWM += yP, tPWM += tP; +} + +double automatics::get_xPWM(){ return xPWM; } + +double automatics::get_yPWM(){ return yPWM; } + +double automatics::get_tPWM(){ return tPWM; } + + \ No newline at end of file