2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。
Dependencies: SBDBT arrc_mbed BNO055
main.cpp
- Committer:
- guesta
- Date:
- 2022-01-21
- Revision:
- 2:612efbe94f42
- Parent:
- 1:ea880e226e5a
- Child:
- 4:867c2b5cf81a
File content as of revision 2:612efbe94f42:
#include "mbed.h" #include "rotary_inc.hpp" #include "PIDco.hpp" #include "TARGETco.hpp" #include "DUALSHOCKco.hpp" #include "sbdbt.hpp" #include "BNO055.h" #include "scrp_slave.hpp" #include "function.hpp" #define SDA D3 #define SCL D6 #define PI 3.1415926535897 ScrpSlave scrp(PC_12,PD_2,PH_1,0x0807f800); RotaryInc data_1(PA_14,PA_15,0); RotaryInc data_2(PA_12,PC_5,0); RotaryInc data_3(PC_0,PC_1,0); RotaryInc data_4(PC_2,PC_3,0); PIDco pid_1; PIDco pid_2; PIDco pid_3; PIDco pid_4; TARGETco TG; BNO055 bno(SDA,SCL); DUALSHOCKco DS; //sbdbt sb(PA_0,PA_1); DigitalOut led1(PA_10); DigitalOut led2(PB_15); DigitalOut led3(PB_2); DigitalOut led4(PC_6); Timer Time; double timer; double theta; double X,Y; int main(){ Time.start(); TG.pass_val(0,0,0); scrp.addCMD(1,getLstick_x); scrp.addCMD(2,getLstick_y); scrp.addCMD(3,getL2); scrp.addCMD(4,getR2); scrp.addCMD(5,change_mode); while(1){ timer = Time.read_us(); bno.setmode(OPERATION_MODE_IMUPLUS); bno.get_angles(); theta = bno.euler.yaw * (PI / 180); if(theta > PI){ theta = -(2 * PI - theta); } DS.pass_val(x_component,y_component,r2_num,l2_num); if(DS.cal_input() == true){ TG.pass_val(DS.obt_X(),DS.obt_Y(),theta); } else{ TG.pass_target(DS.obt_X()); } pid_1.pass_val(data_1.get(),TG.obt_target1()); pid_2.pass_val(data_2.get(),TG.obt_target2()); pid_3.pass_val(data_3.get(),TG.obt_target3()); pid_4.pass_val(data_4.get(),TG.obt_target4()); pid_1.wheel_ctl(PC_9,PC_8); pid_2.wheel_ctl(PB_14,PB_13); pid_3.wheel_ctl(PB_5,PB_4); pid_4.wheel_ctl(PB_7,PB_6); if(TG.obt_target1() != 0){ led1 = 1; }else{ led1 = 0; } if(TG.obt_target2() != 0){ led2 = 1; }else{ led2 = 0; } if(TG.obt_target3() != 0){ led3 = 1; }else{ led3 = 0; } if(TG.obt_target4() != 0){ led4 = 1; }else{ led4 = 0; } 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()); while(Time.read_us() - timer <= 50 * 1000); } } //BNOのピン(PB_3,PB_10)