File content as of revision 58:dc0faefeba39:

#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,
    RESET,
};
int step_num_l, step_num_r;
void WaitAnotherLeg();

void ResetRun()
{
    int target_step = step_num_l;
    leg_lo.setTargetPose(180 + target_step*180);
    leg_li.setTargetPose(180 + target_step*180);
    can_send(RESET, motor_lo.getDutyLimit());
    led1 = 1;
    robot.run();
    led1 = 0;
    motor_lo_f.write(0);
    motor_lo_b.write(0);
    motor_li_f.write(0);
    motor_li_b.write(0);
    FileWrite();
}



//収束していなければLED1をON. param:: d_step_l:左足を動かす歩数。負なら後ろに進む
void Run(WalkMode mode, int d_step_l)
{
    int target_step = step_num_l + d_step_l;
    can_send(mode, motor_lo.getDutyLimit());
    leg_lo.setTargetPose(180 + target_step*180);
    leg_li.setTargetPose(target_step*180);
    //収束前
    led1 = 1;
    robot.run();
    led1 = 0;
    step_num_l = target_step;
    switch(mode) {
        case FORWARD:
            step_num_r++;
            break;
        case STOP:
            break;
        case BACK:
            step_num_r--;
            break;
    }

    motor_lo_f.write(0);
    motor_lo_b.write(0);
    motor_li_f.write(0);
    motor_li_b.write(0);
    FileWrite();
    WaitAnotherLeg();
}
void WaitAnotherLeg()
{
    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 lopeAndInfinity(float target_degree, float tolerance_degree)
{
    enum Mode {
        STRAIGHT,
        LEFT,
        RIGHT,
    };
    int is_turn = 0;
    static int lope_count = 0;
    const int max_count = 3;
    Mode mode;
    if(target_degree - degree0 > tolerance_degree) {
        mode = LEFT;
    } else if(degree0 - target_degree > tolerance_degree) {
        mode = RIGHT;
    } else {
        mode = STRAIGHT;
    }
    //カーブしても治らなければturnに切り替え
    if(lope_count >= max_count) {
//        mode = STRAIGHT;
        is_turn = 1;
        lope_count=0;
    }
    switch(mode) {
        case STRAIGHT:
            straight();
            lope_count=0;
            break;
        case LEFT:
            if(is_turn == 1) {
                turnLeft();
            } else {
                curveLeft();
            }
            lope_count++;
            break;
        case RIGHT:
            if(is_turn == 1) {
                turnRight();
            } else {
                curveRight();
            }
            lope_count++;
            break;
    }
}
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 stop()
{
    Run(STOP,0);
}
void straight()
{
    Run(FORWARD,1);
}
void turnLeft()
{
    Run(FORWARD,-1);
}
void curveLeft()
{
    Run(FORWARD,0);
}
void turnRight()
{
    Run(BACK,1);
}
void curveRight()
{
    Run(STOP, 1);
}
void phasing(float target_degree)
{
    if(step_num_l%2 != step_num_r%2) {
        if(target_degree - degree0 > 0) {
            curveLeft();
        } else {
            curveRight();
        }
    }
}
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);
}
void HandMove()
{
    can_send(STOP,0);
}