nhk2019
Dependencies: mbed kondoSerialServo
main.cpp
- Committer:
- kenken0721
- Date:
- 2019-03-17
- Revision:
- 1:5fb6895ad8e6
- Parent:
- 0:ffe56c419abd
- Child:
- 2:6140746e19b1
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); } }