File content as of revision 26:5fb1aa9cb7f0:

#include "moves.h"
#include "pinnames.h"
#include "sensors.h"
#include "microinfinity.h"
#include "debug.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 WaitAnotherLeg()
{
    FileWrite();
    while(1) {
        if(bus_in.read() == 1) break;
    }
}
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();
    }
    DEBUG("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 climbAndInfinity(float target_degree, float tolerance_degree)
{
    if(target_degree - degree0 > tolerance_degree) {//左に曲がる

        curveLeft();
    } else if(degree0 - target_degree > tolerance_degree) {//右に曲がる

        curveRight();
    } else {

        straight();
    }
}
void onestraight()
{
    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);
    step_num_l++;
    step_num_r++;
}
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);
    WaitAnotherLeg();
    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);
    WaitAnotherLeg();
    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);
    WaitAnotherLeg();
}
void backLeft()
{
    can_send(STOP, 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);
    WaitAnotherLeg();
    step_num_l--;
}
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);
    WaitAnotherLeg();
    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);
    WaitAnotherLeg();
    step_num_l++;
}
void backRight()
{
    can_send(BACK, 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);
    WaitAnotherLeg();
}
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()
{
    motor_lo.setDutyLimit(0.2);
    motor_li.setDutyLimit(0.2);
    while(switch_lo.read()) {
        motor_lo.output(0.2);
    }
    ec_lo.reset();
    motor_lo.output(0.0);
    while(switch_li.read()) {
        motor_li.output(0.2);
    }
    ec_li.reset();
    motor_li.output(0.0);
}