![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
version which is easier to read
Dependencies: mbed KondoServoLibrary Encoder
Diff: User.cpp
- Revision:
- 22:f0e17f3129cc
- Parent:
- 21:0f12ee2322e4
- Child:
- 23:0c6bc29c8f86
--- a/User.cpp Thu Mar 12 07:32:03 2020 +0000 +++ b/User.cpp Fri Mar 13 08:15:38 2020 +0000 @@ -12,7 +12,7 @@ int RSX,RSY,LSX,LSY,BSU,BSL; //Digitalout cs(p8); -int move=5,shoot=0,ball=0,box=0; +int move=5,shoot=0,ball=0,box=0,ball_count=0,box_count=0; CAN controller(p30,p29);//CANpin_name DigitalOut led1(LED1); @@ -39,9 +39,9 @@ int id2 = 1; double SERVO2DEG = 270.0 / (11500 - 3500); double first = (6800 - 3500) * SERVO2DEG; -double grab = (3600 - 3500) * SERVO2DEG; +double grab_ball = (3600 - 3500) * SERVO2DEG; double pass = (5000 - 3500) * SERVO2DEG; -double grab2 = (3550 - 3500) * SERVO2DEG; +double grab_box = (3550 - 3500) * SERVO2DEG; void cal() { @@ -126,16 +126,16 @@ shoot = 1; led1 = 1; } else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X - shoot = 4; + ball = 1; led2 = 1; } else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//△ - shoot = 3; + box = 1; led3 = 1; } else if((ButtonState >> BUTTONSQUARE)&1 == 1) {//□ - shoot = 2; + ball = 2; led4 = 1; } else if((ButtonState >> BUTTONR2)&1 == 1) {//R2 - shoot = 5; + box = 2; led4 = 1; } show_angle(); @@ -229,24 +229,22 @@ void catch_ball() { - switch (shoot) { - case 2: - if(ball ==0) { + + switch (ball) { + case 1: //ball servo + if(ball_count ==0) { servo.set_degree(id, first); wait(0.5); shoot =0; ball = 1; - } else if(ball ==1) { - servo.set_degree(id, grab); + } else if(ball_count ==1) { + servo.set_degree(id, grab_ball); wait(0.5); shoot = 0; ball =0; } break; - case 3: - servo.set_degree(id2, grab2); - break; - case 4: + case 2://ball air if (out1==0) { out1=1; shoot=0; @@ -254,17 +252,32 @@ out1=0; shoot=0; } - break; - case 5: - if (out3==0) { - out3=1; - } else if (out3 == 1) { - out3=0; - } } } void catch_box() { + switch(box) { + case 1:// box servo + if(box_count ==0) { + servo.set_degree(id2, first); + wait(0.5); + shoot =0; + ball = 1; + } else if(box_count ==1) { + servo.set_degree(id2, grab_box); + wait(0.5); + shoot = 0; + ball =0; + } + case 2: //box air + if (out3==0) { + out3=1; + shoot = 0; + } else if (out3 == 1) { + out3=0; + shoot = 0; + } + } } \ No newline at end of file