2022_gorobo_Ateam
2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。
Dependencies: SBDBT arrc_mbed BNO055
Diff: main.cpp
- Revision:
- 12:894e5ac49810
- Parent:
- 11:264f992664b0
--- a/main.cpp Tue Mar 22 00:35:37 2022 +0000
+++ b/main.cpp Fri Apr 08 07:35:55 2022 +0000
@@ -61,7 +61,7 @@
//target
double x_pos_target[] = {81.72,81.72};
double y_pos_target[] = {-90.47,90.47};
-double angle_target[] = {-26.2,26.2};
+double angle_target[] = {-19.0,19.0};
double robot_angle;
@@ -82,19 +82,19 @@
if(l1 == 1) {
regulation = 0.3;
} else {
- regulation = 1;
- }
-
- if(r1 == 1){
- wheel_limit = 0;
- }else{
- wheel_limit = 1;
+ regulation = 1.0;
}
- pid_1.pass_val(data_1.get(),TG.obt_target1() * regulation,0.00005,0,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);
+ 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);
@@ -123,23 +123,24 @@
}
}
-void VectorDrection4(int x,int y,double theta){
+void VectorDrection4(int x,int y,double theta)
+{
x = x - 64;
y = y - 64;
//printf("%d %d\n",x,y);
- if(y > -abs(x)){//上
+ if(y > -abs(x)) { //上
DS2.pass_val(64,y + 64,0,0);
}
- if(y < abs(x)){//下
+ if(y < abs(x)) { //下
DS2.pass_val(64,y + 64,0,0);
}
- if(x < -abs(y)){//左
+ if(x < -abs(y)) { //左
DS2.pass_val(x + 64,64,0,0);
}
- if(x > abs(y)){//右
+ if(x > abs(y)) { //右
DS2.pass_val(x + 64,64,0,0);
}
- if(y == 0 && x == 0){
+ if(y == 0 && x == 0) {
DS2.pass_val(64,64,0,0);
}
DS2.cal_input();
@@ -175,15 +176,15 @@
timer = Time.read_ms();
bno.setmode(OPERATION_MODE_IMUPLUS);
bno.get_angles();
- if(auto_mode == false){
- if(redZone == true){
+ if(auto_mode == false) {
+ if(redZone == true) {
theta = (bno.euler.yaw - 90) * (PI / 180);
theta2 = (bno.euler.yaw - 180) * (PI /180);
- }else{
+ } else {
theta = (bno.euler.yaw + 90) * (PI / 180);
theta2 = bno.euler.yaw * (PI /180);
}
- }else{
+ } else {
theta = bno.euler.yaw * (PI / 180);
theta2 = (90 - bno.euler.yaw) * (PI /180);
}
@@ -201,9 +202,8 @@
pltheta = theta2;
}
- get_position(pltheta);
+
DS.pass_val(x_component,y_component,r2_num,l2_num);
- //printf("%d\n",y_component);
if(auto_mode == false) {
DS.cal_input();
@@ -214,10 +214,11 @@
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],13,0.007,0.0005);
- y_pos.pid_ctl(y_position,y_pos_target[redZone],13,0.007,0.0005);
+ 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);
@@ -230,46 +231,48 @@
printf("arrival_position\n");
}
} 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],5,0.007,0.0001);
- printf("%lf,%lf\n",pid_angle.output,robot_angle);
- TG.pass_target(-1 * pid_angle.output);
+ if(r1 == 1) {
+ TG.pass_val(0,0,theta);
+ rotate();
+ TG.pass_target(0);
rotate();
- if(angle_target[redZone] - robot_angle <= 0.1 && angle_target[redZone] - robot_angle >= -0.1) {
- angle_arrival = true;
- TG.pass_val(64,64,theta);
+ } 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();
- TG.pass_target(0);
- rotate();
- //printf("arrival_angle\n");
+ 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");
+ }
}
}
}
}
}
- //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());
- printf("%lf,%lf\n",x_position,y_position);
- //printf("%lf\n",bno.euler.yaw);
- /*if(auto_mode == false) {
- printf("manual\n");
- } else if(auto_mode == true) {
- printf("auto\n");
- }*/
- //printf("%lf\n",pid_angle.output);
- //printf("%lf\n",bno.euler.yaw);
while(Time.read_ms() - timer <= 50);
- } else {
+ 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();
}
}
}