2022_gorobo_Ateam / gourobo2022_undercarriage_new

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

Dependencies:   SBDBT arrc_mbed BNO055

main.cpp@4:867c2b5cf81a, 2022-01-22 (annotated)

Committer:
guesta
Date:
Sat Jan 22 16:00:04 2022 +0000
Revision:
4:867c2b5cf81a
Parent:
2:612efbe94f42
Child:
5:1a850f68a06c
add odometry_program;

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