Diff: moves/moves.cpp

Revision:

22:0ed9de464f40

Child:

23:ef274a44a867

diff -r 14133581387b -r 0ed9de464f40 moves/moves.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/moves/moves.cpp	Fri May 03 09:34:05 2019 +0000
@@ -0,0 +1,156 @@
+#include "moves.h"
+#include "pinnames.h"
+#include "sensors.h"
+#include "microinfinity.h"
+PwmOut motor_lo_f(pin_pwm[0][0]); //モータ正転
+PwmOut motor_lo_b(pin_pwm[0][1]); //モータ逆転
+PwmOut motor_li_f(pin_pwm[1][0]); //モータ正転
+PwmOut motor_li_b(pin_pwm[1][1]); //モータ逆転
+//PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
+PIDcontroller pid_lo(0.01, 0.000, 0.000);
+PIDcontroller pid_li(0.01, 0.000, 0.000);    //Kp.Ki,Kd
+Motor motor_lo(&motor_lo_f, &motor_lo_b),
+      motor_li(&motor_li_f, &motor_li_b); 
+OneLeg leg_lo, leg_li;
+Robot robot;
+
+enum WalkMode {
+    BACK,
+    STOP,
+    FORWARD,
+};
+int step_num_l, step_num_r;
+void turn(float target_degree)//turn_degreeを超えるまで旋回し続ける関数
+{
+    if(target_degree - degree0 > 0) {
+        while(target_degree - degree0 > 0)
+            turnLeft();
+    } else {
+        while(target_degree - degree0 < 0)
+            turnRight();
+    }
+    printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
+}
+void straightAndInfinity(float target_degree, float tolerance_degree)
+{
+    if(target_degree - degree0 > tolerance_degree) curveLeft();
+    else if(degree0 - target_degree > tolerance_degree) curveRight();
+    else straight();
+}
+void straight()
+{
+    can_send(FORWARD, motor_lo.getDutyLimit());
+    leg_lo.setTargetPose(360+step_num_l*180);
+    leg_li.setTargetPose(180+step_num_l*180);
+    robot.run();
+    motor_lo_f.write(0);
+    motor_lo_b.write(0);
+    motor_li_f.write(0);
+    motor_li_b.write(0);
+    while(1) {
+        if(bus_in.read() == 1) break;
+    }
+    step_num_l++;
+    step_num_r++;
+}
+void turnLeft()
+{
+    can_send(FORWARD, motor_lo.getDutyLimit());
+    leg_lo.setTargetPose(360+(step_num_l-2)*180);
+    leg_li.setTargetPose(180+(step_num_l-2)*180);
+    robot.run();
+    motor_lo_f.write(0);
+    motor_lo_b.write(0);
+    motor_li_f.write(0);
+    motor_li_b.write(0);
+    while(1) {
+        if(bus_in.read() == 1) break;
+    }
+    step_num_r++;
+    step_num_l--;
+}
+void curveLeft()
+{
+    can_send(FORWARD, motor_lo.getDutyLimit());
+    leg_lo.setTargetPose(360+(step_num_l-1)*180);
+    leg_li.setTargetPose(180+(step_num_l-1)*180);
+    robot.run();
+    motor_lo_f.write(0);
+    motor_lo_b.write(0);
+    motor_li_f.write(0);
+    motor_li_b.write(0);
+    while(1) {
+        if(bus_in.read() == 1) break;
+    }
+    step_num_r++;
+}
+void turnRight()
+{
+    can_send(BACK, motor_lo.getDutyLimit());
+    leg_lo.setTargetPose(360+step_num_l*180);
+    leg_li.setTargetPose(180+step_num_l*180);
+    robot.run();
+    motor_lo_f.write(0);
+    motor_lo_b.write(0);
+    motor_li_f.write(0);
+    motor_li_b.write(0);
+    while(1) {
+        if(bus_in.read() == 1) break;
+    }
+    step_num_r--;
+    step_num_l++;
+}
+void curveRight()
+{
+    can_send(STOP, motor_lo.getDutyLimit());
+    leg_lo.setTargetPose(360+step_num_l*180);
+    leg_li.setTargetPose(180+step_num_l*180);
+    robot.run();
+    motor_lo_f.write(0);
+    motor_lo_b.write(0);
+    motor_li_f.write(0);
+    motor_li_b.write(0);
+    while(1) {
+        if(bus_in.read() == 1) break;
+    }
+    step_num_l++;
+}
+void SetupMoves()
+{
+    motor_lo_f.period_us(100);
+    motor_lo_b.period_us(100);
+    motor_li_f.period_us(100);
+    motor_li_b.period_us(100);
+
+    pid_lo.setTolerance(10);
+    pid_li.setTolerance(10);
+    motor_lo.setEncoder(&ec_lo);
+    motor_lo.setResolution(1000);
+    motor_li.setEncoder(&ec_li);
+    motor_li.setResolution(600);
+    leg_lo.setMotor(&motor_lo);
+    leg_lo.setPIDcontroller(&pid_lo);
+    leg_li.setMotor(&motor_li);
+    leg_li.setPIDcontroller(&pid_li);
+    robot.setLeg(&leg_lo, &leg_li);
+    robot.setTickerTime(0.01); //モータ出力間隔 0.01
+}
+
+void reset()
+{
+    while(switch_lo.read()) {
+        motor_lo.output(0.3);
+    }
+    ec_lo.reset();
+    motor_lo.output(0.0);
+    while(switch_li.read()) {
+        motor_li.output(0.3);
+    }
+    ec_li.reset();
+    motor_li.output(0.0);
+}
+
+
+
+
+