2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。
Dependencies: SBDBT arrc_mbed BNO055
main.cpp
- Committer:
- guesta
- Date:
- 2022-04-08
- Revision:
- 13:369f4abc1f36
- Parent:
- 12:894e5ac49810
File content as of revision 13:369f4abc1f36:
#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" #include "odometry.hpp" #define SDA D3 #define SCL D6 #define PI 3.1415926535897 //---------------初期設定--------------- ScrpSlave scrp(PC_12,PD_2,PH_1,1); 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; PIDco pid_angle; PIDco x_pos; PIDco y_pos; TARGETco TG; BNO055 bno(SDA,SCL); DUALSHOCKco DS; DUALSHOCKco DS2; //sbdbt sb(PA_0,PA_1); DigitalOut led1(PA_10); DigitalOut led2(PB_15); DigitalOut led3(PB_2); DigitalOut led4(PC_6); Timer Time; long double timer; double theta; double X,Y; double auto_x_component; double auto_y_component; bool position_arrival = false; bool angle_arrival = false; double regulation; bool wheel_limit; //for odometry double theta2; double pltheta; //target double x_pos_target[] = {81.72,81.72}; double y_pos_target[] = {-90.47,90.47}; double angle_target[] = {-19.0,19.0}; double robot_angle; double max_mini(double a) { if(a > 450) { a = 450; } else if(a < -450) { a = -450; } else if(a == 0) { a = 0; } return a; } void rotate() { if(l1 == 1) { regulation = 0.3; } else { regulation = 1.0; } if(r1 == 1) { wheel_limit = 0.0; } else { wheel_limit = 1.0; } pid_1.pass_val(data_1.get(),TG.obt_target1() * regulation,0.00005,0.0,0); pid_2.pass_val(data_2.get(),TG.obt_target2() * regulation,0.00005,0.0,0); pid_3.pass_val(data_3.get(),TG.obt_target3() * regulation,0.00005,0.0,0); pid_4.pass_val(data_4.get(),TG.obt_target4() * regulation,0.00005,0.0,0); pid_1.wheel_ctl(PC_9,PC_8,wheel_limit); pid_2.wheel_ctl(PB_14,PB_13,wheel_limit); pid_3.wheel_ctl(PB_5,PB_4,wheel_limit); pid_4.wheel_ctl(PB_7,PB_6,wheel_limit); 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; } } void VectorDrection4(int x,int y,double theta) { x = x - 64; y = y - 64; //printf("%d %d\n",x,y); if(y > -abs(x)) { //上 DS2.pass_val(64,y + 64,0,0); } if(y < abs(x)) { //下 DS2.pass_val(64,y + 64,0,0); } if(x < -abs(y)) { //左 DS2.pass_val(x + 64,64,0,0); } if(x > abs(y)) { //右 DS2.pass_val(x + 64,64,0,0); } if(y == 0 && x == 0) { DS2.pass_val(64,64,0,0); } DS2.cal_input(); //printf("%lf %lf\n",DS2.obt_X(),DS2.obt_Y()); TG.pass_val(DS2.obt_X(),DS2.obt_Y(),theta); rotate(); } //------------------------------------- int main() { bno.reset(); 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,getL1); scrp.addCMD(6,getCircle); scrp.addCMD(7,getZone); scrp.addCMD(8,getR1); scrp.addCMD(9,getRstick_x); scrp.addCMD(10,getRstick_y); scrp.addCMD(50,change_mode); scrp.addCMD(51,getStop); while(1) { TG.pass_val(0,0,theta); if(stop == 0) { timer = Time.read_ms(); bno.setmode(OPERATION_MODE_IMUPLUS); bno.get_angles(); if(auto_mode == false) { if(redZone == true) { theta = (bno.euler.yaw - 90) * (PI / 180); theta2 = (bno.euler.yaw - 180) * (PI /180); } else { theta = (bno.euler.yaw + 90) * (PI / 180); theta2 = bno.euler.yaw * (PI /180); } } else { theta = bno.euler.yaw * (PI / 180); theta2 = (90 - bno.euler.yaw) * (PI /180); } if(theta > PI) { theta = -(2 * PI - theta); } else { } theta = theta - (PI / 4); if(theta2 > PI) { pltheta = theta2 - 2 * PI; } else { pltheta = theta2; } DS.pass_val(x_component,y_component,r2_num,l2_num); if(auto_mode == false) { DS.cal_input(); TG.pass_val(DS.obt_X(),DS.obt_Y(),theta); rotate(); DS.cal_input(); TG.pass_target(-1 * DS.obt_rotate()); rotate(); VectorDrection4(l_x_component,l_y_component,theta); } else { get_position(pltheta); if(auto_move == 1) { if(position_arrival == false) { x_pos.pid_ctl(x_position,x_pos_target[redZone],20.0,0.007,0.0005); y_pos.pid_ctl(y_position,y_pos_target[redZone],20.0,0.007,0.0005); auto_x_component = max_mini(x_pos.output); auto_y_component = max_mini(y_pos.output); TG.pass_val(auto_y_component,auto_x_component,theta); rotate(); //printf("go to target\n"); if((x_pos_target[redZone] - x_position) <= 0.5 && (x_pos_target[redZone] - x_position) >= -0.5 && (y_pos_target[redZone] - y_position) <= 0.5 && (y_pos_target[redZone] - y_position) >= -0.5) { position_arrival = true; TG.pass_val(64,64,theta); rotate(); printf("arrival_position\n"); } } else { if(r1 == 1) { TG.pass_val(0,0,theta); rotate(); TG.pass_target(0); rotate(); } else { if(angle_arrival == false) { //printf("go to angle_target\n"); if(bno.euler.yaw >= 0 && bno.euler.yaw <= 180) { robot_angle = bno.euler.yaw; } else { robot_angle = bno.euler.yaw - 360; } pid_angle.pid_ctl(robot_angle,angle_target[redZone],6.0,0.007,0.0001); printf("%lf,%lf\n",pid_angle.output,robot_angle); TG.pass_target(-1 * pid_angle.output); rotate(); if(angle_target[redZone] - robot_angle <= 1 && angle_target[redZone] - robot_angle >= -1) { angle_arrival = true; TG.pass_val(64,64,theta); rotate(); TG.pass_target(0); rotate(); //printf("arrival_angle\n"); } } } } } } while(Time.read_ms() - timer <= 50); if(Time.read_ms() > 2000000){ Time.reset(); } } else if(stop == 1) { TG.pass_val(0,0,theta); rotate(); TG.pass_target(0); rotate(); } else if(stop == 3) { NVIC_SystemReset(); } } } //BNOのピン(PB_3,PB_10)