nhk2019
Dependencies: mbed kondoSerialServo
main.cpp
- Committer:
- kenken0721
- Date:
- 2019-05-01
- Revision:
- 2:6140746e19b1
- Parent:
- 1:5fb6895ad8e6
- Child:
- 3:0366c1adc510
File content as of revision 2:6140746e19b1:
#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);//スタート合図 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; } //----------超音波センサー用関数------------ 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)+200); currentPulse[0][2] = int(fmap(currentAngle[0][2],0,270,3500,11500)); //左前足 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)-200); currentPulse[1][2] = int(fmap(currentAngle[1][2],0,270,3500,11500)-350); //右後足 currentPulse[2][0] = int(fmap(currentAngle[2][0],-135,135,11500,3500)); currentPulse[2][1] = int(fmap(currentAngle[2][1],-135,135,11500,3500)); currentPulse[2][2] = int(fmap(currentAngle[2][2],0,270,3500,11500)); //左後足 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)); currentPulse[3][2] = int(fmap(currentAngle[3][2],0,270,3500,11500)); 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 >= 40){ 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 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 rightMove(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); } void ropeOver(){ float levelPosTest[8] = {200,40,-110,150,170,110,35,80}; float levelSpeTest[2] = {7,8}; ropeFlag = false; ropeSign = true; while(ropeFlag == false){ forwardTrot(1,levelPosTest,levelSpeTest,0); } ropeSign = false; moveSpeed = 6; setSite(0, 200, 60, 180); setSite(1, 200, 160, 180); setSite(2, 200, -10, 180); setSite(3, 200, -110, 180); allMotionWait(); setSite(0, 200, 0, 30); setSite(1, 200, 160, 180); setSite(2, 200, -10, 180); setSite(3, 200, -110, 30); allMotionWait(); setSite(0, 250, 160, 30); setSite(1, 200, 60, 180); setSite(2, 200, -110, 180); setSite(3, 200, -10, 180); allMotionWait(); setSite(0, 250, 100, 180); setSite(0, 250, 100, 200); setSite(1, 200, 0, 30); setSite(2, 200, -110, 30); setSite(3, 200, -10, 200); allMotionWait(); setSite(0, 200, 60, 180); setSite(1, 200, 220, 30); setSite(2, 200, -10, 180); setSite(3, 200, -110, 180); allMotionWait(); setSite(1, 200, 300, 180); allMotionWait(); forwardTrot(1,levelPosTest,levelSpeTest,0); moveSpeed = 6.5; setSite(0, 200, -20, 240); setSite(1, 200, 80, 240); setSite(2, 200, -190, 260); setSite(3, 200, -190, 260); allMotionWait(); setSite(3, 200, -300, 30); allMotionWait(); setSite(3, 200, 0, 30); allMotionWait(); setSite(3, 200, 0, 260); allMotionWait(); setSite(0, 200, -40, 230); setSite(1, 200, 60, 230); setSite(2, 200, -180, 260); setSite(3, 200, -100, 260); allMotionWait(); setSite(2, 200, -300, 30); allMotionWait(); setSite(2, 200, 0, 30); allMotionWait(); setSite(2, 200, 0, 260); allMotionWait(); ropeFlag = false; } //初期姿勢 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(){ update.detach(); float pos1 = 9500.0; float pos2 = 6000.0; float sum1 = 0.5;//0.666;//(5500 - 4834)/1000; float sum2 = 3.5;//(8500- 4034)/1000; 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(); } //------平地と坂のモード決定---------- 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,7600);//持ち上げ機構初期位置へ wait(0.1); servo[0].move(5,5100);//持ち上げ機構初期位置へ initPos();//スタート姿勢 update.attach(&siteUpdate,0.02);//20msごとに座標値更新 //wait(20.0); setSite(0, 200, 30, 160); setSite(1, 200, 160, 160); setSite(2, 200, -30, 170); setSite(3, 200, -160, 170); allMotionWait(); //while(1){}; wait(1.0); //----------ゲルゲ受け取り待機------------- bool flag = false; int count = 0; while(flag == false){ if(start.read() == 0){ 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] = {8.0,10}; if(retry.read() == false){ //段差前 forwardTrot(9,levelPosTest,levelSpeTest,-1); forwardTrot(2,levelPosTest,levelSpeTest,18); stepOver(); forwardTrot(3,levelPosTest,levelSpeTest,0); moveSpeed = 6; setSite(0, 200, 30, 160); setSite(1, 200, 160, 160); setSite(2, 200, -30, 170); setSite(3, 200, -160, 170); allMotionWait(); while(1){ if(retry.read() == true){ break; } } wait(0.5); ropeTouch.pulsewidth_us(600); forwardTrot(4,levelPosTest,levelSpeTest,-35); //forwardTrot(2,levelPosTest,levelSpeTest,4); ropeOver(); float levelPosTest2[8] = {200,60,-140,160,170,80,70,80}; rightMove(2,levelPosTest2,levelSpeTest,85); forwardTrot(1,levelPosTest,levelSpeTest,10); ropeFlag = false; ropeOver(); forwardTrot(7,levelPosTest,levelSpeTest,0); } else{ //forwardTrot(2,levelPosTest,levelSpeTest,17); ropeTouch.pulsewidth_us(600); forwardTrot(4,levelPosTest,levelSpeTest,-35); //forwardTrot(2,levelPosTest,levelSpeTest,4); ropeOver(); float levelPosTest2[8] = {200,60,-140,160,170,80,70,80}; rightMove(2,levelPosTest2,levelSpeTest,85); forwardTrot(1,levelPosTest,levelSpeTest,10); ropeFlag = false; ropeOver(); forwardTrot(7,levelPosTest,levelSpeTest,0); } /* float levelPosTest[8] = {200,60,-110,150,170,80,70,80}; float levelSpeTest[2] = {7,8}; */ /* float levelPosTest[8] = {200,60,-160,160,170,140,50,60}; float levelSpeTest[2] = {8.5,8}; //forwardTrot(9,levelPosTest,levelSpeTest,0); forwardTrot(3,levelPosTest,levelSpeTest,15); stepOver(); forwardTrot(4,levelPosTest,levelSpeTest,0); */ /* float levelPosTest3[8] = {190,60,-110,160,170,100,60,80}; float levelSpeTest3[2] = {7,8}; forwardTrot(3,levelPosTest3,levelSpeTest3,7); float levelPosTest4[8] = {180,60,-110,160,170,100,60,80}; float levelSpeTest4[2] = {7,8}; forwardTrot(5,levelPosTest4,levelSpeTest4,-40); float levelPosTest5[8] = {180,60,-110,160,170,100,60,80}; float levelSpeTest5[2] = {7,8}; forwardTrot(5,levelPosTest5,levelSpeTest5,0); */ //---------------------------------------------------------- wait(10); } //---------坂モード----------------- else{ servo[0].move(4,7600);//持ち上げ機構初期位置へ//5500 //servo[0].move(4,9700);//持ち上げ機構初期位置へ//5500 wait(0.1); servo[0].move(5,5800);//持ち上げ機構初期位置へ//8500 initPos();//スタート姿勢 update.attach(&siteUpdate,0.02);//20msごとに座標値更新 //while(1){} wait(1.0); setSite(0, 200, 30, 150); setSite(1, 200, 170, 150); setSite(2, 200, 0, 180);//-70 setSite(3, 200, 0, 180);//-70 allMotionWait(); /* setSite(0, 200, 20, 190); setSite(1, 200, 160, 190); setSite(2, 200, -200, 190);//-70 setSite(3, 200, -60, 190);//-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,9700);//持ち上げ機構初期位置へ//5500 wait(0.1); update.attach(&siteUpdate,0.02);//20msごとに座標値更新 //-----スタート------------------- //float levelPosTest[8] = {200,150,-60,150,170,-100,50,50}; //float levelSpeTest[2] = {6,7}; //forwardTrot(3,levelPosTest,levelSpeTest,0); //loat levelPosTest2[8] = {200,150,-60,150,170,-100,50,120}; //float levelSpeTest2[2] = {6,7}; //forwardTrot(1,levelPosTest2,levelSpeTest2,0); //float slopePosTest2[8] = {200,160,-60,190,190,-140,80,80}; //float slopeSpeTest2[8] = {6,4}; //backTrot(2,slopePosTest2,slopeSpeTest2,0); 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,150,130,-130,110,50}; float slopeSpeTest[8] = {6,4}; float lackUpPos[8] = {170,150,-150,390,390,0,0,0}; float lackUpSpe[2] = {4.3,4}; backTrot(4,slopePosTest,slopeSpeTest,20); backTrot(6,slopePosTest,slopeSpeTest,0); standUp(lackUpPos,lackUpSpe); lackUp(); } }