File content as of revision 10:2973cea54efd:

#include "mbed.h"
#include "pin.h"
#include "debug.h"
#include "robot.h"
enum WALKMODE {
    BACK,
    STOP,
    FORWARD,
};
enum EVENT {
    NORMAL,
    GEREGE,
    GOAL,
};

float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。
int hand_mode=NORMAL;

void setup();
void forward();
void stop();
void back();
void can_receive(int &mode, float &duty);
void reset();


//PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
//しかし変更を多々行うためポインタ渡しにしてある
//文が長くなると困るため, PID係数の変更は直接PIDコントローラを介して行う
PIDcontroller pid_ro(0.01, 0.000, 0.000);
PIDcontroller pid_ri(0.01, 0.000, 0.000);    //Kp.Ki,Kd
Motor motor_ro(&motor_ro_f, &motor_ro_b),
      motor_ri(&motor_ri_f, &motor_ri_b); //forward,backのピンを代入
OneLeg leg_ro, leg_ri;
Robot robot;

int step_num_r = 0;
int main()
{
    printf("standby ok\n\r");
    setup();
    pid_ro.setTolerance(10);
    pid_ri.setTolerance(10);

    motor_ro.setEncoder(&ec_ro);
    motor_ro.setResolution(1000);
    motor_ri.setEncoder(&ec_ri);
    motor_ri.setResolution(1000);

    leg_ro.setMotor(&motor_ro);
    leg_ro.setPIDcontroller(&pid_ro);
    leg_ri.setMotor(&motor_ri);
    leg_ri.setPIDcontroller(&pid_ri);

    robot.setLeg(&leg_ro, &leg_ri);
    robot.setTickerTime(0.01); //モータ出力間隔 0.01

    motor_ro.setDutyLimit(0.5);
    motor_ri.setDutyLimit(0.5);

    reset();
    for(int i=0; i<5; ++i)
        servo.set_degree(0,0);
    bus_out = 1;
    printf("start\n\r");
    int mode = -1;
    air_hand = 1;

    while(1) {
        float duty = 0.0;
        can_receive(mode,duty);
        printf("mode:%d duty:%.3f\n\r", mode, duty);
        motor_ro.setDutyLimit(duty);
        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));
        }
        bus_out = 0;
        //printf("target is %d\n\r",target_ro);
        if(mode == FORWARD) forward();
        else if(mode == STOP) stop();
        else if(mode == BACK) back();
        bus_out = 1;
    }

}

void forward()
{
    leg_ro.setTargetPose(360+step_num_r*180);
    leg_ri.setTargetPose(180+step_num_r*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++;
}
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);
}

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--;
}
//////////////////////////////////////////////
void setup()
{
    can.frequency(1000000);
    motor_ro_f.period_us(100);
    motor_ro_b.period_us(100);
    motor_ri_f.period_us(100);
    motor_ri_b.period_us(100);
    air_hand = 1;//ゲレゲを離す手の形にしておく
    switch_ro.mode(PullUp);
    switch_ri.mode(PullUp);
    servo.init();
    wait(0.01);
    for(int i=0; i<5; ++i)
        servo.set_degree(0,0);
    //(7200 - 3500) * 270.0/(11500 - 3500)
}

void reset()
{
    while(switch_ro.read()) {
        motor_ro.output(0.3);
    }
    ec_ro.reset();
    motor_ro.output(0.0);
    printf("ro OK\n\r");
    while(switch_ri.read()) {
        motor_ri.output(0.3);
    }

    ec_ri.reset();
    motor_ri.output(0.0);
    printf("ri OK\n\r");
}

void can_receive(int &mode, float &duty)
{
    CANMessage msg;
    while(1) {
        //printf("roop\r\n");
        if(can.read(msg)) {
            if(msg.id==0) {
                mode= msg.data[0]*0.1;
                duty = (msg.data[0]%10)*0.1;
                hand_mode= msg.data[1];
                break;
            }
            led2=1;
        } else led2=0;
    }
}