2022_gorobo_Ateam / 2022_Ateam_MOTORprogram

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

Dependencies:   SBDBT arrc_mbed BNO055

main.cpp@4:920acf362fb6, 2022-01-21 (annotated)

Committer:
guesta
Date:
Fri Jan 21 09:15:33 2022 +0000
Revision:
4:920acf362fb6
Parent:
2:612efbe94f42 
a;

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 0:a1238c4cd105 6 #include "sbdbt.hpp"
kazumayamanaka 0:a1238c4cd105 7 #include "BNO055.h"
guesta 4:920acf362fb6 8
kazumayamanaka 0:a1238c4cd105 9 #define SDA D3
kazumayamanaka 0:a1238c4cd105 10 #define SCL D6
kazumayamanaka 0:a1238c4cd105 11 #define PI 3.1415926535897
guesta 4:920acf362fb6 12
guesta 4:920acf362fb6 13 Serial pc(USBTX,USBRX);
guesta 4:920acf362fb6 14
kazumayamanaka 0:a1238c4cd105 15 RotaryInc data_1(PA_14,PA_15,0);
kazumayamanaka 0:a1238c4cd105 16 RotaryInc data_2(PA_12,PC_5,0);
kazumayamanaka 0:a1238c4cd105 17 RotaryInc data_3(PC_0,PC_1,0);
kazumayamanaka 0:a1238c4cd105 18 RotaryInc data_4(PC_2,PC_3,0);
guesta 4:920acf362fb6 19
kazumayamanaka 0:a1238c4cd105 20 PIDco pid_1;
kazumayamanaka 0:a1238c4cd105 21 PIDco pid_2;
kazumayamanaka 0:a1238c4cd105 22 PIDco pid_3;
kazumayamanaka 0:a1238c4cd105 23 PIDco pid_4;
guesta 4:920acf362fb6 24
kazumayamanaka 0:a1238c4cd105 25 TARGETco TG;
kazumayamanaka 0:a1238c4cd105 26 BNO055 bno(SDA,SCL);
kazumayamanaka 0:a1238c4cd105 27 DUALSHOCKco DS;
guesta 4:920acf362fb6 28
guesta 4:920acf362fb6 29 sbdbt sb(PA_0,PA_1);
guesta 4:920acf362fb6 30
kazumayamanaka 0:a1238c4cd105 31 Timer Time;
kazumayamanaka 0:a1238c4cd105 32 double timer;
kazumayamanaka 0:a1238c4cd105 33 double theta;
kazumayamanaka 0:a1238c4cd105 34 double X,Y;
guesta 4:920acf362fb6 35
kazumayamanaka 0:a1238c4cd105 36 int main(){
kazumayamanaka 0:a1238c4cd105 37 Time.start();
kazumayamanaka 0:a1238c4cd105 38
kazumayamanaka 0:a1238c4cd105 39 while(1){
kazumayamanaka 0:a1238c4cd105 40 timer = Time.read_us();
kazumayamanaka 0:a1238c4cd105 41 bno.setmode(OPERATION_MODE_IMUPLUS);
kazumayamanaka 0:a1238c4cd105 42 bno.get_angles();
kazumayamanaka 1:ea880e226e5a 43
kazumayamanaka 1:ea880e226e5a 44 theta = bno.euler.yaw * (PI / 180);
kazumayamanaka 1:ea880e226e5a 45
kazumayamanaka 0:a1238c4cd105 46 if(theta > PI){
kazumayamanaka 1:ea880e226e5a 47 theta = -(2 * PI - theta);
kazumayamanaka 0:a1238c4cd105 48 }
kazumayamanaka 0:a1238c4cd105 49
guesta 4:920acf362fb6 50 DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An());
kazumayamanaka 0:a1238c4cd105 51
kazumayamanaka 0:a1238c4cd105 52 if(DS.cal_input() == true){
kazumayamanaka 0:a1238c4cd105 53 TG.pass_val(DS.obt_X(),DS.obt_Y(),theta);
kazumayamanaka 0:a1238c4cd105 54 }
kazumayamanaka 0:a1238c4cd105 55 else{
kazumayamanaka 0:a1238c4cd105 56 TG.pass_target(DS.obt_X());
kazumayamanaka 0:a1238c4cd105 57 }
kazumayamanaka 0:a1238c4cd105 58
kazumayamanaka 0:a1238c4cd105 59 pid_1.pass_val(data_1.get(),TG.obt_target1());
kazumayamanaka 0:a1238c4cd105 60 pid_2.pass_val(data_2.get(),TG.obt_target2());
kazumayamanaka 0:a1238c4cd105 61 pid_3.pass_val(data_3.get(),TG.obt_target3());
kazumayamanaka 0:a1238c4cd105 62 pid_4.pass_val(data_4.get(),TG.obt_target4());
kazumayamanaka 0:a1238c4cd105 63
kazumayamanaka 1:ea880e226e5a 64 pid_1.wheel_ctl(PC_9,PC_8);
kazumayamanaka 0:a1238c4cd105 65 pid_2.wheel_ctl(PB_14,PB_13);
kazumayamanaka 0:a1238c4cd105 66 pid_3.wheel_ctl(PB_5,PB_4);
kazumayamanaka 0:a1238c4cd105 67 pid_4.wheel_ctl(PB_7,PB_6);
kazumayamanaka 0:a1238c4cd105 68
guesta 4:920acf362fb6 69 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 70 while(Time.read_us() - timer <= 50 * 1000);
kazumayamanaka 0:a1238c4cd105 71 }
kazumayamanaka 0:a1238c4cd105 72 }
guesta 4:920acf362fb6 73
guesta 4:920acf362fb6 74 //BNOのピン(PB_3,PB_10)