nhk2019
Dependencies: mbed kondoSerialServo
Diff: main.cpp
- Revision:
- 0:ffe56c419abd
- Child:
- 1:5fb6895ad8e6
diff -r 000000000000 -r ffe56c419abd main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Mar 10 09:07:27 2019 +0000 @@ -0,0 +1,541 @@ +#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); + } +} \ No newline at end of file