File content as of revision 3:df0c9883e403:

#include "mbed.h"
#include "EC.h"
#include "KondoServo.h"
#include "hcsr04.h"
#include "pin.h"
#include "microinfinity.h"


Timer timer;

//パラメタ
double dutylimit_nomal = 0.2; // duty出力の上限値(通常時)
double dutylimit_turn = 0.4; // duty出力の上限値（回転時）
int ecgear = 25; //エンコーダのギアの歯数
/*double Kp_r = 0.342000; //P値
double Ki_r = 0.013062; //I値
double Kd_r = 0.003266; //D値
double Kp_l = 0.342000; //P値
double Ki_l= 0.013062; //I値
double Kd_l = 0.003266; //D値*/
double Kp_r_n = 0.0015; //P値  通常時右
double Ki_r_n = 0.001; //I値
double Kd_r_n = 0.0001; //D値
double Kp_l_n = 0.0015; //P値  回転時右
double Ki_l_n = 0.001; //I値
double Kd_l_n = 0.0001; //D値
double Kp_r_t = 0.004; //P値
double Ki_r_t = 0.001; //I値
double Kd_r_t = 0.0001; //D値
double Kp_l_t = 0.004; //P値
double Ki_l_t = 0.001; //I値
double Kd_l_t = 0.0001; //D値
double leg_wait_time = 0.2; //脚の伸縮にかかる時間
double leg_wait_time_l = 1; //長い足にかかる時間
double target_max = 310; //直動機構の目標座標の上限（オーバーシュートを考慮して短めに設定）
double target_min = 20; //直動機構の目標座標の下限（オーバーシュートを考慮して短めに設定）
double servo_iniposi=105;  //サーボ初期角度
double error_target=10;//PIDでの許容誤差

double pid_time=0.01;//PIDの割り込み時間
double accel_max=0.01;//加速制限



//定数
#define Pi 3.14159265359 //円周率π

//変数
int target_num=0;           //現在の目標プロット
bool move_r=0,move_l=0;    //
double target_r_out=0,target_l_out=0;
bool reach_target_r=0,reach_target_l=0;
double posi_r=0,posi_l=0;
double pre_out_r=0,pre_out_l=0;
bool turn_mode=0;






void setup();
void out_r(double);//モーターの出力
void out_l(double);//モーターの出力
void pid_r(double,double); //PID   (移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
void pid_l(double,double); //PID   (移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
void move(double,double);  //(target_r,target_l)
void up(int);
void down(int);
//void reset();    //脚位置の初期化
void turn(double);   //旋回動作
void turn_2(double);
void straight(double);   //目的地まで直進
void wait_MR1();
double get_dist();
void output();
void sanddune();
void tussock();


Ticker ticker;


int main()
{
    setup();

    ticker.attach(&output,pid_time);
    
    /*while(1){
        printf("%f\r\n",degree0);
    }*/
    
    wait(3);
    /*down(long_r);
    down(long_l);
    wait(1);
    up(long_r);*/
    
    //straight(2800);
    //turn_2(45);
    //move(50,50);
    //move(190,190);
    sanddune();
    
    /*for(int i=0;i<15;i++){
        move(220,220);
        move(80,80);
    }*/
    
    /*for(int i=0;i<5;i++){
        move(300,300);
        move(30,30);
    }
    turn_2(45);
    for(int i=0;i<2;i++){
        move(300,300);
        move(30,30);
    }
    turn_2(0);
    
    for(int i=0;i<4;i++){
        move(300,300);
        move(30,30);
    }*/
    
    
    
    //sanddune();
    
    //move(300,300);
    //move(-300,-300);

    //wait(5);
    /*move(300,300);
    move(30,30);
    move(300,300);
    move(30,30);
    move(300,300);
    move(30,30);
    move(300,300);
    move(30,30);
    move(300,300);
    move(30,30);*/


    /*
        wait_MR1();

        for(int i=0; i<4; i++) {
            move(360,360);
            move(10,10);
        }
        turn(700);
        sanddune();

        move(360,360);
        move(10,10);
        move(10,10);

        turn(-700);

        tussock();*/
}

double get_dist()
{
    sensor.start();
    //wait_ms(50);           //sensor.start()で信号を送ったあと反射波が帰ってきてから距離データが更新されるため、少し待つ必要がある。
    //何ループも回す場合は不要。また、時間は適当だから調整が必要。
    return sensor.get_dist_cm();
}



//初期設定
void setup()
{
    motor_r_f.period_us(100);
    motor_r_b.period_us(100);
    motor_l_f.period_us(100);
    motor_l_b.period_us(100);
    switch_max.mode(PullUp);
    switch_min.mode(PullUp);
    switch_x.mode(PullUp);
    switch_y.mode(PullUp);
    switch_hand.mode(PullUp);
    for(int i=0; i<4; i++) {
        air[i]=1;
    }
    air[4]=0;
    air[5]=0;
    servo.init();
    wait_us(1000);
    servo.set_degree(0,servo_iniposi);
    
    device.baud(115200);
    device.format(8,Serial::None,1);
    device.attach(dev_rx, Serial::RxIrq);
    
    wait(0.05);
    theta0=degree0;
}
int k=0;
void output()
{
    posi_r=1.0*ecgear*Pi*ec_r.getCount()/resolution;
    posi_l=1.0*ecgear*Pi*ec_l.getCount()/resolution;
    
    if(move_r) pid_r(target_r_out,error_target);
    else out_r(0);
    if(move_l) pid_l(target_l_out,error_target);
    else out_l(0);
    
    k++;
    if(k>50){
        printf("%f, %f, %f\r\n",posi_r,posi_l,degree0);
        k=0;
    }
}


//モーターの出力
void out_r (double duty)
{
    double dutylimit=0;
    if(turn_mode)dutylimit=dutylimit_turn;
    else dutylimit=dutylimit_nomal;
    
    if(duty > 0) { //入力duty比が正の場合
        if(duty-pre_out_r >accel_max)duty=pre_out_r+accel_max;
        //if(switch_max == 0) { //上限のリミットスイッチが押されていない場合
        if( fabs( duty ) < dutylimit ) { //制限値内
            motor_r_f = fabs(duty);
            motor_r_b = 0;
        } else { //制限値超
            motor_r_f = dutylimit;
            motor_r_b = 0;
        }
        /*} else { //上限のリミットスイッチが押されている場合
            motor_r_f = 0;
            motor_r_b = 0;
        }*/

    } else {//入力duty比が負の場合
        if(pre_out_r-duty >accel_max)duty=pre_out_r-accel_max;
        //if(switch_min == 0) { //下限のリミットスイッチが押されていない場合
        if( fabs(duty) < dutylimit) { //制限値内
            motor_r_f = 0;
            motor_r_b = fabs(duty);
        } else { //制限値超
            motor_r_f = 0;
            motor_r_b = dutylimit;
        }
        /*} else { //下限のリミットスイッチが押されている場合
            motor_r_f = 0;
            motor_r_b = 0;
        }*/
    }
    pre_out_r=duty;
    //if(k>49)printf("%f\r\n",dutylimit);
}

void out_l (double duty)
{
    double dutylimit=0;
    if(turn_mode)dutylimit=dutylimit_turn;
    else dutylimit=dutylimit_nomal;
    
    if(duty > 0) { //入力duty比が正の場合
        if(duty-pre_out_l >accel_max)duty=pre_out_l+accel_max;
        //if(switch_max == 0) { //上限のリミットスイッチが押されていない場合
        if( fabs( duty ) < dutylimit ) { //制限値内
            motor_l_f = fabs(duty);
            motor_l_b = 0;
        } else { //制限値超
            motor_l_f = dutylimit;
            motor_l_b = 0;
        }
        /*} else { //上限のリミットスイッチが押されている場合
            motor_l_f = 0;
            motor_l_b = 0;
        }*/

    } else {//入力duty比が負の場合
        if(pre_out_l-duty >accel_max)duty=pre_out_l-accel_max;
        //if(switch_min == 0) { //下限のリミットスイッチが押されていない場合
        if( fabs(duty) < dutylimit) { //制限値内
            motor_l_f = 0;
            motor_l_b = fabs(duty);
        } else { //制限値超
            motor_l_f = 0;
            motor_l_b = dutylimit;
        }
        /*} else { //下限のリミットスイッチが押されている場合
            motor_l_f = 0;
            motor_l_b = 0;
        }*/
    }
    pre_out_l=duty;
}


double pile_r=0,distance_r_old=0,pile_l=0,distance_l_old=0;

//PID制御
void pid_r(double target,double error) //(移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
{
    double Kp_r,Ki_r,Kd_r;
    if(turn_mode){
        Kp_r=Kp_r_t;
        Ki_r=Ki_r_t;
        Kd_r=Kd_r_t;
    }else{
        Kp_r=Kp_r_n;
        Ki_r=Ki_r_n;
        Kd_r=Kd_r_n;
    }
    move_r=1;
    double deviation = target - posi_r;
    //printf("%f,%f\r\n",posi_r,deviation);
    pile_r += deviation;
    out_r(deviation * Kp_r - (posi_r - distance_r_old) / pid_time * Kd_r + pile_r *  Ki_r * pid_time);
    distance_r_old = posi_r;
    if (fabs(deviation) < error) {
        out_r(0);
        pile_r=0;
        move_r=0;
        reach_target_r=1;
    }
}

void pid_l(double target,double error)   //(移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
{
    double Kp_l,Ki_l,Kd_l;
    if(turn_mode){
        Kp_l=Kp_l_t;
        Ki_l=Ki_l_t;
        Kd_l=Kd_l_t;
    }else{
        Kp_l=Kp_l_n;
        Ki_l=Ki_l_n;
        Kd_l=Kd_l_n;
    }
    move_l=1;
    double deviation = target - posi_l;
    pile_l += deviation;
    out_l(deviation * Kp_l - (posi_l - distance_l_old) / pid_time * Kd_l + pile_l *  Ki_l * pid_time);
    distance_l_old = posi_l;
    if (fabs(deviation) < error) {
        out_l(0);
        pile_l=0;
        move_l=0;
        reach_target_l=1;
    }
}


//脚を上げる
void up(int i)   // j=1 待ち時間あり
{
    air[i]=0;
    //if(j)wait(leg_wait_time);
}

//脚を降ろす
void down(int i)
{
    air[i]=1;
    //if(j)wait(leg_wait_time);
}



//移動
void move (double target_r,double target_l)
{
    //pc.printf("x  target=%f , posi=%f\r\n",x,linear_posi);
    int upleg_r=0,upleg_l=0;
    if(fabs(target_r)>target_max)target_r=target_max;
    else if(fabs(target_r)<target_min)target_r=target_min;
    if(target_r>0) {
        if(posi_r<target_r)upleg_r=r_o_s;
        else upleg_r=r_i_s;
    } else {
        if(posi_r>-target_r)upleg_r=r_o_s;
        else upleg_r=r_i_s;
    }

    if(fabs(target_l)>target_max)target_l=target_max;
    else if(fabs(target_l)<target_min)target_l=target_min;
    if(target_l>0) {
        if(posi_l>target_l)upleg_l=l_o_s;
        else upleg_l=l_i_s;
    } else {
        if(posi_l<-target_l)upleg_l=l_o_s;
        else upleg_l=l_i_s;
    }
    up(upleg_r);
    up(upleg_l);

    wait(leg_wait_time);
    target_r_out=fabs(target_r);
    target_l_out=fabs(target_l);
    reach_target_r=0;
    reach_target_l=0;
    move_r=1;
    move_l=1;
    //printf("%d\r\n",reach_target_r);
    /*while(reach_target_r==0 || reach_target_l==0){
        printf("%d\r\n",reach_target_r);
    };*/
    while(reach_target_r==0 || reach_target_l==0){
        wait(0.01);
        //printf("%d\r\n",reach_target_r);
    }
    down(upleg_r);
    down(upleg_l);
    wait(leg_wait_time);
    reach_target_r=0;
    reach_target_l=0;
}



void straight(double distance)       //とりあえず左右で位置をずらさない前提
{
    bool reach=0;
    while(1) {
        if(distance>0) {
            if(target_max-posi_r > posi_r-target_min) {
                if(target_max-posi_r > distance) {
                    move(posi_r+distance,posi_r+distance);
                    reach=1;
                } else {
                    distance-=target_max-posi_r;
                    move(target_max,target_max);
                }
            } else {
                if(posi_r-target_min > distance) {
                    move(posi_r-distance,posi_r-distance);
                    reach=1;
                } else {
                    distance-=posi_r-target_min;
                    move(target_min,target_min);
                }
            }
        } else {
            if(target_max-posi_r > posi_r-target_min) {
                if(target_max-posi_r > -distance) {
                    move(-posi_r+distance,-posi_r+distance);
                    reach=1;
                } else {
                    distance+=target_max-posi_r;
                    move(-target_max,-target_max);
                }
            } else {
                if(posi_r-target_min> -distance) {
                    move(-posi_r-distance,-posi_r-distance);
                    reach=1;
                } else {
                    distance+=posi_r-target_min;
                    move(-target_min,-target_min);
                }
            }
        }
        if(reach==1)break;
    }
}

void turn(double distance)                  //理論計算の元気が出ないから脚の動かす量のみ  distance>0:左回転
{
    while(1) {
        if(distance>0) {
            if(target_max-posi_r > posi_r-target_min) {
                if(target_max-posi_r > distance) {
                    move(posi_r+distance,-(posi_r+distance));
                    break;
                } else {
                    distance-=target_max-posi_r;
                    move(target_max,-target_max);
                }
            } else {
                if(posi_r-target_min > distance) {
                    move(-(posi_r-distance),posi_r-distance);
                    break;
                } else {
                    distance-=posi_r-target_min;
                    move(-target_min,target_min);
                }
            }
        } else {
            if(target_max-posi_r > posi_r-target_min) {
                if(target_max-posi_r > -distance) {
                    move(-(-posi_r+distance),-posi_r+distance);
                    break;
                } else {
                    distance+=target_max-posi_r;
                    move(target_max,-target_max);
                }
            } else {
                if(posi_r-target_min> -distance) {
                    move(-posi_r-distance,-(-posi_r-distance));
                    break;
                } else {
                    distance+=posi_r-target_min;
                    move(-target_min,target_min);
                }
            }
        }
    }
}


double a=140,b=70;
void turn_2(double target){
    turn_mode=1;
    if(target-degree0>0){
        move(a,a);
        while(1){
            move(a+b,-(a+b));
            if(fabs(target-degree0)<5)break;
            move(a,-a);
            if(fabs(target-degree0)<5)break;
            //printf("%f\r\n",degree0);
        }
    }else{    
        move(a,a);
        while(1){
            move(-(a+b),a+b);
            if(fabs(target-degree0)<5)break;
            move(-a,a);
            if(fabs(target-degree0)<5)break;
            //printf("%f\r\n",degree0);
        }
    }
    turn_mode=0;
}

void sanddune()
{
    move(160,160);
    down(long_r);
    down(long_l);
    wait(1);

    up(long_l);
    wait(leg_wait_time_l);
    target_r_out=50;
    //target_l_out=50;
    target_l_out=270;
    reach_target_r=0;
    reach_target_l=0;
    move_r=1;
    move_l=1;
    while(reach_target_r==0 || reach_target_l==0){
        wait(0.01);
    }
    down(long_l);
    //down(l_o_s);
    //down(r_i_s);
    wait(leg_wait_time_l);
    reach_target_r=0;
    reach_target_l=0;

    up(long_r);
    wait(leg_wait_time_l);
    target_r_out=250;
    //target_l_out=250;
    target_l_out=70;
    reach_target_r=0;
    reach_target_l=0;
    move_r=1;
    move_l=1;
    while(reach_target_r==0 || reach_target_l==0){
        wait(0.01);
    }
    down(long_r);
    wait(leg_wait_time_l);
    reach_target_r=0;
    reach_target_l=0;

    up(long_l);
    //up(l_o_s);
    //up(r_i_s);
    wait(leg_wait_time_l);
    target_r_out=160;
    target_l_out=160;
    reach_target_r=0;
    reach_target_l=0;
    move_r=1;
    move_l=1;
    while(reach_target_r==0 || reach_target_l==0){
        wait(0.01);
    }
    down(long_l);
    wait(leg_wait_time_l);
    reach_target_r=0;
    reach_target_l=0;

    up(long_r);
    wait(leg_wait_time_l);
    target_r_out=220;
    //target_l_out=220;
    target_l_out=100;
    reach_target_r=0;
    reach_target_l=0;
    move_r=1;
    move_l=1;
    while(reach_target_r==0 || reach_target_l==0){
        wait(0.01);
    }
    down(long_r);
    //down(r_o_s);
    //down(l_i_s);
    wait(leg_wait_time_l);
    reach_target_r=0;
    reach_target_l=0;
    
    up(long_l);
    wait(leg_wait_time_l);
    target_r_out=80;
    //target_l_out=80;
    target_l_out=240;
    reach_target_r=0;
    reach_target_l=0;
    move_r=1;
    move_l=1;
    while(reach_target_r==0 || reach_target_l==0){
        wait(0.01);
    }
    down(long_l);
    wait(leg_wait_time_l);
    reach_target_r=0;
    reach_target_l=0;
    
    //up(r_o_s);
    //up(l_i_s);
    up(long_r);
    wait(leg_wait_time_l);
    target_r_out=0;
    target_l_out=0;
    reach_target_r=160;
    reach_target_l=160;
    move_r=1;
    move_l=1;
    while(reach_target_r==0 || reach_target_l==0){
        wait(0.01);
    }
    down(long_r);
    wait(leg_wait_time_l);
    reach_target_r=0;
    reach_target_l=0;
}

void tussock()
{
    for(int i=0; i<3; i++) {
        up(long_l);
        wait(leg_wait_time);
        target_r_out=360;
        target_l_out=360;
        while(reach_target_r==0 || reach_target_l==0);
        down(long_l);
        wait(leg_wait_time);
        reach_target_r=0;
        reach_target_l=0;

        up(long_r);
        wait(leg_wait_time);
        target_r_out=10;
        target_l_out=10;
        while(reach_target_r==0 || reach_target_l==0);
        down(long_r);
        wait(leg_wait_time);
        reach_target_r=0;
        reach_target_l=0;
    }
}

/*int k=0;
//脚位置の初期化
void reset()
{
    up_x();
    while(switch_min == 0) {
        out(-0.2);
        k++;
        if(k>5000) {
            servo.set_degree(0,servo_iniposi);
            k=0;
        }
    }
    //pc.printf("%d\r\n",ec.getCount());
    ec.reset();
    out(0);
    down_x();
    //pc.printf("%d\r\n",ec.getCount());
}*/





/*void straight(double target)
{
    while(1) {
        double xleg_posi=1.0*ecgear*Pi*ec.getCount()/resolution;    //機体内のX脚位置
        double dist=target-linear_posi;
        pc.printf("leg=%f,dist=%f,linear_posi=%f\r\n",xleg_posi,dist,linear_posi);
        if(fabs(dist)<10) {
            break;
        } else if((dist>0 && dist<=(target_max-xleg_posi)) || (dist<0 && dist>=(target_min-xleg_posi))) {
            move_x(xleg_posi+dist,10);
        } else if(dist<0) {
            pc.printf("distance < 0");
            break;
        } else if(dist<=(target_max-target_min-10)) {
            if(dist<=(rail_len*0.5-target_min-10)) {
                move_y(dist+xleg_posi-rail_len*0.5,10);
            } else {
                move_y(target_min,10);
            }
        } else {
            if(xleg_posi<(rail_len*0.5))move_x(target_max,10);
            else move_y(target_min,10);
        }
    }
}*/

void wait_MR1()
{
    while(switch_x==1) {

    }
    //while(switch_hand==0);
    //air_hand=1;
    wait(1);

}