2022_gorobo_Ateam
2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。
Dependencies: SBDBT arrc_mbed BNO055
Diff: main.cpp
- Revision:
- 11:264f992664b0
- Parent:
- 10:ad8fced7d6b6
- Child:
- 12:894e5ac49810
--- a/main.cpp Fri Mar 11 04:24:27 2022 +0000
+++ b/main.cpp Tue Mar 22 00:35:37 2022 +0000
@@ -34,6 +34,7 @@
TARGETco TG;
BNO055 bno(SDA,SCL);
DUALSHOCKco DS;
+DUALSHOCKco DS2;
//sbdbt sb(PA_0,PA_1);
@@ -43,7 +44,7 @@
DigitalOut led4(PC_6);
Timer Time;
-double timer;
+long double timer;
double theta;
double X,Y;
double auto_x_component;
@@ -51,31 +52,106 @@
bool position_arrival = false;
bool angle_arrival = false;
double regulation;
+bool wheel_limit;
//for odometry
double theta2;
double pltheta;
//target
-double x_pos_target[] = {25};
-double y_pos_target[] = {0};
-double angle_target[] = {-17.75};
+double x_pos_target[] = {81.72,81.72};
+double y_pos_target[] = {-90.47,90.47};
+double angle_target[] = {-26.2,26.2};
double robot_angle;
-double max_mini(double a){
- if(a > 450){
+double max_mini(double a)
+{
+ if(a > 450) {
a = 450;
- }else if(a < -450){
+ } else if(a < -450) {
a = -450;
- }else if(a == 0){
+ } else if(a == 0) {
a = 0;
}
return a;
-}
+}
+
+void rotate()
+{
+ if(l1 == 1) {
+ regulation = 0.3;
+ } else {
+ regulation = 1;
+ }
+
+ if(r1 == 1){
+ wheel_limit = 0;
+ }else{
+ wheel_limit = 1;
+ }
+
+ pid_1.pass_val(data_1.get(),TG.obt_target1() * regulation,0.00005,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_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(){
+int main()
+{
bno.reset();
Time.start();
TG.pass_val(0,0,0);
@@ -83,129 +159,119 @@
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){
- timer = Time.read_us();
- bno.setmode(OPERATION_MODE_IMUPLUS);
- bno.get_angles();
-
-
- 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;
- }
-
- get_position(pltheta);
- if(l1 == 0){
- DS.pass_val(x_component,y_component,r2_num,l2_num);
- }else{
- DS.pass_val(x_component * regulation,y_component * regulation,r2_num * regulation,l2_num * regulation);
- }
- //printf("%d\n",y_component);
-
- if(auto_mode == false){
- if(DS.cal_input() == true){
- TG.pass_val(DS.obt_X(),DS.obt_Y(),theta);
- }else{
- TG.pass_target(-1 * DS.obt_X());
- }
- }else{
-
- //printf("%lf\n",bno.euler.yaw);
- if(position_arrival == false){
- x_pos.pid_ctl(x_position,x_pos_target[0],10,0.007,0);
- y_pos.pid_ctl(y_position,y_pos_target[0],10,0.007,0);
- 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);
- printf("go to target\n");
- if((x_pos_target[0] - x_position) <= 0.5 && (x_pos_target[0] - x_position) >= -0.5 && (y_pos_target[0] - y_position) <= 0.5 && (y_pos_target[0] - y_position) >= -0.5){
- position_arrival = true;
- TG.pass_val(0,0,theta);
- printf("arrival_position\n");
+ 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{
- 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[0],5,0.007,0.0);
- printf("%lf,%lf\n",pid_angle.output,robot_angle);
- TG.pass_target(-1 * pid_angle.output);
- if(angle_target[0] - robot_angle <= 0.1 && angle_target[0] - robot_angle >= -0.1){
- angle_arrival = true;
- TG.pass_val(0,0,theta);
- printf("arrival_angle\n");
+ 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;
+ }
+
+ 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();
+ 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 {
+ 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);
+ 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(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);
+ 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);
+ rotate();
+ TG.pass_target(0);
+ rotate();
+ //printf("arrival_angle\n");
+ }
+ }
}
}
}
- }
-
- if(l1 == 1){
- regulation = 0.5;
- }else{
- regulation = 1;
- }
-
- pid_1.pass_val(data_1.get(),TG.obt_target1(),0.00007,0.000001,0.00000014);
- pid_2.pass_val(data_2.get(),TG.obt_target2(),0.00007,0.000001,0.00000014);
- pid_3.pass_val(data_3.get(),TG.obt_target3(),0.00007,0.000001,0.00000014);
- pid_4.pass_val(data_4.get(),TG.obt_target4(),0.00007,0.000001,0.00000014);
-
- pid_1.wheel_ctl(PC_9,PC_8,regulation);
- pid_2.wheel_ctl(PB_14,PB_13,regulation);
- pid_3.wheel_ctl(PB_5,PB_4,regulation);
- pid_4.wheel_ctl(PB_7,PB_6,regulation);
-
- if(TG.obt_target1() != 0){
- led1 = 1;
- }else{
- led1 = 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());
+ 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 {
+ TG.pass_val(0,0,theta);
+ rotate();
+ TG.pass_target(0);
+ rotate();
}
- 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());
- printf("%lf,%lf\n",x_position,y_position);
- 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_us() - timer <= 50 * 1000);
- }
+ }
}
//BNOのピン(PB_3,PB_10)
\ No newline at end of file