File content as of revision 4:e66bd933bafa:

#include "mbed.h"
#include "SerialServo.h"

//Serial pc(USBTX,USBRX);//デバック用シリアル
Ticker update;//現在地座標の更新割り込み

SerialServo servo[4] = {SerialServo(PC_4,PC_5),//serial1
                        SerialServo(PA_2,PA_3),//serial5
                        //SerialServo(PC_10,PC_11),//serial5
                        SerialServo(PB_10,PB_11),//serial3
                        SerialServo(PC_12,PD_2)};//serial4


PwmOut ropeTouch(PA_13);//タッチセンサ位置切り替え
DigitalIn sw(PC_1);//平地と坂の切り替え
DigitalIn start(PA_0);//gerege受け取り時の合図
DigitalIn touch(PA_1);//糸探知用タッチセンサ
DigitalIn retry(PC_13);
DigitalIn color(PC_0);//赤、青モード切替

//---非接触合図(超音波センサー)---
DigitalOut trig(PA_4);
InterruptIn echo(PB_0);
Timer timer;
//DigitalOut Led(PA_5);//スタート合図

//-----------------------------
float initLack1;
float initLack2;
int flatCurve1;
int flatCurve2;
float slopeLack1;
float slopeLack2;
int ropeCurve1;
int ropeCurve2;
int ropeClab1;
int ropeClab2;
int clabCount;
//-----------------------------

const float PI = 3.1415926;
bool modeFlag = true;
bool ropeFlag = false;
bool ropeSign = false;
int ropeCount = 0;

//---各足の長さ----
const float L1 = 76;
const float L2 = 168;
const float L3 = 252;

//移動速度
float moveSpeed = 1;
//現在の座標
float currentSite[4][3];
//次の座標
float nextSite[4][3];
//値更新用スピード
float tempSpeed[4][3];
//現在の角度
float currentAngle[4][3];
//現在の周波数
int currentPulse[4][3];

float fmap(float x, float in_min, float in_max, float out_min, float out_max){
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

//----------色に合わせて初期化-------------
void initRed(){//左側
    initLack1 = 7600;
    initLack2 = 3500;
    flatCurve1 = 0;
    flatCurve2 = 15;
    slopeLack1 = 9700;//9700
    slopeLack2 = 3800;
    ropeCurve1 = -20;
    ropeCurve2 = -50;
    ropeClab1 = 70;
    ropeClab2 = 1;
    clabCount = 2;
}

void initBlue(){//右側
    initLack1 = 7600;
    initLack2 = 11500;
    flatCurve1 = -7;
    flatCurve2 = -17;
    slopeLack1 = 9700;
    slopeLack2 = 11100;
    ropeCurve1 = 15;
    ropeCurve2 = 50;
    ropeClab1 = -40;
    ropeClab2 = 0;
    clabCount = 2;
}

//--------------------------------------
//----------超音波センサー用関数------------
float Distance = 0;
void pulseRise(){
    timer.start();
}
void pulseFall(){
    timer.stop();
    Distance = timer.read_us();
    timer.reset();
}
//--------------------------------------
//座標指定
void setSite(int leg, float x, float y, float z){
    float length[3] = {0.0,0.0,0.0};
    length[0] = x - currentSite[leg][0];
    length[1] = y - currentSite[leg][1];
    length[2] = z - currentSite[leg][2];
    float L = sqrt(pow(length[0],2)+pow(length[1],2)+pow(length[2],2));
    tempSpeed[leg][0] = (length[0]/L)*moveSpeed;
    tempSpeed[leg][1] = (length[1]/L)*moveSpeed;
    tempSpeed[leg][2] = (length[2]/L)*moveSpeed;
    nextSite[leg][0] = x;
    nextSite[leg][1] = y;
    nextSite[leg][2] = z;
}

void toAngle(int leg){
    float x = currentSite[leg][0];
    float y = currentSite[leg][1];
    float z = currentSite[leg][2];
    float L = sqrt((x*x)+(y*y));
    float w = L - L1;
    float v = sqrt((w*w)+(z*z));
    float alpha = atan2(z,w) - acos(((v*v)+(L2*L2)-(L3*L3))/(2*L2*v));
    float beta = PI - acos(((L2*L2) + (L3*L3) - (v*v))/(2*L2*L3));
    float gamma = atan2(y, x);
    currentAngle[leg][0] = gamma = (gamma / PI * 180);
    currentAngle[leg][1] = alpha = (alpha / PI * 180);
    currentAngle[leg][2] = beta = (beta / PI * 180);
}


void toPulse(){
    //右前足
    currentPulse[0][0] = int(fmap(currentAngle[0][0],-135,135,11500,3500));
    currentPulse[0][1] = int(fmap(currentAngle[0][1],-135,135,11500,3500)+100);//+200
    currentPulse[0][2] = int(fmap(currentAngle[0][2],0,270,3500,11500));//0
    //左前足
    currentPulse[1][0] = int(fmap(currentAngle[1][0],-135,135,3500,11500)-100);
    currentPulse[1][1] = int(fmap(currentAngle[1][1],-135,135,11500,3500)-100);//-200
    currentPulse[1][2] = int(fmap(currentAngle[1][2],0,270,3500,11500)-400);//-350
    //右後足
    currentPulse[2][0] = int(fmap(currentAngle[2][0],-135,135,11500,3500)+500);
    currentPulse[2][1] = int(fmap(currentAngle[2][1],-135,135,11500,3500));//0
    currentPulse[2][2] = int(fmap(currentAngle[2][2],0,270,3500,11500)-100);//0
    //左後足
    currentPulse[3][0] = int(fmap(currentAngle[3][0],-135,135,3500,11500)+100);
    currentPulse[3][1] = int(fmap(currentAngle[3][1],-135,135,11500,3500));//0
    currentPulse[3][2] = int(fmap(currentAngle[3][2],0,270,3500,11500)-100);//0
    
    for(int i=0;i<3;i++){
        for(int j=0;j<4;j++){
            servo[j].move(i+1,int(currentPulse[j][i]));
            wait_us(100);
        }
        wait_us(400);
    }
}

void siteUpdate(){
    if(ropeSign == true){
        if(touch.read() == 0){
            ropeCount++;
        }else{
            ropeCount--;
        }
        if(ropeCount >= 25){
            ropeFlag = true;
            ropeCount = 0;
        }else if(ropeCount <= 0){
            ropeFlag = false;
            ropeCount = 0;
        }
    }
    for(int i=0;i<4;i++){
        for(int j=0;j<3;j++){
            float siteLength = abs(currentSite[i][j] - nextSite[i][j]);
            if( siteLength >= abs(tempSpeed[i][j]))
                currentSite[i][j] += tempSpeed[i][j];
            else
                currentSite[i][j] = nextSite[i][j];
        }
        toAngle(i);
    }
    toPulse();
}

void motionWait(int leg){
    bool flag = false;
    while(flag == false){
        wait_us(1);
        if(currentSite[leg][0] == nextSite[leg][0]){
            if(currentSite[leg][1] == nextSite[leg][1]){
                if(currentSite[leg][2] == nextSite[leg][2]){
                    flag = true;
                }
            }
        }
    }
}

//すべての足が動き終わるまで待機
void allMotionWait(){
    for(int i=0;i<4;i++){
        motionWait(i);
    }
}
//------------------歩行モーション------------------------------------
//静歩行

//座位
void standDown(float *pos,float *speed){
    moveSpeed = speed[0];
    setSite(0, pos[0], pos[1], pos[3]);
    setSite(1, pos[0], pos[1], pos[3]);
    setSite(2, pos[0], pos[2], pos[4]);
    setSite(3, pos[0], pos[2], pos[4]);
    allMotionWait();
}
//直立姿勢
void standUp(float *pos,float *speed){
    moveSpeed = speed[0];
    setSite(0, pos[0], pos[1], pos[3]);
    setSite(1, pos[0], pos[1], pos[3]);
    setSite(2, pos[0], pos[2], pos[4]);
    setSite(3, pos[0], pos[2], pos[4]);
    allMotionWait();
}
void Turn(unsigned int count,float *pos, float *speed,bool direction){
    moveSpeed = speed[0];
    float stepRY = 0;
    float stepLY = 0;
    if(direction == 0){
        stepRY = pos[5];
    }else{
        stepLY = pos[5];
    }
    while(count-- > 0){
        //0,3
        setSite(0, pos[0], pos[1] + stepRY, pos[3]-pos[6]);
        setSite(1, pos[0], pos[1], pos[3]);
        setSite(2, pos[0], pos[2], pos[4]);
        setSite(3, pos[0], pos[2] + stepLY, pos[4]-pos[7]);
        allMotionWait();
        setSite(0, pos[0], pos[1] + stepRY, pos[3]);
        setSite(3, pos[0], pos[2] + stepLY, pos[4]);
        allMotionWait();
        //1,2
        setSite(0, pos[0], pos[1], pos[3]);
        setSite(1, pos[0], pos[1] + stepLY, pos[3]-pos[6]);
        setSite(2, pos[0], pos[2] + stepRY, pos[4]-pos[7]);
        setSite(3, pos[0], pos[2], pos[4]);
        allMotionWait();
        setSite(1, pos[0], pos[1] + stepLY, pos[3]);
        setSite(2, pos[0], pos[2] + stepRY, pos[4]);
        allMotionWait();
    }
}
//トロット歩容
void forwardTrot(unsigned int count, float *pos, float *speed, float turn, bool alta = 1){
    float stepRY = pos[5];
    float stepLY = pos[5];
    moveSpeed = speed[0];
    if(turn > 0){
        stepRY += turn;
        stepLY -= turn;
    }else if(turn < 0){
        stepRY += turn;
        stepLY -= turn;
    }
    while(count-- > 0){
        if(alta == 1){
            //0,3
            setSite(0, pos[0], pos[1] + stepRY, pos[3]-pos[6]);
            setSite(1, pos[0], pos[1], pos[3]);
            setSite(2, pos[0], pos[2], pos[4]);
            setSite(3, pos[0], pos[2] + stepLY, pos[4]-pos[7]);
            allMotionWait();
            setSite(0, pos[0], pos[1] + stepRY, pos[3]);
            setSite(3, pos[0], pos[2] + stepLY, pos[4]);
            allMotionWait();
            //1,2
            setSite(0, pos[0], pos[1], pos[3]);
            setSite(1, pos[0], pos[1] + stepLY, pos[3]-pos[6]);
            setSite(2, pos[0], pos[2] + stepRY, pos[4]-pos[7]);
            setSite(3, pos[0], pos[2], pos[4]);
            allMotionWait();
            setSite(1, pos[0], pos[1] + stepLY, pos[3]);
            setSite(2, pos[0], pos[2] + stepRY, pos[4]);
            allMotionWait();
        }else{
            //1,2
            setSite(0, pos[0], pos[1], pos[3]);
            setSite(1, pos[0], pos[1] + stepLY, pos[3]-pos[6]);
            setSite(2, pos[0], pos[2] + stepRY, pos[4]-pos[7]);
            setSite(3, pos[0], pos[2], pos[4]);
            allMotionWait();
            setSite(1, pos[0], pos[1] + stepLY, pos[3]);
            setSite(2, pos[0], pos[2] + stepRY, pos[4]);
            allMotionWait();
            //0,3
            setSite(0, pos[0], pos[1] + stepRY, pos[3]-pos[6]);
            setSite(1, pos[0], pos[1], pos[3]);
            setSite(2, pos[0], pos[2], pos[4]);
            setSite(3, pos[0], pos[2] + stepLY, pos[4]-pos[7]);
            allMotionWait();
            setSite(0, pos[0], pos[1] + stepRY, pos[3]);
            setSite(3, pos[0], pos[2] + stepLY, pos[4]);
            allMotionWait();
        }
    }
}
bool forwardTrot2(float *pos, float *speed, float turn){
    float stepRY = pos[5];
    float stepLY = pos[5];
    bool alta = 0;
    moveSpeed = speed[0];
    if(turn > 0){
        stepRY += turn;
        stepLY -= turn;
    }else if(turn < 0){
        stepRY += turn;
        stepLY -= turn;
    }
    while(1){
        if(alta == 0){
            //0,3
            setSite(0, pos[0], pos[1] + stepRY, pos[3]-pos[6]);
            setSite(1, pos[0], pos[1], pos[3]);
            setSite(2, pos[0], pos[2], pos[4]);
            setSite(3, pos[0], pos[2] + stepLY, pos[4]-pos[7]);
            allMotionWait();
            setSite(0, pos[0], pos[1] + stepRY, pos[3]);
            setSite(3, pos[0], pos[2] + stepLY, pos[4]);
            allMotionWait();
        }else{
            //1,2
            setSite(0, pos[0], pos[1], pos[3]);
            setSite(1, pos[0], pos[1] + stepLY, pos[3]-pos[6]);
            setSite(2, pos[0], pos[2] + stepRY, pos[4]-pos[7]);
            setSite(3, pos[0], pos[2], pos[4]);
            allMotionWait();
            setSite(1, pos[0], pos[1] + stepLY, pos[3]);
            setSite(2, pos[0], pos[2] + stepRY, pos[4]);
            allMotionWait();
        }
        if(ropeFlag == true){
            return alta;
        }
        alta = !alta;
    }
}

void clabWalk(unsigned int count, float *pos, float *speed, float turn){
    moveSpeed = speed[0];
    while(count-- > 0){
        //0,3
        setSite(0, pos[0]+turn, pos[1] + pos[5], pos[3]-pos[6]);
        setSite(1, pos[0], pos[1], pos[3]);
        setSite(2, pos[0], pos[2], pos[4]);
        setSite(3, pos[0]-turn, pos[2] + pos[5], pos[4]-pos[7]);
        allMotionWait();
        setSite(0, pos[0]+turn, pos[1] + pos[5], pos[3]);
        setSite(3, pos[0]-turn, pos[2] + pos[5], pos[4]);
        allMotionWait();
        //1,2
        setSite(0, pos[0], pos[1], pos[3]);
        setSite(1, pos[0]-turn, pos[1] + pos[5], pos[3]-pos[6]);
        setSite(2, pos[0]+turn, pos[2] + pos[5], pos[4]-pos[7]);
        setSite(3, pos[0], pos[2], pos[4]);
        allMotionWait();
        setSite(1, pos[0]-turn, pos[1] + pos[5], pos[3]);
        setSite(2, pos[0]+turn, pos[2] + pos[5], pos[4]);
        allMotionWait();  
    }
}

void backTrot(unsigned int count,float *pos,float *speed, float turn){
    float stepRY = pos[5];
    float stepLY = pos[5];
    if(turn > 0){
        stepRY += turn;
        stepLY -= turn;
    }else if(turn < 0){
        stepRY += turn;
        stepLY -= turn;
    }
    moveSpeed = speed[0];
    while(count-- > 0){
        //0,3
        setSite(0, pos[0], pos[1] + stepRY, pos[3]-pos[6]);
        setSite(1, pos[0], pos[1], pos[3]);
        setSite(2, pos[0], pos[2], pos[4]);
        setSite(3, pos[0], pos[2] + stepLY, pos[4]-pos[7]);
        allMotionWait();
        setSite(0, pos[0], pos[1] + stepRY, pos[3]);
        setSite(3, pos[0], pos[2] + stepLY, pos[4]);
        allMotionWait();
        //1,2
        setSite(0, pos[0], pos[1], pos[3]);
        setSite(1, pos[0], pos[1] + stepLY, pos[3]-pos[6]);
        setSite(2, pos[0], pos[2] + stepRY, pos[4]-pos[7]);
        setSite(3, pos[0], pos[2], pos[4]);
        allMotionWait();
        setSite(1, pos[0], pos[1] + stepLY, pos[3]);
        setSite(2, pos[0], pos[2] + stepRY, pos[4]);
        allMotionWait();  
    }
}
void stepOver(){
    moveSpeed = 8;//-30
    setSite(0, 200, 20, 250);
    setSite(1, 200, 155, 250);
    setSite(2, 200, -25, 250);
    setSite(3, 200, -160, 250);
    allMotionWait();
    
    setSite(0, 200, -20, 30);
    allMotionWait();
    setSite(0, 200, 200, 30);
    allMotionWait();
    setSite(0, 200, 200, 140);
    allMotionWait();
        
    setSite(1, 200, 30, 30);
    allMotionWait();
    setSite(1, 200, 270, 30);
    allMotionWait();
    setSite(1, 200, 270, 140);
    allMotionWait();
    
    float stepPosTest[8] = {200,10,-150,110,250,70,60,80};
    float stepSpeTest[2] = {7,5};
    forwardTrot(2,stepPosTest,stepSpeTest,0);
    
    moveSpeed = 8;
    setSite(2, 200, -200, 30);
    allMotionWait();
    setSite(2, 200, 40, 30);
    allMotionWait();
    setSite(2, 200, 40, 140);
    allMotionWait();
    
    setSite(0, 200, 105, 30);
    allMotionWait();
    setSite(0, 200, 280, 30);
    allMotionWait();
    setSite(0, 200, 280, 240);
    allMotionWait();
    
    moveSpeed = 7;
    setSite(0, 200, 180, 230);//80
    setSite(1, 200, -20, 140);
    setSite(2, 200, -70, 140);//-80
    setSite(3, 200, -240, 240);
    allMotionWait();
    
    moveSpeed = 8;
    setSite(3, 200, -250, 30);
    allMotionWait();
    setSite(3, 200, -150, 30);
    allMotionWait();
    setSite(3, 200, -150, 140);//-150
    allMotionWait();
    
    setSite(1, 200, -10, 30);
    allMotionWait();
    setSite(1, 200, 250, 30);
    allMotionWait();
    setSite(1, 200, 250, 240);//150
    allMotionWait();
    
    float stepPosTest2[8] = {200,30,-150,250,140,100,60,120};
    float stepSpeTest2[2] = {7,6};
    forwardTrot(1,stepPosTest2,stepSpeTest2,0);
    
    float stepPosTest3[8] = {200,30,-120,200,180,100,60,140};
    float stepSpeTest3[2] = {7,6};
    forwardTrot(1,stepPosTest3,stepSpeTest3,0);
    
        
}

bool ropeOver(){
    float levelPosTest[8] = {200,30,-160,160,170,145,60,70};
    float levelSpeTest[2] = {7.5,8};
    
    ropeFlag = false;
    ropeSign = true;
    bool alta;
    alta = forwardTrot2(levelPosTest,levelSpeTest,0);
    ropeTouch.pulsewidth_us(2400);
    ropeSign = false;
    moveSpeed = 6;
    int f1;
    int f2;
    int b1;
    int b2;
    if(alta == true){
        f1 = 0;
        f2 = 1;
        b1 = 3;
        b2 = 2;
    }else{
        f1 = 1;
        f2 = 0;
        b1 = 2;
        b2 = 3;
    }
    
    setSite(f1, 200, 60, 180);
    setSite(f2, 200, 160, 180);
    
    setSite(b2, 200, -10, 180);
    setSite(b1, 200, -110, 180);
    allMotionWait();
        
    setSite(f1, 200, 0, 30);
    setSite(f2, 200, 160, 180);
    setSite(b2, 200, -10, 180);
    setSite(b1, 200, -110, 30);
    allMotionWait();
        
    setSite(f1, 250, 160, 30);
    setSite(f2, 200, 60, 180);
    setSite(b2, 200, -110, 180);
    setSite(b1, 200, -10, 180);
    allMotionWait();
    
    setSite(f1, 250, 100, 180);
        
    setSite(f1, 250, 100, 200);
    setSite(f2, 200, 0, 30);
    setSite(b2, 200, -110, 30);
    setSite(b1, 200, -10, 200);
    allMotionWait();

    setSite(f1, 200, 60, 180);
    setSite(f2, 200, 220, 30);
    setSite(b2, 200, -10, 180);
    setSite(b1, 200, -110, 180);
    allMotionWait();

    setSite(f2, 200, 300, 180);
    allMotionWait();
    
    forwardTrot(1,levelPosTest,levelSpeTest,0,alta);
    moveSpeed = 9.0;
    setSite(f1, 200, -20, 240);
    setSite(f2, 200, 80, 240);
    setSite(b2, 200, -190, 260);
    setSite(b1, 200, -190, 260);
    allMotionWait();
        
    setSite(b1, 200, -300, 30);
    allMotionWait();
    setSite(b1, 200, 0, 30);
    allMotionWait();
    setSite(b1, 200, 0, 260);
    allMotionWait();
        
    setSite(f1, 230, -40, 230);
    setSite(f2, 200, 60, 230);
    setSite(b2, 200, -180, 260);
    setSite(b1, 200, -100, 260);
    allMotionWait();
        
    setSite(b2, 200, -300, 30);
    allMotionWait();
    setSite(b2, 200, 0, 30);
    allMotionWait();
    setSite(b2, 200, 0, 260);
    allMotionWait();
    ropeFlag = false;
    return alta;
}
//初期姿勢
void initPos(){
    moveSpeed = 1;
    float pos[3] = {150,0,100};
    for(int i=0;i<4;i++){
        for(int j=0;j<3;j++){
            currentSite[i][j] = pos[j];
            nextSite[i][j] = pos[j];
        }
    }
}
//7680,10100
//5800,9600
void lackUp(float initPos1,float initPos2){
    //1->10170,2->7500
    update.detach();
    float pos1 = initPos1;
    float pos2 = initPos2;
    float sum1 = float((10170.0 - float(initPos1))/1000.0);
    float sum2 = float((7500.0 - float(initPos2))/1000.0);
    for(int i=0;i<1000;i++){
        pos1 = pos1 + sum1;
        pos2 = pos2 + sum2;
        servo[0].move(4,int(pos1));
        servo[0].move(5,int(pos2));
        wait(0.0008);
    }
}

//---------------------------------------------------------------

int main() {
    //Led = 0;
    ropeTouch.pulsewidth_us(2400);
    for(int i=0;i<4;i++){
        servo[i].init();
    }
    //------赤、青モード決定-------------
    color.mode(PullUp);
    int colorCount = 0;
    for(int i=0;i<100;i++){
        if(color.read() == true){
            colorCount++;
        }else{
            colorCount--;
        }
        if(colorCount >= 0)
            initRed();
        else
            initBlue();
    }
    //--------------------------------
    //------平地と坂のモード決定----------
    sw.mode(PullUp);
    int modeCount = 0;
    while(1){
        if(sw.read() == true){
            modeCount++;
        }else{
            modeCount--;
        }
        if(modeCount >= 100){
            modeFlag = true;
            break;
        }else if(modeCount <= -100){
            modeFlag = false;
            break;
        }
    }
    //--------------------------------
    
    //---------平地モード---------------
    if(modeFlag == true){
        retry.mode(PullUp);
        servo[0].move(4,initLack1);//持ち上げ機構初期位置へ
        wait(0.1);
        servo[0].move(5,initLack2);//持ち上げ機構初期位置へ
        initPos();//スタート姿勢
        update.attach(&siteUpdate,0.02);//20msごとに座標値更新
        //wait(20.0);
        setSite(0, 200, 30, 160);
        setSite(1, 200, 175, 160);
        setSite(2, 200, -15, 170);
        setSite(3, 200, -160, 170);
        allMotionWait();
        //while(1){};
        wait(5.0);
        //----------ゲルゲ受け取り待機-------------
        bool flag = false;
        int count = 0;
        while(flag == false){
            if(int(start.read()) == false){
                count++;
            }else{
                count--;
            }
            if(count >= 5000){
                flag = true;
            }else if(count <= 0){
                flag = false;
                count = 0;
            }
        }
        wait(0.2);
        //Led = 1;
        //-------スタート--------------------
        float levelPosTest[8] = {200,30,-160,160,170,145,60,70};
        float levelSpeTest[2] = {9.0,10};
        if(retry.read() == false){
            //段差前
            forwardTrot(10,levelPosTest,levelSpeTest,flatCurve1);
            forwardTrot(2,levelPosTest,levelSpeTest,flatCurve2);
            //段差乗り越え
            stepOver();
            forwardTrot(3,levelPosTest,levelSpeTest,0);
            moveSpeed = 6;
            //リトライ待機
            setSite(0, 200, 30, 160);
            setSite(1, 200, 175, 160);
            setSite(2, 200, -15, 170);
            setSite(3, 200, -160, 170);
            allMotionWait();
            while(1){
                if(retry.read() == true){
                    break;
                }
            }
            wait(0.5);
            //ロープ
            ropeTouch.pulsewidth_us(700);
            forwardTrot(2,levelPosTest,levelSpeTest,ropeClab2);
            Turn(2,levelPosTest,levelSpeTest,1);
            ropeOver();
            ropeTouch.pulsewidth_us(700);
            float levelPosTest2[8] = {200,60,-140,160,170,0,60,70};
            float levelSpeTest2[2] = {8.0,10};
            clabWalk(clabCount,levelPosTest2,levelSpeTest2,ropeClab1);
            Turn(1,levelPosTest,levelSpeTest,ropeClab2);
            ropeOver();
            forwardTrot(1,levelPosTest,levelSpeTest,0);
            //坂
            while(true){
                if(sw.read() == false){
                    break;
                }
            }
            wait(1.0);
            setSite(0, 200, 30, 150);
            setSite(1, 200, 170, 150);
            setSite(2, 200, -30, 180);//-70
            setSite(3, 200, -30, 180);//-70
            allMotionWait();
            update.detach();
            wait(0.1);
            servo[0].move(5,slopeLack2);//持ち上げ機構初期位置へ
            wait(0.1);
            servo[0].speed(4,40);
            wait(0.005);
            servo[0].move(4,slopeLack1);
            wait(0.1);
            
                
        }else{
            ropeTouch.pulsewidth_us(700);
            forwardTrot(2,levelPosTest,levelSpeTest,0);
            Turn(2,levelPosTest,levelSpeTest,ropeClab2);
            ropeOver();
            ropeTouch.pulsewidth_us(700);
            float levelPosTest2[8] = {200,60,-140,160,170,0,60,70};
            float levelSpeTest2[2] = {8.0,10};
            clabWalk(clabCount,levelPosTest2,levelSpeTest2,ropeClab1);
            Turn(1,levelPosTest,levelSpeTest,ropeClab2);
            ropeOver();
            forwardTrot(1,levelPosTest,levelSpeTest,0);
        }
            
            
        //----------------------------------------------------------
        wait(10);
    }
    //---------坂モード-----------------
    else{
        servo[0].move(4,initLack1);//持ち上げ機構初期位置へ
        wait(0.1);
        servo[0].move(5,slopeLack2);//持ち上げ機構初期位置へ
        
        initPos();//スタート姿勢
        update.attach(&siteUpdate,0.02);//20msごとに座標値更新
        
        wait(1.0);
        setSite(0, 200, 30, 150);
        setSite(1, 200, 170, 150);
        setSite(2, 200, -30, 180);//-70
        setSite(3, 200, -30, 180);//-70
        allMotionWait();
        
        update.detach();
        //----------非接触合図待機-----------------
        trig = 0;
        echo.rise(&pulseRise);
        echo.fall(&pulseFall);
        bool flag = false;
        int count = 0;
        timer.reset();
        while(flag == false){
            trig = 0;
            wait_us(1);
            trig = 1;
            wait_us(11);
            trig = 0;
            wait_us(1);
            wait(0.01);
            if(Distance <= 250)
                count++;
            if(count >= 30)
                flag = true;
        }
        echo.rise(NULL);
        echo.fall(NULL);
        //Led = 1;
        servo[0].speed(4,40);
        wait(0.005);
        servo[0].move(4,slopeLack1);
        wait(0.1);
        //while(1){}
        update.attach(&siteUpdate,0.02);//20msごとに座標値更新
        //-----スタート-------------------
        
        moveSpeed = 8;
        setSite(2, 200, 0, 50);
        allMotionWait();
        setSite(2, 200, -200, 50);
        allMotionWait();
        setSite(2, 200, -200, 160);
        allMotionWait();
        setSite(3, 200, 0, 50);
        allMotionWait();
        setSite(3, 200, -120, 50);
        allMotionWait();
        setSite(3, 200, -90, 160);
        allMotionWait();
        
        
        float slopePosTest[8] = {210,170,-20,140,130,-130,110,60};
        float slopeSpeTest[8] = {7,7};
        
        float lackUpPos[8] = {170,150,-150,390,390,0,0,0};
        float lackUpSpe[2] = {4.3,4};
        //backTrot(10,slopePosTest,slopeSpeTest,0);
        backTrot(4,slopePosTest,slopeSpeTest,15);
        backTrot(6,slopePosTest,slopeSpeTest,0);
        
        standUp(lackUpPos,lackUpSpe);
        lackUp(slopeLack1,slopeLack2);
    }
}