File content as of revision 1:3ae63be5592b:

//2018/12/15 腰回転をシリンダに変更
#include "mbed.h"
#include "EC.h"
#include "KondoServo.h"
#include "hcsr04.h"

Serial pc(USBTX,USBRX);
Timer timer;

//パラメタ
double dutylimit = 0.4; // duty出力の上限値
int resolution = 1000; //エンコーダの分解能×逓倍
int ecgear = 35; //エンコーダのギアの歯数
double Kp = 0.342000; //P値
double Ki = 0.013062; //I値
double Kd = 0.003266; //D値
double leg_wait_time = 0.5; //脚の伸縮にかかる時間
double turn_wait_time = 0.7; //旋回にかかる時間
double target_max = 300; //直動機構の目標座標の上限（オーバーシュートを考慮して短めに設定）
double target_min = 20; //直動機構の目標座標の下限（オーバーシュートを考慮して短めに設定）
double rail_len = 710;
double turn_error=10;   //旋回の際のY脚のずれの許容誤差
double servo_iniposi=105;  //サーボ初期角度

double target_plot[][2]={      //(mode,target)     mode 0→standby 1→straight 2→turn 3→sand dune 4→tussock 5→wait_shagai 6→mountain~finish
    {0,0},
    {1,3450},
    {2,-45},
    {3,0},
    {2,-45},
    {1,1030},
    {2,90},
    {1,2390},
    {2,90},
    {5,0},
    {6,0}
};



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

//変数
double linear_posi=0;       //直線移動での距離 １プロットごとにリセットする?
int target_num=0;           //現在の目標プロット



//ピン宣言
PwmOut motor_f(p21); //モータ正転
PwmOut motor_b(p22); //モータ逆転
DigitalIn switch_max(p9); //直動機構の上限のリミットスイッチ
DigitalIn switch_min(p8); //直動機構の下限のリミットスイッチ
DigitalIn switch_x(p7); //X脚の接地を判定するスイッチ（今後実装予定）
DigitalIn switch_y(p5); //Y脚の接地を判定するスイッチ（今後実装予定）
DigitalIn switch_hand(p6); //ゲルゲ回収アームのスイッチ
DigitalOut air_x(p18); //X脚の伸縮
DigitalOut air_y(p17); //Y脚の伸縮
DigitalOut air_x_sub(p14); //X脚の先端の伸縮（今後実装予定）
DigitalOut air_turn(p16); //Y脚の先端の伸縮（今後実装予定）
DigitalOut air_hand(p19); //ゲルゲ回収アームのハンド部開閉
DigitalOut air_trig(p20); //ゲルゲ持ち上げ機構のロック解除
Ec ec(p24,p23,NC,resolution,0.01); //直動機構のエンコーダ
KondoServo servo(p28,p27,1,115200); //旋回用のシリアルサーボ
HCSR04 sensor(p30, p29);



void setup();
void out(double);//モーターの出力
void move(double,double); //PID   (移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
void up_x();
void down_x();
void up_y();
void down_y();
void move_x (double,double);//X脚を移動
void move_y (double,double);//Y脚を移動
void reset();    //脚位置の初期化
void turn(double);   //旋回動作
void straight(double);   //目的地まで直進
void wait_MR1();


int main() {
   setup();
   while(switch_x==1){}
    reset();
   while(1) {
        switch((int)(target_plot[target_num][0])){
            case 0:
                wait_MR1();
                break;
                
            case 1:
                straight(target_plot[target_num][1]);
                pc.printf("%f\r\n",linear_posi);
                //while(1);
                break;
                
            case 2:
                turn(target_plot[target_num][1]);
                break;
                
            case 3:
                move_x(target_max,10);
                move_y(target_min,10);
                move_x(target_max,10);
                move_y(target_min,10);
                move_x(rail_len*0.5,10);
                
            default:
                break;
        }
        target_num++;
        linear_posi=0;
    }
}

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



//初期設定
void setup()
{
    motor_f.period_us(100);
    motor_b.period_us(100);
    switch_max.mode(PullUp);
    switch_min.mode(PullUp);
    switch_x.mode(PullUp);
    switch_y.mode(PullUp);
    switch_hand.mode(PullUp);
    air_x=0;
    air_y=0;
    air_x_sub=0;
    air_turn=0;
    air_hand=0;
    air_trig=0;
    servo.setSpeed(0,127);
    wait_us(1000);
    servo.init();
    //servo.set_degree(0,servo_iniposi);
}
//モーターの出力
void out (double duty)
{
    if(duty > 0) { //入力duty比が正の場合
      if(switch_max == 0){ //上限のリミットスイッチが押されていない場合
        if( fabs( duty ) < dutylimit ) { //制限値内
            motor_f = fabs(duty);
            motor_b = 0;
        } else { //制限値超
            motor_f = dutylimit;
            motor_b = 0;
        }
      } else { //上限のリミットスイッチが押されている場合
        motor_f = 0;
        motor_b = 0;
        //pc.printf("%f\r\n",1.0*ecgear*Pi*ec.getCount()/resolution);
      }

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

//PID制御
void move(double target,double error) //(移動先の座標(機体内)[mm],許容誤差の絶対値[mm])
{
    double pile=0,deviation,distance,distance_old=0;
    int i=0;
    timer.start();
    while(1) {
        distance = 1.0*ecgear*Pi*ec.getCount()/resolution;
        deviation = target - distance;
        pile += deviation;
        double period = timer.read();
        out(deviation * Kp - (distance - distance_old) / period * Kd + pile *  Ki * period);
        timer.reset();
        distance_old = distance;
        if (fabs(deviation) < error) {
            i++;
            if (i==3000) {
                out(0);
                timer.stop();
                //pc.printf("%f\r\n",1.0*ecgear*Pi*ec.getCount()/resolution);
                break;
            }
        } else {
            i=0;
        }
    }
}

//X脚を上げる
void up_x()
{
    air_x =1;
    wait(leg_wait_time);
}

//X脚を降ろす
void down_x()
{
    air_x =0;
    wait(leg_wait_time);
}

//Y脚を上げる
void up_y()
{
    air_y =1;
    wait(leg_wait_time);
}

//Y脚を降ろす
void down_y()
{
    air_y =0;
    wait(leg_wait_time);
}
//X脚を移動
void move_x (double x,double error)
{
    //pc.printf("x  target=%f , posi=%f\r\n",x,linear_posi);
    double pre_posi=1.0*ecgear*Pi*ec.getCount()/resolution;
    up_x();
    if(x>target_max)x=target_max;
    else if(x<target_min)x=target_min;
    move(x,error);
    down_x();
    linear_posi+=1.0*ecgear*Pi*ec.getCount()/resolution-pre_posi;
}

//Y脚を移動
void move_y (double y,double error)
{
    //pc.printf("y  target=%f , posi=%f\r\n",y,linear_posi);
    up_y();
    if(y>target_max)y=target_max;
    else if(y<target_min)y=target_min;
    move(y,error);
    down_y();
}

int k=0;
//脚位置の初期化
void reset()
{
    //servo.set_degree(0,servo_iniposi);
    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 turn(double degree)
{
    if(fabs(1.0*ecgear*Pi*ec.getCount()/resolution-rail_len*0.5)>turn_error){
        move_y(rail_len*0.5,turn_error);
    }
   /* bool turn_finish=0;
    while(turn_finish==0){
        double degree_out=0;
        if(fabs(degree)<=45){
            degree_out=degree;
            turn_finish=1;
        }else if(degree>45){
            degree_out=45;
            degree-=45;
        }else{
            degree_out=-45;
            degree+=45;
        }*///!!削除
        if(degree == 45){
          up_y();
          air_turn=1;
          wait(turn_wait_time);
          down_y();
          up_x();
          air_turn=0;
          wait(turn_wait_time);
          down_x();
        } else if(degree == 90){
          up_y();
          air_turn=1;
          wait(turn_wait_time);
          down_y();
          up_x();
          air_turn=0;
          wait(turn_wait_time);
          down_x();
          
          up_y();
          air_turn=1;
          wait(turn_wait_time);
          down_y();
          up_x();
          air_turn=0;
          wait(turn_wait_time);
          down_x();
        }
      if(degree == -45){
        up_x();
        air_turn=1;
        wait(turn_wait_time);
        down_x();
        up_y();
        air_turn=0;
        wait(turn_wait_time);
        down_y();
      }else if(degree == -90){
        up_x();
        air_turn=1;
        wait(turn_wait_time);
        down_x();
        up_y();
        air_turn=0;
        wait(turn_wait_time);
        down_y();
        
        up_x();
        air_turn=1;
        wait(turn_wait_time);
        down_x();
        up_y();
        air_turn=0;
        wait(turn_wait_time);
        down_y();
      }else{}
}



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