File content as of revision 1:5fb6895ad8e6:

#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


const float PI = 3.1415926;

//---各足の長さ----
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 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,3500,11500));
    currentPulse[0][1] = int(fmap(currentAngle[0][1],-135,135,11500,3500)+100);
    currentPulse[0][2] = int(fmap(currentAngle[0][2],0,270,3500,11500));
    //左前足
    currentPulse[1][0] = int(fmap(currentAngle[1][0],-135,135,11500,3500));
    currentPulse[1][1] = int(fmap(currentAngle[1][1],-135,135,11500,3500)-200);
    currentPulse[1][2] = int(fmap(currentAngle[1][2],0,270,3500,11500)-300);
    //右後足
    currentPulse[2][0] = int(fmap(currentAngle[2][0],-135,135,3500,11500));
    currentPulse[2][1] = int(fmap(currentAngle[2][1],-135,135,11500,3500)-200);
    currentPulse[2][2] = int(fmap(currentAngle[2][2],0,270,3500,11500));
    //左後足
    currentPulse[3][0] = int(fmap(currentAngle[3][0],-135,135,11500,3500));
    currentPulse[3][1] = int(fmap(currentAngle[3][1],-135,135,11500,3500));
    currentPulse[3][2] = int(fmap(currentAngle[3][2],0,270,3500,11500)+100);
    
    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(){
    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 forwardWalk1(unsigned int count,float *pos ,float *speed){
    while(count-- > 0){
        //2
        moveSpeed = speed[1];
        setSite(2,pos[0],pos[2],pos[4]-pos[7]);
        allMotionWait();
        setSite(2,pos[0],pos[2]+pos[5]*2,pos[4]-pos[7]);
        allMotionWait();
        setSite(2,pos[0],pos[2]+pos[5]*2,pos[4]);
        allMotionWait();
        //0
        setSite(0, pos[0], pos[1], pos[3]-pos[6]);
        allMotionWait();
        setSite(0, pos[0], pos[1] + pos[5]*2, pos[3]-pos[6]);
        allMotionWait();
        setSite(0, pos[0], pos[1] + pos[5]*2, pos[3]);
        allMotionWait();
        //機体移動
        moveSpeed = speed[0];
        setSite(0,pos[0], pos[1] + pos[5], pos[3]);
        setSite(1,pos[0], pos[1], pos[3]);
        setSite(2,pos[0], pos[2] + pos[5], pos[4]);
        setSite(3,pos[0], pos[2], pos[4]);
        allMotionWait();
        //3
        moveSpeed = speed[1];
        setSite(3,pos[0],pos[2],pos[4]-pos[7]);
        allMotionWait();
        setSite(3,pos[0],pos[2]+pos[5]*2,pos[4]-pos[7]);
        allMotionWait();
        setSite(3,pos[0],pos[2]+pos[5]*2,pos[4]);
        allMotionWait();
        //1
        setSite(1, pos[0], pos[1], pos[3]-pos[6]);
        allMotionWait();
        setSite(1, pos[0], pos[1] + pos[5]*2, pos[3]-pos[6]);
        allMotionWait();
        setSite(1, pos[0], pos[1] + pos[5]*2, pos[3]);
        allMotionWait();
        //機体移動
        moveSpeed = speed[0];
        setSite(0,pos[0], pos[1]  , pos[3]);
        setSite(1,pos[0], pos[1] + pos[5], pos[3]);
        setSite(2,pos[0], pos[2], pos[4]);
        setSite(3,pos[0], pos[2] + pos[5], pos[4]);
        allMotionWait();
    }
}
//座位
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 forwardTrot(unsigned int count, float *pos, float *speed, float turn){
    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){
        //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 backTrot(unsigned int count, float turn,float *pos,float *speed){
    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[3]);
        setSite(3, pos[0], pos[2] + stepLY, pos[3]-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[3]);
        allMotionWait();
        setSite(1, pos[0], pos[1] + stepLY, pos[3]);
        setSite(2, pos[0], pos[2] + stepRY, pos[4]);
        allMotionWait();  
    }
}
//初期姿勢
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];
        }
    }
}
//---------------------------------------------------------------

int main() {
    for(int i=0;i<4;i++){
        servo[i].init();
    }
    wait(1.0);
    initPos();
    update.attach(&siteUpdate,0.02);//20msごとに座標値更新
    wait(5.0);
    float levelPosTest[8] = {170,60,-110,170,170,100,70,70};
    float levelSpeTest[2] = {6,6};
    float slopePosTest[8] = {200,40,-90,130,230,50,100,100};
    float slopeSpeTest[2] = {4,4};
    float slopePosTest2[8] = {180,60,-110,180,160,100,90,90};
    float slopeSpeTest2[2] = {5,6};
    
    while (true) {
        forwardTrot(4,levelPosTest,levelSpeTest,0);
        moveSpeed = 1.5;
        setSite(0, 200, 100, 240);
        setSite(1, 200, 100, 240);
        setSite(2, 200, -20, 230);
        setSite(3, 200, -20, 230);
        allMotionWait();
        setSite(0, 200, 100, 30);
        allMotionWait();
        setSite(0, 200, 200, 30);
        allMotionWait();
        setSite(0, 200, 200, 130);
        allMotionWait();
        setSite(1, 200, 100, 30);
        allMotionWait();
        setSite(1, 200, 200, 30);
        allMotionWait();
        setSite(1, 200, 200, 130);
        allMotionWait();
        setSite(0, 200, 100, 130);
        setSite(1, 200, 100, 130);
        setSite(2, 200, -120, 230);
        setSite(3, 200, -120, 230);
        allMotionWait();
        forwardTrot(4,slopePosTest,slopeSpeTest,0);
        moveSpeed = 1.5;
        setSite(0, 200, 70, 130);
        setSite(1, 200, 70, 130);
        setSite(2, 200, -150, 240);
        setSite(3, 200, -150, 240);
        allMotionWait();
        
        setSite(2, 200, -120, 30);
        allMotionWait();
        setSite(2, 200, 0, 30);
        allMotionWait();
        setSite(2, 200, 0, 130);
        allMotionWait();
        
        setSite(0, 200, 20, 130);
        setSite(1, 200, 20, 130);
        setSite(2, 200, -100, 130);
        setSite(3, 200, -200, 230);
        allMotionWait();
        
        setSite(0, 200, 20, 30);
        allMotionWait();
        setSite(0, 200, 100, 30);
        allMotionWait();
        setSite(0, 200, 100, 200);
        allMotionWait();
        
        setSite(3, 200, -120, 30);
        allMotionWait();
        setSite(3, 200, 0, 30);
        allMotionWait();
        setSite(3, 200, 0, 130);
        allMotionWait();
        
        setSite(1, 200, 20, 30);
        allMotionWait();
        setSite(1, 200, 200, 30);
        allMotionWait();
        setSite(1, 200, 200, 200);
        allMotionWait();
        /*
        setSite(0, 190, 150, 180);
        setSite(1, 190, 150, 180);
        setSite(2, 190, -150, 160);
        setSite(3, 190, -150, 160);
        allMotionWait();
        */
        forwardTrot(10,slopePosTest2,slopeSpeTest2,0);
        
        wait(10);
        //backTrot(100,0);
    }
}