2022_gorobo_Ateam / 2022_Ateam_MOTORprogram

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

Dependencies:   SBDBT arrc_mbed BNO055

main.cpp@5:e189d55ef292, 2022-01-28 (annotated)

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 "mbed.h"
kazumayamanaka 0:a1238c4cd105 2 #include "rotary_inc.hpp"
kazumayamanaka 0:a1238c4cd105 3 #include "PIDco.hpp"
kazumayamanaka 0:a1238c4cd105 4 #include "TARGETco.hpp"
kazumayamanaka 0:a1238c4cd105 5 #include "DUALSHOCKco.hpp"
kazumayamanaka 5:e189d55ef292 6 #include "Odmetry.hpp"
kazumayamanaka 0:a1238c4cd105 7 #include "sbdbt.hpp"
kazumayamanaka 0:a1238c4cd105 8 #include "BNO055.h"
kazumayamanaka 0:a1238c4cd105 9
kazumayamanaka 0:a1238c4cd105 10 #define SDA D3
kazumayamanaka 0:a1238c4cd105 11 #define SCL D6
kazumayamanaka 0:a1238c4cd105 12 #define PI 3.1415926535897
kazumayamanaka 0:a1238c4cd105 13
kazumayamanaka 0:a1238c4cd105 14 Serial pc(USBTX,USBRX);
kazumayamanaka 0:a1238c4cd105 15
kazumayamanaka 0:a1238c4cd105 16 RotaryInc data_1(PA_14,PA_15,0);
kazumayamanaka 0:a1238c4cd105 17 RotaryInc data_2(PA_12,PC_5,0);
kazumayamanaka 0:a1238c4cd105 18 RotaryInc data_3(PC_0,PC_1,0);
kazumayamanaka 0:a1238c4cd105 19 RotaryInc data_4(PC_2,PC_3,0);
kazumayamanaka 0:a1238c4cd105 20
kazumayamanaka 5:e189d55ef292 21 RotaryInc MW_1(PA_13,PC_4,0);
kazumayamanaka 5:e189d55ef292 22 RotaryInc MW_2(PC_10,PC_11,0);
kazumayamanaka 5:e189d55ef292 23 RotaryInc MW_3(PA_7,PA_6,0);
kazumayamanaka 5:e189d55ef292 24 RotaryInc MW_4(PA_8,PA_9,0);
kazumayamanaka 5:e189d55ef292 25
kazumayamanaka 0:a1238c4cd105 26 PIDco pid_1;
kazumayamanaka 0:a1238c4cd105 27 PIDco pid_2;
kazumayamanaka 0:a1238c4cd105 28 PIDco pid_3;
kazumayamanaka 0:a1238c4cd105 29 PIDco pid_4;
kazumayamanaka 0:a1238c4cd105 30
kazumayamanaka 0:a1238c4cd105 31 TARGETco TG;
kazumayamanaka 0:a1238c4cd105 32 BNO055 bno(SDA,SCL);
kazumayamanaka 0:a1238c4cd105 33 DUALSHOCKco DS;
kazumayamanaka 5:e189d55ef292 34 Odmetry odmetry;
kazumayamanaka 0:a1238c4cd105 35 sbdbt sb(PA_0,PA_1);
kazumayamanaka 0:a1238c4cd105 36
kazumayamanaka 0:a1238c4cd105 37 Timer Time;
kazumayamanaka 0:a1238c4cd105 38 double timer;
kazumayamanaka 0:a1238c4cd105 39 double theta;
kazumayamanaka 0:a1238c4cd105 40
kazumayamanaka 0:a1238c4cd105 41 int main(){
kazumayamanaka 0:a1238c4cd105 42 Time.start();
kazumayamanaka 0:a1238c4cd105 43
kazumayamanaka 5:e189d55ef292 44
kazumayamanaka 0:a1238c4cd105 45 while(1){
kazumayamanaka 0:a1238c4cd105 46 timer = Time.read_us();
kazumayamanaka 0:a1238c4cd105 47 bno.setmode(OPERATION_MODE_IMUPLUS);
kazumayamanaka 0:a1238c4cd105 48 bno.get_angles();
kazumayamanaka 1:ea880e226e5a 49
kazumayamanaka 1:ea880e226e5a 50 theta = bno.euler.yaw * (PI / 180);
kazumayamanaka 1:ea880e226e5a 51
kazumayamanaka 0:a1238c4cd105 52 if(theta > PI){
kazumayamanaka 1:ea880e226e5a 53 theta = -(2 * PI - theta);
kazumayamanaka 0:a1238c4cd105 54 }
kazumayamanaka 0:a1238c4cd105 55
kazumayamanaka 5:e189d55ef292 56 theta = theta - PI / 4;
kazumayamanaka 5:e189d55ef292 57
kazumayamanaka 0:a1238c4cd105 58 DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An());
kazumayamanaka 0:a1238c4cd105 59
kazumayamanaka 0:a1238c4cd105 60 if(DS.cal_input() == true){
kazumayamanaka 0:a1238c4cd105 61 TG.pass_val(DS.obt_X(),DS.obt_Y(),theta);
kazumayamanaka 0:a1238c4cd105 62 }
kazumayamanaka 0:a1238c4cd105 63 else{
kazumayamanaka 0:a1238c4cd105 64 TG.pass_target(DS.obt_X());
kazumayamanaka 0:a1238c4cd105 65 }
kazumayamanaka 0:a1238c4cd105 66
kazumayamanaka 0:a1238c4cd105 67 pid_1.pass_val(data_1.get(),TG.obt_target1());
kazumayamanaka 0:a1238c4cd105 68 pid_2.pass_val(data_2.get(),TG.obt_target2());
kazumayamanaka 0:a1238c4cd105 69 pid_3.pass_val(data_3.get(),TG.obt_target3());
kazumayamanaka 0:a1238c4cd105 70 pid_4.pass_val(data_4.get(),TG.obt_target4());
kazumayamanaka 0:a1238c4cd105 71
kazumayamanaka 1:ea880e226e5a 72 pid_1.wheel_ctl(PC_9,PC_8);
kazumayamanaka 0:a1238c4cd105 73 pid_2.wheel_ctl(PB_14,PB_13);
kazumayamanaka 0:a1238c4cd105 74 pid_3.wheel_ctl(PB_5,PB_4);
kazumayamanaka 0:a1238c4cd105 75 pid_4.wheel_ctl(PB_7,PB_6);
kazumayamanaka 0:a1238c4cd105 76
kazumayamanaka 5:e189d55ef292 77 odmetry.pass_pulse(MW_1.get(),MW_2.get(),MW_3.get(),MW_4.get());
kazumayamanaka 5:e189d55ef292 78 odmetry.pass_angle(theta);
kazumayamanaka 5:e189d55ef292 79 odmetry.print_Pos();
kazumayamanaka 5:e189d55ef292 80 //pc.printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",TG.obt_target1(),TG.obt_target2(),TG.obt_target3(),TG.obt_target4(),pid_1.obt_spd(),pid_2.obt_spd(),pid_3.obt_spd(),pid_4.obt_spd());
kazumayamanaka 0:a1238c4cd105 81 while(Time.read_us() - timer <= 50 * 1000);
kazumayamanaka 0:a1238c4cd105 82 }
kazumayamanaka 0:a1238c4cd105 83 }
kazumayamanaka 0:a1238c4cd105 84
kazumayamanaka 0:a1238c4cd105 85 //BNOのピン(PB_3,PB_10)
kazumayamanaka 0:a1238c4cd105 86
kazumayamanaka 0:a1238c4cd105 87
kazumayamanaka 0:a1238c4cd105 88