File content as of revision 14:1a3a673d85e3:

#include "mbed.h"
#include "pin.h"
#include "microinfinity.h"

//#define DEBUG_ON

#ifdef DEBUG_ON
#define DEBUG(...) printf("" __VA_ARGS__);
#else
#define DEBUG(...)
#endif

#define Pi 3.14159265359 //円周率π

enum WalkMode {
    BACK,
    STOP,
    FORWARD,
};
enum EVENT {
    NORMAL,
    GEREGE,
    GOAL,
};
float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。

class PIDcontroller //distanceをvalueに置き換えました
{
    float Kp_, Ki_, Kd_, tolerance_, time_delta_;
    float pile_, value_old_, target_;

public:
    bool IsConvergence_;                                //収束したかどうか
    PIDcontroller(float Kp, float Ki, float Kd);        //初期設定で係数を入力
    void setCoefficients(float Kp, float Ki, float Kd)
    {
        Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;
    }; //係数を変更するときに使う
    void setTimeDelta(float delta)
    {
        time_delta_ = delta;
    };
    void setTarget(float target);                       //目標位置の設定
    void setTolerance(float tolerance)
    {
        tolerance_ = tolerance;
    }; //許容誤差の設定
    float calc(float nowVal);           //現在位置と目標を比較してPID補正
    bool knowConvergence()
    {
        return IsConvergence_;
    };             //収束したかどうかを外部に伝える
};

class Motor //PIDコントローラ、エンコーダを含むモータのクラス
{
    PwmOut *pin_forward_, *pin_back_;
    Ec *ec_;                             //対応するエンコーダ
    float duty_, pre_duty_, duty_limit_; //dutyと現在のモータ位置
    int resolution_;
public:
    Motor(PwmOut *forward, PwmOut *back); //ピンをポインタ渡し
    void setDutyLimit(float limit)
    {
        duty_limit_ = limit;
    };
    float getDutyLimit()
    {
        return duty_limit_;
    };
    float getPosi();                   //ポジをエンコーダから取得
    void calcDuty(PIDcontroller *pid); //Duty比を計算
    void setEncoder(Ec *ec)
    {
        ec_ = ec;
    };           //エンコーダを設定
    void setResolution(int reso)
    {
        resolution_ = reso;
    };
    void output();                     //出力するだけ
    void output(float duty);
};

class OneLeg //足の挙動を制御する
{
    Motor *motor_;
    float target_pose_;

public:
    PIDcontroller *pid_;
    OneLeg() {};
    void setMotor(Motor *motor)
    {
        motor_ = motor;
    };
    void setPIDcontroller(PIDcontroller *pid)
    {
        pid_ = pid;
    };
    void setTargetPose(float target_pose);
    void actMotor();//モータ出力
};

class Robot
{
    float ticker_time_, air_wait_time_;
    OneLeg *Leg1_, *Leg2_;
    Timer timer;

public:
    Robot()
    {
        timer.reset();
        timer.start();
    };
    void setLeg(OneLeg *Leg1_, OneLeg *Leg2_);
    void setTickerTime(float ticker_time);
    void run();//ここがメインで走る記述
};



PIDcontroller::PIDcontroller(float Kp, float Ki, float Kd)
{
    Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;
    DEBUG("set Kp:%.3f  KI:%.3f  Kd:%.3f \n\r", Kp_, Ki_, Kd_);
    IsConvergence_=true;
}
void PIDcontroller::setTarget(float target)
{
    if (IsConvergence_) { //収束時のみ変更可能
        target_ = target;
        DEBUG("set Target: %.3f\n\r", target_);
        IsConvergence_ = false;
    } else {
        DEBUG("error: setTarget permission denied!\n");
    }
}
float PIDcontroller::calc(float nowVal)
{
    float out = 0;
    //PID計算ここで行う
    float deviation = target_ - nowVal; //目標との差分
    pile_ += deviation;                 //積分用に和を取る
    out = deviation * Kp_ - (nowVal - value_old_) / time_delta_ * Kd_ + pile_ * Ki_ * time_delta_;
    value_old_ = nowVal; //今のデータを保存
    //
    if (fabs(deviation) < tolerance_) { //収束した場合
        DEBUG("complete !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\r");
        out = 0;
        pile_ = 0;
        value_old_ = 0;
        IsConvergence_ = true;
    }
    return out;
}

Motor::Motor(PwmOut *forward, PwmOut *back)
{
    pin_forward_ = forward;
    pin_back_ = back;
}
float Motor::getPosi()
{
    float posi = 2.0*180*((float)(ec_->getCount)()/(float)resolution_);

    //DEBUG("value :%d  :%d\n\r", (ec_->getCount)(),resolution_);
    DEBUG("posi is %.4f\n\r",posi);
    return posi;
}
void Motor::calcDuty(PIDcontroller *pid)
{
    duty_ = pid->calc(getPosi());
    DEBUG("duty is %.4f\n\r",duty_);
}
void Motor::output()
{
    //DEBUG("dutyOut %.3f\n\r",duty_);
    //加速度が一定値を超えたら変更加える
    if (duty_ > 0) {
        //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);
        DEBUG("forward %.3f\n\r",pin_forward_->read());
    } else {
        //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);
        DEBUG("back %.3f\n\r",pin_back_->read());
    }
    pre_duty_ = duty_;
}
void Motor::output(float duty)
{
    duty_ = duty;
    //DEBUG("dutyOut %.3f\n\r",duty_);
    //加速度が一定値を超えたら変更加える
    if (duty_ > 0) {
        //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);
        DEBUG("forward %.3f\n\r",pin_forward_->read());
    } else {
        //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);
        DEBUG("back %.3f\n\r",pin_back_->read());
    }
    pre_duty_ = duty_;
}

void OneLeg::setTargetPose(float target_pose)
{
    target_pose_ = target_pose;
    //PIDにtargetを送る
    pid_->setTarget(target_pose_);
}
void OneLeg::actMotor()
{
    motor_->calcDuty(pid_);
    motor_->output();
}



void Robot::setLeg(OneLeg *Leg1, OneLeg *Leg2)
{
    Leg1_ = Leg1;
    Leg2_ = Leg2;
}
void Robot::setTickerTime(float ticker_time)
{
    ticker_time_ = ticker_time;
    Leg1_->pid_->setTimeDelta(ticker_time_);
    Leg2_->pid_->setTimeDelta(ticker_time_);
}
void Robot::run()
{
    while (!Leg1_->pid_->IsConvergence_ || !Leg2_->pid_->IsConvergence_) { //片方が収束していない時*/
        //ticker_time毎にモータに出力する
        float time_s = timer.read();
        Leg1_->actMotor();
        Leg2_->actMotor();
        float rest_time_s = ticker_time_ - (timer.read() - time_s);
        //ticker_timeまで待機
        if (rest_time_s > 0) {
            wait(rest_time_s);
            DEBUG("start:%.3f last: %.3f restTime: %.3f\n\r",time_s, timer.read(),rest_time_s);
        }

        else //時間が足りない場合警告
            printf("error: restTime not enough\n\r");
        DEBUG("loop end\n\r")
    }

}



////////////関数
void reset();
void setup();
void set_gyro();
double get_dist_forward();
double get_dist_back();
void can_send(int mode, float duty);
void straight();
void turnLeft();
void curveLeft();
void turnRight();
void curveRight();
void turn(float target_degree);//回転角, 収束許容誤差
void straightAndInfinity(float target_degree, float tolerance_degree);//角度を補正するのと前進

void wait_gerege();

////////////定数

////////////変数
int hand_mode=NORMAL;

//PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
//しかし変更を多々行うためポインタ渡しにしてある
//文が長くなると困るため, PID係数の変更は直接PIDコントローラを介して行う
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); //forward,backのピンを代入
OneLeg leg_lo, leg_li;
Robot robot;

int step_num_l, step_num_r;

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int main()
{
    setup();

    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
    motor_lo.setDutyLimit(0.5);//0.5
    motor_li.setDutyLimit(0.5);
    printf("reset standby\n\r");
    reset();
    printf("bus standby\n\r");

    while(1) {
        if(bus_in.read() == 1) break;
    }
    printf("bus is %d\n\r", bus_in.read());

    wait(0.5);

    straight();

    wait_gerege();

    hand_mode = GEREGE;

    set_gyro();
    //Sample
    //スタート直進
    printf("dist:%.3f\r\n", get_dist_forward());

    for(int i=0;i<3;++i){
        while(get_dist_back() < 280)
        {
            straightAndInfinity(0, 5);
            //wait(0.01);
            printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
        }
    }
    //printf("back dist:%.3f\r\n", get_dist_back());
    //段差前旋回
    motor_lo.setDutyLimit(0.4);//0.5
    motor_li.setDutyLimit(0.4);
    turn(40.0);
    //段差乗り越え
    for(int i= 0;i<5;++i) straight();
    while(get_dist_back() > 40) straight();
    //段差後旋回
    printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
    turn(90.0);
    printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
    //前進
    motor_lo.setDutyLimit(0.5);//0.5
    motor_li.setDutyLimit(0.5);
    for(int i=0;i<3;++i)
    {
        while(get_dist_forward() > 65)
        {
            straightAndInfinity(90.0, 5.0);
            printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
        }
    }
    //ロープ前旋回
    printf("before roop deg:%.3f\n\r",degree0);
    turn(0);

    //ロープ
    while(mode4.read() == 0)
    {

        straightAndInfinity(0, 5);
    }

    printf("go to uhai deg:%.3f forward dist:%.3f \n\r",degree0,get_dist_forward());
    for(int i=0;i<3;++i)
    {
        while(get_dist_forward() > 65)//65
        {
            straightAndInfinity(0, 5);
            printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
        }
    }
    printf("turn");
    turn(-90);

    
    motor_lo.setDutyLimit(0.2);//0.5
    motor_li.setDutyLimit(0.2);

    for(int i=0; i<15; ++i)straightAndInfinity(0, 5);
    printf("wall standby");
    while(get_dist_back() < 250) {
        straightAndInfinity(0, 5);
        printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
    }
    for(int i=0; i<2; ++i) {
        while(get_dist_forward() > 65) {
            straightAndInfinity(0, 5);
            printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
        }
    }
    hand_mode = GOAL;
    straight();
    printf("uhai!!!!!!!!!!!!!!!");

    
}
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 setup()
{
    can1.frequency(1000000);
    motor_lo_f.period_us(100);
    motor_lo_b.period_us(100);
    motor_li_f.period_us(100);
    motor_li_b.period_us(100);

    hand.mode(PullUp);
    switch_lo.mode(PullUp);
    switch_li.mode(PullUp);
    mode4.mode(PullUp);

}

void set_gyro()
{
    device.baud(115200);
    device.format(8,Serial::None,1);
    device.attach(dev_rx, Serial::RxIrq);
    wait(0.05);
    theta0=degree0;
    check_gyro();
}


//////////////////////////////////////can
void can_send(int mode, float duty)
{
    char data[2]= {0};
    int send=mode * 10 + (int)(duty * 10.0);
    //printf("duty is %.3f\n\r",duty);
    data[0]=send & 0b11111111;
    data[1]=hand_mode;

    if(can1.write(CANMessage(0,data,2)))led4=1;
    else led4=0;
}
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);
}
double get_dist_back()
{
    sensor_back.start();
    wait_ms(10);//10           //sensor.start()で信号を送ったあと反射波が帰ってきてから距離データが更新されるため、少し待つ必要がある。
    //何ループも回す場合は不要。また、時間は適当だから調整が必要。
    float dist = sensor_back.get_dist_cm();
    if(dist < 0.1) dist = 2000.0;
    return dist;
}
double get_dist_forward()
{
    sensor_forward.start();
    wait_ms(10);//10
    float dist = sensor_forward.get_dist_cm();
    if(dist < 0.1) dist = 2000.0;
    return dist;
}

void wait_gerege()
{
    int i = 0;
    while(i!=100) {
        if(hand.read()==0)i++;
    }
}