yuto kawamura
/
George_Slave_BOTHMOVE
right and left move at the same time
Diff: main.cpp
- Revision:
- 4:db1640bd0e89
- Parent:
- 3:29999b02e940
- Child:
- 5:28581157108b
diff -r 29999b02e940 -r db1640bd0e89 main.cpp --- a/main.cpp Wed May 01 05:46:53 2019 +0000 +++ b/main.cpp Wed May 01 06:42:07 2019 +0000 @@ -140,7 +140,7 @@ //加速度が一定値を超えたら変更加える if (duty_ > 0) { - if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max; + //if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max; double output_duty=min(fabs(duty_), duty_limit_); pin_forward_->write(output_duty); pin_back_->write(0); @@ -148,8 +148,8 @@ } else { - if (pre_duty_ - duty_ > accel_max) - duty_ = pre_duty_ - accel_max; + //if (pre_duty_ - duty_ > accel_max) + // duty_ = pre_duty_ - accel_max; double output_duty=min(fabs(duty_), duty_limit_); pin_forward_->write(0); pin_back_->write(output_duty); @@ -234,6 +234,9 @@ ////////////関数 void setup(); +void straight(int &step_num_l, int &step_num_r); +void turnLeft(int &step_num_l, int &step_num_r); +void turnRight(int &step_num_l, int &step_num_r); void can_receive(float &tar_ro, float &tar_ri); void reset(); @@ -270,8 +273,8 @@ robot.setLeg(&leg_ro, &leg_ri); robot.setTickerTime(0.01); //モータ出力間隔 0.01 - motor_ro.setDutyLimit(0.5); - motor_ri.setDutyLimit(0.5); + motor_ro.setDutyLimit(0.3); + motor_ri.setDutyLimit(0.3); reset(); bus_out = 1; @@ -279,20 +282,20 @@ - float target_ro, target_ri; + int target_ro, target_ri; while(1) { - float target_ro_now, target_ri_now; + float target_ro_now = target_ro; + float target_ri_now = target_ri; can_receive(target_ro_now,target_ri_now); - printf("tar_pre:%.3f tar_now:%.3f\n\r",target_ro,target_ro_now); - if(target_ro_now != target_ro) + //printf("tar_pre:%.3f tar_now:%.3f\n\r",target_ro,target_ro_now); + if((int)target_ro_now != target_ro) { - target_ro = target_ro_now; - target_ri = target_ri_now; + target_ro = (int)target_ro_now; + target_ri = (int)target_ri_now; bus_out = 0; - leg_ro.setTargetPose(target_ro); - leg_ri.setTargetPose(target_ri); - robot.run(); + printf("target is %d\n\r",target_ro); + straight(target_ro, target_ri); } motor_ro_f.write(0); motor_ro_b.write(0); @@ -303,6 +306,39 @@ } +void straight(int &step_num_l, int &step_num_r) +{ + leg_ro.setTargetPose(360+step_num_l*180); + leg_ri.setTargetPose(180+step_num_l*180); + robot.run(); + motor_ro_f.write(0); + motor_ro_b.write(0); + motor_ri_f.write(0); + motor_ri_b.write(0); +} +void turnLeft(int &step_num_l, int &step_num_r) +{ + leg_ro.setTargetPose(360+(step_num_l-2)*180); + leg_ri.setTargetPose(180+(step_num_l-2)*180); + robot.run(); + motor_ro_f.write(0); + motor_ro_b.write(0); + motor_ri_f.write(0); + motor_ri_b.write(0); +} +void turnRight(int &step_num_l, int &step_num_r) +{ + leg_ro.setTargetPose(360+(step_num_l-1)*180); + leg_ri.setTargetPose(180+(step_num_l-1)*180); + robot.run(); + motor_ro_f.write(0); + motor_ro_b.write(0); + motor_ri_f.write(0); + motor_ri_b.write(0); + +} + + ////////////////////////////////////////////// void setup() { @@ -318,6 +354,8 @@ servo.init(); } + + void reset() { while(switch_ro.read()) { @@ -336,6 +374,8 @@ } + + //////////////////////////////////////////can void can_receive(float &tar_ro, float &tar_ri) {