File content as of revision 6:2bc2950f32d8:

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


//#define DEBUG_ON

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

#define Pi 3.14159265359 //円周率π


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 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)
    {
        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
    {
        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());
    }
}

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")
    }

}




////////////変数
bool hand_mode=0;

////////////関数
void setup();
void straight(int &step_num_l, int &step_num_r);
void turnLeft(int &step_num_l, int &step_num_r);
void turnRight(int &step_num_l, int &step_num_r);
void can_receive(float &tar_ro, float &tar_ri);
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 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();
    bus_out = 1;
    printf("start\n\r");
    
    
    
    int target_ro, target_ri;
    while(1) 
    {
        float target_ro_now = target_ro;
        float target_ri_now = target_ri;
        can_receive(target_ro_now,target_ri_now);
        //printf("tar_pre:%.3f tar_now:%.3f\n\r",target_ro,target_ro_now);
        if((int)target_ro_now != target_ro)
        {
            target_ro = (int)target_ro_now;
            target_ri = (int)target_ri_now;
            bus_out = 0;
            //printf("target is %d\n\r",target_ro);
            straight(target_ro, target_ri);
        }
        bus_out = 1;
    }
    
}

void straight(int &step_num_l, int &step_num_r)
{
    leg_ro.setTargetPose(360+step_num_l*180);
    leg_ri.setTargetPose(180+step_num_l*180);
    robot.run();
    motor_ro_f.write(0);
    motor_ro_b.write(0);
    motor_ri_f.write(0);
    motor_ri_b.write(0);
}
void turnLeft(int &step_num_l, int &step_num_r)
{
    leg_ro.setTargetPose(360+(step_num_l-2)*180);
    leg_ri.setTargetPose(180+(step_num_l-2)*180);
    robot.run();
    motor_ro_f.write(0);
    motor_ro_b.write(0);
    motor_ri_f.write(0);
    motor_ri_b.write(0);
}
void turnRight(int &step_num_l, int &step_num_r)
{
    leg_ro.setTargetPose(360+step_num_l*180);
    leg_ri.setTargetPose(180+step_num_l*180);
    robot.run();
    motor_ro_f.write(0);
    motor_ro_b.write(0);
    motor_ri_f.write(0);
    motor_ri_b.write(0);
    
}


//////////////////////////////////////////////
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);

    switch_ro.mode(PullUp);
    switch_ri.mode(PullUp);

    servo.init();
}



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");
}




//////////////////////////////////////////can
void can_receive(float &tar_ro, float &tar_ri)
{
    CANMessage msg;
    for(int i=0; i<5; i++) {
        if(can.read(msg)) {
            if(msg.id==0) {
                tar_ro= msg.data[0] + ((msg.data[2]&0b1111)<<8) - 360;
                tar_ri= msg.data[1] + ((msg.data[2]&0b11110000)<<4) - 360;
                hand_mode= msg.data[3];

                break;
            }
            led2=1;
        } else led2=0;
    }
}