File content as of revision 0:ffe56c419abd:

#include "mbed.h"
#include <math.h>
#include "SerialServo.h"

Serial pc(USBTX,USBRX);//デバック用シリアル
Ticker update;//現在地座標の更新割り込み
Ticker press;
SerialServo serServo[2] = {SerialServo(PC_4,PA_10),
                           SerialServo(PB_9,PB_8)};

PwmOut pwmServo[4][2] = {
    {PwmOut(PA_13),
     PwmOut(PA_14)},
    {PwmOut(PA_15),
     PwmOut(PC_1)},
    {PwmOut(PC_2),
     PwmOut(PC_3)},
    {PwmOut(PC_13),
     PwmOut(PC_7)}
};

AnalogIn pressure[4] = {
    AnalogIn(PA_0),
    AnalogIn(PA_1),
    AnalogIn(PA_4),
    AnalogIn(PB_0)
};

DigitalOut leds[4] = {
    DigitalOut(PA_12),
    DigitalOut(PA_11),
    DigitalOut(PB_12),
    DigitalOut(PB_11)
};
    
const float DS3218MIN = 500;
const float DS3218MAX = 2500;

const float PDI2060MIN = 850;
const float PDI2060MAX = 2350;

const float bodyLength = 161.72;

float PI = 3.1415926;
//---各足の長さ----
const float L1 = 76;
const float L2 = 155.5;
const float L3 = 290;

//------移動速度--------
float moveSpeed = 3;
float legSpeed = 7;
float bodySpeed = 4;

float groundZ = 300;
float stepZ = 190;
    
    
float defaultX = 210;
float stepY = 100;
float forwardDefaultY = 0;//50
float backDefaultY = -180;//-150
float downZ[4] = {300,300,300,300};
float upZ = 70;
float offsetZ= 20;

float alpha;
float beta;
float gamma;

//現在の座標
float currentSite[4][3];
//次の座標
float nextSite[4][3];
//値更新用スピード
float tempSpeed[4][3];

//現在の角度
float currentAngle[4][3];

//現在の周波数
int currentPulse[4][3];

bool pressFlag[4] = {false,false,false,false};
bool pressSign[4] = {false,false,false,false};
bool onTheStep[4] = {false,false,false,false};

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, bool mode = false){
    onTheStep[leg] = mode;
    if(mode == false){
        pressFlag[leg] == false;
        downZ[leg] = groundZ;
        if(z != upZ){
            z = downZ[leg];
        }
    }
    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 pressUpdate(){
    for(int i=0;i<4;i++){
        //pc.printf("%f \t",downZ[i]);
        float val = pressure[i].read();
        if(val <= 0.55){
            pressSign[i] = true;
            leds[i] = 1;
        }else{
            pressSign[i] = false;
            leds[i] = 0;
        }
    }
    //pc.printf("\n");  
}

void toPulse(int leg){
    switch(leg){
        case 0://右前足
            currentPulse[leg][0] = int(fmap(currentAngle[leg][0],-90.0,90.0,DS3218MAX,DS3218MIN)+100);//+100
            currentPulse[leg][1] = int(fmap(currentAngle[leg][1],90,-90,PDI2060MIN,PDI2060MAX)+20);
            currentPulse[leg][2] = int(fmap(currentAngle[leg][2],0,270,3500,11500)-300);
            serServo[0].move(leg,int(currentPulse[leg][2]));
            break;
        case 1://左前足
            currentPulse[leg][0] = int(fmap(currentAngle[leg][0],-90.0,90.0,DS3218MIN,DS3218MAX));
            currentPulse[leg][1] = int(fmap(currentAngle[leg][1],90,-90,PDI2060MIN,PDI2060MAX));
            currentPulse[leg][2] = int(fmap(currentAngle[leg][2],0,270,3500,11500));
            serServo[1].move(leg,int(currentPulse[leg][2]));
            break;
        case 2://右後足
            currentPulse[leg][0] = int(fmap(currentAngle[leg][0],-90.0,90.0,DS3218MAX,DS3218MIN)-10);//-50
            currentPulse[leg][1] = int(fmap(currentAngle[leg][1],90,-90,PDI2060MIN,PDI2060MAX));
            currentPulse[leg][2] = int(fmap(currentAngle[leg][2],0,270,3500,11500));
            serServo[0].move(leg,int(currentPulse[leg][2])+250);
            break;
        case 3://左後足
            currentPulse[leg][0] = int(fmap(currentAngle[leg][0],-90.0,90.0,DS3218MIN,DS3218MAX)-50);//-40 max min
            currentPulse[leg][1] = int(fmap(currentAngle[leg][1],90,-90,PDI2060MIN,PDI2060MAX)-70);
            currentPulse[leg][2] = int(fmap(currentAngle[leg][2],0,270,3500,11500));
            serServo[1].move(leg,int(currentPulse[leg][2]));
            break;
        default:
        break;
    }
    pwmServo[leg][0].pulsewidth_us(currentPulse[leg][0]);
    pwmServo[leg][1].pulsewidth_us(currentPulse[leg][1]);
}

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(j==2 && siteLength >= 110 && onTheStep[i] == true && pressSign[i] == true && pressFlag[i] == false){
                pressFlag[i] = true;
                //nextSite[i][0] = currentSite[i][0];
                downZ[i] = stepZ;
                nextSite[i][2] = stepZ;
            }
            if( siteLength >= abs(tempSpeed[i][j]))
                currentSite[i][j] += tempSpeed[i][j];
            else
                currentSite[i][j] = nextSite[i][j];
        }
        toAngle(i);
        toPulse(i);
    }
}

void motionWait(int leg){
    bool flag = false;
    while(flag == false){
        wait_us(0.001);
        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){
    defaultX = 210;
    stepY = 60;
    forwardDefaultY = 10;
    backDefaultY = -150;
    groundZ = 300;
    stepZ  = 160;
    for(int i=0;i<4;i++){
        downZ[i] = groundZ;
    }
    upZ = 50;
    offsetZ= 10;
    legSpeed = 3;
    bodySpeed = 3;
    while(count-- > 0){
        //2
        moveSpeed = legSpeed;
        setSite(2,defaultX,backDefaultY,upZ);
        allMotionWait();
        setSite(2,defaultX+100,backDefaultY+stepY*2,upZ);
        allMotionWait();
        setSite(2,defaultX,backDefaultY+stepY*2,downZ[2],1);
        allMotionWait();
        //0
        moveSpeed = legSpeed;
        setSite(0, defaultX+100, forwardDefaultY, upZ);
        allMotionWait();
        setSite(0, defaultX, forwardDefaultY + stepY*2, upZ);
        allMotionWait();
        setSite(0, defaultX, forwardDefaultY + stepY*2, downZ[0],1);
        allMotionWait();
        
        moveSpeed = bodySpeed;
        setSite(0,defaultX, forwardDefaultY + stepY, downZ[0],1);
        setSite(1,defaultX, forwardDefaultY, downZ[1],1);
        setSite(2,defaultX, backDefaultY + stepY, downZ[2],1);
        setSite(3,defaultX, backDefaultY, downZ[3],1);
        allMotionWait();
        
        //3
        moveSpeed = legSpeed;
        setSite(3,defaultX,backDefaultY,upZ);
        allMotionWait();
        setSite(3,defaultX+100,backDefaultY+stepY*2,upZ);
        allMotionWait();
        setSite(3,defaultX,backDefaultY+stepY*2,downZ[3],1);
        allMotionWait();
        
        //1
        moveSpeed = legSpeed;
        setSite(1, defaultX+100, forwardDefaultY, upZ);
        allMotionWait();
        setSite(1, defaultX, forwardDefaultY + stepY*2, upZ);
        allMotionWait();
        setSite(1, defaultX, forwardDefaultY + stepY*2, downZ[1],1);
        allMotionWait();
    
        
        
        moveSpeed = bodySpeed;
        setSite(0,defaultX, forwardDefaultY  , downZ[0],1);
        setSite(1,defaultX, forwardDefaultY + stepY, downZ[1],1);
        setSite(2,defaultX, backDefaultY, downZ[2],1);
        setSite(3,defaultX, backDefaultY + stepY, downZ[3],1);
        allMotionWait();
    }
}
void backWalk1(unsigned int count){
    defaultX = 180;
    stepY = -40;
    forwardDefaultY = 120;//0,1
    backDefaultY = 0;//2,3
    for(int i=0;i<4;i++){
        downZ[i] = 200;
    }
    upZ = 110;
    legSpeed = 3;
    bodySpeed = 3;
    while(count-- > 0){
        //2
        moveSpeed = legSpeed;
        setSite(3,defaultX,backDefaultY,upZ);
        allMotionWait();
        setSite(3,defaultX,backDefaultY+stepY*2,upZ);
        allMotionWait();
        setSite(3,defaultX,backDefaultY+stepY*2,downZ[3]);
        allMotionWait();
        //0
        moveSpeed = legSpeed;
        setSite(1, defaultX, forwardDefaultY, upZ);
        allMotionWait();
        setSite(1, defaultX, forwardDefaultY + stepY*2, upZ);
        allMotionWait();
        setSite(1, defaultX, forwardDefaultY + stepY*2, downZ[1]);
        allMotionWait();
    
        moveSpeed = bodySpeed;
        setSite(0,defaultX, forwardDefaultY, downZ[0]);
        setSite(1,defaultX, forwardDefaultY + stepY, downZ[1]);
        setSite(2,defaultX, backDefaultY, downZ[2]);
        setSite(3,defaultX, backDefaultY + stepY, downZ[3]);
        allMotionWait();
        
        //2
        moveSpeed = legSpeed;
        setSite(2,defaultX,backDefaultY,upZ);
        allMotionWait();
        setSite(2,defaultX,backDefaultY+stepY*2,upZ);
        allMotionWait();
        setSite(2,defaultX,backDefaultY+stepY*2,downZ[2]);
        allMotionWait();
        
        //0
        moveSpeed = legSpeed;
        setSite(0, defaultX, forwardDefaultY, upZ);
        allMotionWait();
        setSite(0, defaultX, forwardDefaultY + stepY*2, upZ);
        allMotionWait();
        setSite(0, defaultX, forwardDefaultY + stepY*2, downZ[0]);
        allMotionWait();
    
        moveSpeed = bodySpeed;
        setSite(0,defaultX, forwardDefaultY + stepY  , downZ[0]);
        setSite(1,defaultX, forwardDefaultY, downZ[2]);
        setSite(2,defaultX, backDefaultY + stepY, downZ[2]);
        setSite(3,defaultX, backDefaultY, downZ[3]);
        allMotionWait();
    }
}
//トロット歩容
void forwardWalk2(unsigned int count){
    defaultX = 210;
    stepY = 100;
    forwardDefaultY = 0;
    backDefaultY = -120;
    for(int i=0;i<4;i++){
        downZ[i] = 180;
    }
    upZ = 100;
    offsetZ= 0;
    const float offsetX = 20;
    
    while(count-- > 0){
        moveSpeed = 6;
        
        setSite(0, defaultX+offsetX, forwardDefaultY + stepY, upZ -offsetZ);
        setSite(1, defaultX+offsetX, forwardDefaultY, downZ[1] - offsetZ);
        setSite(2, defaultX, backDefaultY, downZ[2]);
        setSite(3, defaultX, backDefaultY + stepY, upZ);
        allMotionWait();
        
        setSite(0, defaultX+offsetX, forwardDefaultY + stepY, downZ[0] - offsetZ);
        setSite(1, defaultX+offsetX, forwardDefaultY, downZ[1] - offsetZ);
        setSite(2, defaultX, backDefaultY, downZ[2]);
        setSite(3, defaultX, backDefaultY + stepY, downZ[3]);
        allMotionWait();
        
        setSite(0, defaultX+offsetX, forwardDefaultY, downZ[0] - offsetZ);
        setSite(1, defaultX+offsetX, forwardDefaultY + stepY, upZ - offsetZ);
        setSite(2, defaultX, backDefaultY + stepY, upZ);
        setSite(3, defaultX, backDefaultY, downZ[3]);
        allMotionWait();
        
        setSite(0, defaultX+offsetX, forwardDefaultY, downZ[0] - offsetZ);
        setSite(1, defaultX+offsetX, forwardDefaultY + stepY, downZ[1] - offsetZ);
        setSite(2, defaultX, backDefaultY + stepY, downZ[2]);
        setSite(3, defaultX, backDefaultY, downZ[3]);
        allMotionWait();  
    }
}

void standDown(){
    defaultX = 210;
    forwardDefaultY = 120;
    backDefaultY = -120;
    for(int i=0;i<4;i++){
        downZ[i] = 180;
    }
    
    upZ = 100;
    offsetZ= 0;
    
    setSite(0, defaultX, forwardDefaultY, downZ[0]);
    setSite(1, defaultX, forwardDefaultY, downZ[1]);
    setSite(2, defaultX, backDefaultY, downZ[2]);
    setSite(3, defaultX, backDefaultY, downZ[3]);
    allMotionWait();
}

void standUp(){
    defaultX = 210;
    forwardDefaultY = 120;
    backDefaultY = -120;
    for(int i=0;i<4;i++){
        downZ[i] = 180;
    }
    
    upZ = 100;
    offsetZ= 0;
    const float standZ = 400;
    moveSpeed = 0.5;
    setSite(0, defaultX, forwardDefaultY, standZ);
    setSite(1, defaultX, forwardDefaultY, standZ);
    setSite(2, defaultX, backDefaultY, standZ);
    setSite(3, defaultX, backDefaultY, standZ);
    allMotionWait();
}

int main() {
    serServo[0].init();
    serServo[1].init();
    for(int i=0;i<4;i++){
        leds[i] = 0;
    }
    for(int i=0;i<2;i++){
        currentSite[i][0] = defaultX;
        currentSite[i][1] = forwardDefaultY;
        currentSite[i][2] = downZ[i];
        nextSite[i][0] = defaultX;
        nextSite[i][1] = forwardDefaultY;
        nextSite[i][2] = downZ[i];
    }
    for(int i=2;i<4;i++){
        currentSite[i][0] = defaultX;
        currentSite[i][1] = backDefaultY;
        currentSite[i][2] = downZ[i];
        nextSite[i][0] = defaultX;
        nextSite[i][1] = backDefaultY;
        nextSite[i][2] = downZ[i];
    }
    update.attach(&siteUpdate,0.02);//20msごとに座標値更新
    press.attach(&pressUpdate,0.1);
    wait(3.0);
    while (true) {
        /*
        moveSpeed = 1;
        upZ = 190;
        setSite(3, defaultX, backDefaultY + 150, upZ);
        allMotionWait();
        setSite(3, defaultX, backDefaultY , upZ);
        allMotionWait();
        */
        /*
        setSite(0,defaultX+100,forwardDefaultY,50);
        allMotionWait();
        
        setSite(0,defaultX,forwardDefaultY+150,50);
        allMotionWait();
        
        setSite(0,defaultX,forwardDefaultY+150,190);
        allMotionWait();
        wait(10.0);
        */
        /*
        pressFlag = true;
        moveSpeed = 1;
        setSite(0, defaultX, forwardDefaultY + 50, downZ);
        setSite(1, defaultX, forwardDefaultY, downZ);
        setSite(2, defaultX, backDefaultY + 50, downZ);
        setSite(3, defaultX, backDefaultY + 100, downZ);
        allMotionWait();
        pressFlag = false;
        setSite(1,defaultX+100, forwardDefaultY, 50);
        allMotionWait();
        setSite(1,defaultX, forwardDefaultY+100, 50);
        allMotionWait();
        pressFlag = true;
        setSite(1,defaultX, forwardDefaultY+100, 300);
        allMotionWait();
        */
        /*
        setSite(2,defaultX+100, backDefaultY+100, 50);
        allMotionWait();
        setSite(2,defaultX, backDefaultY+100, 300);
        allMotionWait();
        */
        //setSite(2,defaultX+50, backDefaultY+100, upZ-30);
        /*
        setSite(3,defaultX, backDefaultY, downZ);
        allMotionWait();
        
        setSite(3,defaultX+60, backDefaultY, upZ);
        allMotionWait();
        
        setSite(3,defaultX+60, backDefaultY+100, upZ);
        allMotionWait();
        
        
        setSite(3,defaultX, backDefaultY+100, downZ);
        allMotionWait();
        
        */
        //setSite(3,defaultX, backDefaultY, downZ);
        //allMotionWait();
        
        //setSite(1,defaultX+60, forwardDefaultY, downZ-70);
        
        //setSite(2,defaultX, backDefaultY+100, downZ);
        //allMotionWait();
        
        //moveSpeed = 3;
        //setSite(3, 200, 0, 200);
        //allMotionWait();
        /*
        pc.printf("%f \t",currentAngle[3][1]);
        pc.printf("%f \n",currentAngle[3][2]);
        
        setSite(3, 300, 0, 200);
        allMotionWait();
        pc.printf("%f \t",currentAngle[3][1]);
        pc.printf("%f \n",currentAngle[3][2]);
        */
        forwardWalk1(100);
        //forwardWalk2(100);
        //backWalk1(100);
        //standDown();
        //standUp();
        //wait(30);
    }
}