自己位置推定機能を追加

Dependencies:   SBDBT arrc_mbed BNO055

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers AUTOmatics.cpp Source File

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