yuto kawamura
/
George_Slave_BOTHMOVE
right and left move at the same time
Diff: main.cpp
- Revision:
- 11:0522b336eb82
- Parent:
- 10:2973cea54efd
- Child:
- 12:e6fd3a3201f0
diff -r 2973cea54efd -r 0522b336eb82 main.cpp --- a/main.cpp Wed May 08 08:21:08 2019 +0000 +++ b/main.cpp Wed May 08 08:45:48 2019 +0000 @@ -33,6 +33,8 @@ motor_ri(&motor_ri_f, &motor_ri_b); //forward,backのピンを代入 OneLeg leg_ro, leg_ri; Robot robot; +DigitalOut led[] = {LED1,LED2,LED3,LED4}; + int step_num_r = 0; int main() @@ -74,7 +76,7 @@ motor_ri.setDutyLimit(duty); if(hand_mode == GEREGE) { air_hand = 0; - + } else if(hand_mode == GOAL) { for(int i=0; i<10; ++i) servo.set_degree(0,(7200 - 3500) * 270.0/(11500 - 3500)); @@ -88,39 +90,34 @@ } } - -void forward() +void Run(int d_step_r) { - leg_ro.setTargetPose(360+step_num_r*180); - leg_ri.setTargetPose(180+step_num_r*180); + int target_step = step_num_r + d_step_r; + leg_ro.setTargetPose(180+target_step*180); + leg_ri.setTargetPose(target_step*180); + led[0] = 1; robot.run(); + led[0] = 0; + step_num_r = target_step; motor_ro_f.write(0); motor_ro_b.write(0); motor_ri_f.write(0); motor_ri_b.write(0); - step_num_r++; + +} + +void forward() +{ + Run(1); } void stop() { - leg_ro.setTargetPose(360+(step_num_r-1)*180); - leg_ri.setTargetPose(180+(step_num_r-1)*180); - robot.run(); - motor_ro_f.write(0); - motor_ro_b.write(0); - motor_ri_f.write(0); - motor_ri_b.write(0); + Run(0); } void back() { - leg_ro.setTargetPose(360+(step_num_r-2)*180); - leg_ri.setTargetPose(180+(step_num_r-2)*180); - robot.run(); - motor_ro_f.write(0); - motor_ro_b.write(0); - motor_ri_f.write(0); - motor_ri_b.write(0); - step_num_r--; + Run(-1); } ////////////////////////////////////////////// void setup()