kazuma yamanaka / 2022_Ateam_MOTORprogram

自己位置推定機能を追加

Dependencies:   SBDBT arrc_mbed BNO055

main.cpp@3:612efbe94f42, 2022-01-21 (annotated)

Committer:
guesta
Date:
Fri Jan 21 08:57:07 2022 +0000
Revision:
3:612efbe94f42
Parent:
1:ea880e226e5a
Child:
5:920acf362fb6
first_commit;

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 3:612efbe94f42 8 #include "scrp_slave.hpp"
guesta 3:612efbe94f42 9 #include "function.hpp"
kazumayamanaka 0:a1238c4cd105 10
kazumayamanaka 0:a1238c4cd105 11 #define SDA D3
kazumayamanaka 0:a1238c4cd105 12 #define SCL D6
kazumayamanaka 0:a1238c4cd105 13 #define PI 3.1415926535897
kazumayamanaka 0:a1238c4cd105 14
guesta 3:612efbe94f42 15 ScrpSlave scrp(PC_12,PD_2,PH_1,0x0807f800);
kazumayamanaka 0:a1238c4cd105 16
kazumayamanaka 0:a1238c4cd105 17 RotaryInc data_1(PA_14,PA_15,0);
kazumayamanaka 0:a1238c4cd105 18 RotaryInc data_2(PA_12,PC_5,0);
kazumayamanaka 0:a1238c4cd105 19 RotaryInc data_3(PC_0,PC_1,0);
kazumayamanaka 0:a1238c4cd105 20 RotaryInc data_4(PC_2,PC_3,0);
kazumayamanaka 0:a1238c4cd105 21
kazumayamanaka 0:a1238c4cd105 22 PIDco pid_1;
kazumayamanaka 0:a1238c4cd105 23 PIDco pid_2;
kazumayamanaka 0:a1238c4cd105 24 PIDco pid_3;
kazumayamanaka 0:a1238c4cd105 25 PIDco pid_4;
kazumayamanaka 0:a1238c4cd105 26
kazumayamanaka 0:a1238c4cd105 27 TARGETco TG;
kazumayamanaka 0:a1238c4cd105 28 BNO055 bno(SDA,SCL);
kazumayamanaka 0:a1238c4cd105 29 DUALSHOCKco DS;
kazumayamanaka 0:a1238c4cd105 30
guesta 3:612efbe94f42 31 //sbdbt sb(PA_0,PA_1);
guesta 3:612efbe94f42 32
guesta 3:612efbe94f42 33 DigitalOut led1(PA_10);
guesta 3:612efbe94f42 34 DigitalOut led2(PB_15);
guesta 3:612efbe94f42 35 DigitalOut led3(PB_2);
guesta 3:612efbe94f42 36 DigitalOut led4(PC_6);
kazumayamanaka 0:a1238c4cd105 37
kazumayamanaka 0:a1238c4cd105 38 Timer Time;
kazumayamanaka 0:a1238c4cd105 39 double timer;
kazumayamanaka 0:a1238c4cd105 40 double theta;
kazumayamanaka 0:a1238c4cd105 41 double X,Y;
kazumayamanaka 0:a1238c4cd105 42
kazumayamanaka 0:a1238c4cd105 43 int main(){
kazumayamanaka 0:a1238c4cd105 44 Time.start();
guesta 3:612efbe94f42 45 TG.pass_val(0,0,0);
guesta 3:612efbe94f42 46 scrp.addCMD(1,getLstick_x);
guesta 3:612efbe94f42 47 scrp.addCMD(2,getLstick_y);
guesta 3:612efbe94f42 48 scrp.addCMD(3,getL2);
guesta 3:612efbe94f42 49 scrp.addCMD(4,getR2);
guesta 3:612efbe94f42 50 scrp.addCMD(5,change_mode);
guesta 3:612efbe94f42 51
kazumayamanaka 0:a1238c4cd105 52
kazumayamanaka 0:a1238c4cd105 53 while(1){
kazumayamanaka 0:a1238c4cd105 54 timer = Time.read_us();
kazumayamanaka 0:a1238c4cd105 55 bno.setmode(OPERATION_MODE_IMUPLUS);
kazumayamanaka 0:a1238c4cd105 56 bno.get_angles();
kazumayamanaka 1:ea880e226e5a 57
kazumayamanaka 1:ea880e226e5a 58 theta = bno.euler.yaw * (PI / 180);
kazumayamanaka 1:ea880e226e5a 59
kazumayamanaka 0:a1238c4cd105 60 if(theta > PI){
kazumayamanaka 1:ea880e226e5a 61 theta = -(2 * PI - theta);
kazumayamanaka 0:a1238c4cd105 62 }
kazumayamanaka 0:a1238c4cd105 63
guesta 3:612efbe94f42 64 DS.pass_val(x_component,y_component,r2_num,l2_num);
kazumayamanaka 0:a1238c4cd105 65
kazumayamanaka 0:a1238c4cd105 66 if(DS.cal_input() == true){
kazumayamanaka 0:a1238c4cd105 67 TG.pass_val(DS.obt_X(),DS.obt_Y(),theta);
kazumayamanaka 0:a1238c4cd105 68 }
kazumayamanaka 0:a1238c4cd105 69 else{
kazumayamanaka 0:a1238c4cd105 70 TG.pass_target(DS.obt_X());
kazumayamanaka 0:a1238c4cd105 71 }
kazumayamanaka 0:a1238c4cd105 72
kazumayamanaka 0:a1238c4cd105 73 pid_1.pass_val(data_1.get(),TG.obt_target1());
kazumayamanaka 0:a1238c4cd105 74 pid_2.pass_val(data_2.get(),TG.obt_target2());
kazumayamanaka 0:a1238c4cd105 75 pid_3.pass_val(data_3.get(),TG.obt_target3());
kazumayamanaka 0:a1238c4cd105 76 pid_4.pass_val(data_4.get(),TG.obt_target4());
kazumayamanaka 0:a1238c4cd105 77
kazumayamanaka 1:ea880e226e5a 78 pid_1.wheel_ctl(PC_9,PC_8);
kazumayamanaka 0:a1238c4cd105 79 pid_2.wheel_ctl(PB_14,PB_13);
kazumayamanaka 0:a1238c4cd105 80 pid_3.wheel_ctl(PB_5,PB_4);
kazumayamanaka 0:a1238c4cd105 81 pid_4.wheel_ctl(PB_7,PB_6);
kazumayamanaka 0:a1238c4cd105 82
guesta 3:612efbe94f42 83 if(TG.obt_target1() != 0){
guesta 3:612efbe94f42 84 led1 = 1;
guesta 3:612efbe94f42 85 }else{
guesta 3:612efbe94f42 86 led1 = 0;
guesta 3:612efbe94f42 87 }
guesta 3:612efbe94f42 88 if(TG.obt_target2() != 0){
guesta 3:612efbe94f42 89 led2 = 1;
guesta 3:612efbe94f42 90 }else{
guesta 3:612efbe94f42 91 led2 = 0;
guesta 3:612efbe94f42 92 }
guesta 3:612efbe94f42 93 if(TG.obt_target3() != 0){
guesta 3:612efbe94f42 94 led3 = 1;
guesta 3:612efbe94f42 95 }else{
guesta 3:612efbe94f42 96 led3 = 0;
guesta 3:612efbe94f42 97 }
guesta 3:612efbe94f42 98 if(TG.obt_target4() != 0){
guesta 3:612efbe94f42 99 led4 = 1;
guesta 3:612efbe94f42 100 }else{
guesta 3:612efbe94f42 101 led4 = 0;
guesta 3:612efbe94f42 102 }
guesta 3:612efbe94f42 103
guesta 3:612efbe94f42 104 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 105 while(Time.read_us() - timer <= 50 * 1000);
kazumayamanaka 0:a1238c4cd105 106 }
kazumayamanaka 0:a1238c4cd105 107 }
kazumayamanaka 0:a1238c4cd105 108
kazumayamanaka 0:a1238c4cd105 109 //BNOのピン(PB_3,PB_10)
kazumayamanaka 0:a1238c4cd105 110
kazumayamanaka 0:a1238c4cd105 111
kazumayamanaka 0:a1238c4cd105 112