自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
AUTOmatics.cpp
- Committer:
- kazumayamanaka
- Date:
- 2022-03-05
- Revision:
- 8:f2407caf81ba
File content as of revision 8:f2407caf81ba:
#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; }