kazuma yamanaka / 2022_Ateam_MOTORprogram

自己位置推定機能を追加

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?

UserRevisionLine numberNew 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