自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
AUTOmatics.cpp
00001 #include "AUTOmatics.hpp" 00002 00003 void automatics::getTarget(double TARGETX,double TARGETY,double TARGETT){ 00004 targetX = TARGETX, targetY = TARGETY, targetT = TARGETT; 00005 } 00006 00007 void automatics::getCurrent(double CURRENTX,double CURRENTY,double CURRENTT){ 00008 currentX = CURRENTX, currentY = CURRENTY, currentT = CURRENTT; 00009 } 00010 00011 void automatics::Calc_Cp(){ 00012 xCp = MAXSPEED / targetX, yCp = MAXSPEED / targetY, tCp = MAXSPEED / targetT; 00013 } 00014 00015 void automatics::Calc_diff(){ 00016 diffX = targetX - currentX, diffY = targetY - currentY , diffT = targetT - currentT; 00017 } 00018 00019 void automatics::Calc_Pval(){ 00020 xP = diffX * xCp, yP = diffY * yCp, tP = diffT * tCp; 00021 } 00022 00023 void automatics::Calc_PWM(){ 00024 xPWM += xP, yPWM += yP, tPWM += tP; 00025 } 00026 00027 double automatics::get_xPWM(){ return xPWM; } 00028 00029 double automatics::get_yPWM(){ return yPWM; } 00030 00031 double automatics::get_tPWM(){ return tPWM; } 00032 00033
Generated on Thu Jul 14 2022 19:42:28 by 1.7.2