足回り用プログラム(修正版)

Dependencies:   SBDBT arrc_mbed BNO055

Committer:
kazumayamanaka
Date:
Fri Jan 28 15:12:04 2022 +0000
Revision:
5:e189d55ef292
Parent:
1:ea880e226e5a
program  of wheel;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kazumayamanaka 0:a1238c4cd105 1 #include "TARGETco.hpp"
kazumayamanaka 0:a1238c4cd105 2
kazumayamanaka 0:a1238c4cd105 3 void TARGETco::pass_val(double XPOWER,double YPOWER,double THETA){
kazumayamanaka 0:a1238c4cd105 4 Xpower = XPOWER;
kazumayamanaka 0:a1238c4cd105 5 Ypower = YPOWER;
kazumayamanaka 0:a1238c4cd105 6 theta = THETA;
kazumayamanaka 0:a1238c4cd105 7
kazumayamanaka 0:a1238c4cd105 8 cal_angular();
kazumayamanaka 0:a1238c4cd105 9 }
kazumayamanaka 0:a1238c4cd105 10
kazumayamanaka 0:a1238c4cd105 11 void TARGETco::pass_target(double TARGET){
kazumayamanaka 0:a1238c4cd105 12 for(int i = 0;i < 4;i ++){
kazumayamanaka 0:a1238c4cd105 13 target[i] = TARGET;
kazumayamanaka 0:a1238c4cd105 14 }
kazumayamanaka 0:a1238c4cd105 15 }
kazumayamanaka 0:a1238c4cd105 16
kazumayamanaka 0:a1238c4cd105 17 void TARGETco::cal_angular(){
kazumayamanaka 0:a1238c4cd105 18 Xvelocity = Xpower * cos(theta) - Ypower * sin(theta);
kazumayamanaka 1:ea880e226e5a 19 Yvelocity = -Xpower * sin(theta) - Ypower * cos(theta);
kazumayamanaka 0:a1238c4cd105 20 target[0] = -Xvelocity;
kazumayamanaka 0:a1238c4cd105 21 target[1] = -Yvelocity;
kazumayamanaka 0:a1238c4cd105 22 target[2] = Xvelocity;
kazumayamanaka 0:a1238c4cd105 23 target[3] = Yvelocity;
kazumayamanaka 0:a1238c4cd105 24 }
kazumayamanaka 0:a1238c4cd105 25
kazumayamanaka 0:a1238c4cd105 26 double TARGETco::obt_target1(){ return target[0]; }
kazumayamanaka 0:a1238c4cd105 27
kazumayamanaka 0:a1238c4cd105 28 double TARGETco::obt_target2(){ return target[1]; }
kazumayamanaka 0:a1238c4cd105 29
kazumayamanaka 0:a1238c4cd105 30 double TARGETco::obt_target3(){ return target[2]; }
kazumayamanaka 0:a1238c4cd105 31
kazumayamanaka 0:a1238c4cd105 32 double TARGETco::obt_target4(){ return target[3]; }