2022_gorobo_Ateam / gourobo2022_undercarriage_new

2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。

Dependencies:   SBDBT arrc_mbed BNO055

main.cpp@0:a1238c4cd105, 2022-01-19 (annotated)

Committer:
kazumayamanaka
Date:
Wed Jan 19 12:43:26 2022 +0000
Revision:
0:a1238c4cd105
Child:
1:ea880e226e5a
2022robotcontest

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"
kazumayamanaka 0:a1238c4cd105 8
kazumayamanaka 0:a1238c4cd105 9 #define SDA D3
kazumayamanaka 0:a1238c4cd105 10 #define SCL D6
kazumayamanaka 0:a1238c4cd105 11 #define PI 3.1415926535897
kazumayamanaka 0:a1238c4cd105 12
kazumayamanaka 0:a1238c4cd105 13 Serial pc(USBTX,USBRX);
kazumayamanaka 0:a1238c4cd105 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);
kazumayamanaka 0:a1238c4cd105 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;
kazumayamanaka 0:a1238c4cd105 24
kazumayamanaka 0:a1238c4cd105 25 TARGETco TG;
kazumayamanaka 0:a1238c4cd105 26 BNO055 bno(SDA,SCL);
kazumayamanaka 0:a1238c4cd105 27 DUALSHOCKco DS;
kazumayamanaka 0:a1238c4cd105 28
kazumayamanaka 0:a1238c4cd105 29 sbdbt sb(PA_0,PA_1);
kazumayamanaka 0:a1238c4cd105 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;
kazumayamanaka 0:a1238c4cd105 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 0:a1238c4cd105 43 theta = (360 - (double)bno.euler.yaw) / 180 * PI;
kazumayamanaka 0:a1238c4cd105 44
kazumayamanaka 0:a1238c4cd105 45 if(theta > PI){
kazumayamanaka 0:a1238c4cd105 46 theta = theta - 2 * PI;
kazumayamanaka 0:a1238c4cd105 47 }
kazumayamanaka 0:a1238c4cd105 48
kazumayamanaka 0:a1238c4cd105 49 DS.pass_val(sb.rsx(),sb.rsy(),sb.r2An(),sb.l2An());
kazumayamanaka 0:a1238c4cd105 50
kazumayamanaka 0:a1238c4cd105 51 if(DS.cal_input() == true){
kazumayamanaka 0:a1238c4cd105 52 TG.pass_val(DS.obt_X(),DS.obt_Y(),theta);
kazumayamanaka 0:a1238c4cd105 53 }
kazumayamanaka 0:a1238c4cd105 54 else{
kazumayamanaka 0:a1238c4cd105 55 TG.pass_target(DS.obt_X());
kazumayamanaka 0:a1238c4cd105 56 }
kazumayamanaka 0:a1238c4cd105 57
kazumayamanaka 0:a1238c4cd105 58 pid_1.pass_val(data_1.get(),TG.obt_target1());
kazumayamanaka 0:a1238c4cd105 59 pid_2.pass_val(data_2.get(),TG.obt_target2());
kazumayamanaka 0:a1238c4cd105 60 pid_3.pass_val(data_3.get(),TG.obt_target3());
kazumayamanaka 0:a1238c4cd105 61 pid_4.pass_val(data_4.get(),TG.obt_target4());
kazumayamanaka 0:a1238c4cd105 62
kazumayamanaka 0:a1238c4cd105 63 pid_1.wheel_ctl(PB_9,PB_8);
kazumayamanaka 0:a1238c4cd105 64 pid_2.wheel_ctl(PB_14,PB_13);
kazumayamanaka 0:a1238c4cd105 65 pid_3.wheel_ctl(PB_5,PB_4);
kazumayamanaka 0:a1238c4cd105 66 pid_4.wheel_ctl(PB_7,PB_6);
kazumayamanaka 0:a1238c4cd105 67
kazumayamanaka 0:a1238c4cd105 68 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 69 while(Time.read_us() - timer <= 50 * 1000);
kazumayamanaka 0:a1238c4cd105 70 }
kazumayamanaka 0:a1238c4cd105 71 }
kazumayamanaka 0:a1238c4cd105 72
kazumayamanaka 0:a1238c4cd105 73 //BNOのピン(PB_3,PB_10)
kazumayamanaka 0:a1238c4cd105 74
kazumayamanaka 0:a1238c4cd105 75
kazumayamanaka 0:a1238c4cd105 76