nhk2019
Dependencies: mbed kondoSerialServo
Diff: main.cpp
- Revision:
- 2:6140746e19b1
- Parent:
- 1:5fb6895ad8e6
- Child:
- 3:0366c1adc510
--- a/main.cpp Sun Mar 17 11:24:51 2019 +0000 +++ b/main.cpp Wed May 01 03:47:34 2019 +0000 @@ -11,26 +11,40 @@ 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]; @@ -38,6 +52,17 @@ 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}; @@ -71,21 +96,21 @@ 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][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,11500,3500)); + 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)-300); + currentPulse[1][2] = int(fmap(currentAngle[1][2],0,270,3500,11500)-350); //右後足 - 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][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,11500,3500)); + 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)+100); + 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++){ @@ -97,6 +122,20 @@ } 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]); @@ -132,54 +171,7 @@ } //------------------歩行モーション------------------------------------ //静歩行 -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]; @@ -231,8 +223,31 @@ 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 turn,float *pos,float *speed){ +void backTrot(unsigned int count,float *pos,float *speed, float turn){ float stepRY = pos[5]; float stepLY = pos[5]; if(turn > 0){ @@ -247,8 +262,8 @@ //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]); + 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]); @@ -257,13 +272,161 @@ 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]); + 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; @@ -275,99 +438,247 @@ } } } +//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(); } - 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}; + //------平地と坂のモード決定---------- + 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; + } + } + //-------------------------------- - 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); + //---------平地モード--------------- + 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(); - setSite(0, 200, 100, 30); + //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(); - 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); + 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, 100, 130); - setSite(1, 200, 100, 130); - setSite(2, 200, -120, 230); - setSite(3, 200, -120, 230); + /* + setSite(0, 200, 20, 190); + setSite(1, 200, 160, 190); + setSite(2, 200, -200, 190);//-70 + setSite(3, 200, -60, 190);//-70 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); + */ + 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(); - 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(); + float slopePosTest[8] = {210,170,-20,150,130,-130,110,50}; + float slopeSpeTest[8] = {6,4}; - setSite(3, 200, -120, 30); - allMotionWait(); - setSite(3, 200, 0, 30); - allMotionWait(); - setSite(3, 200, 0, 130); - allMotionWait(); + 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); - 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); + standUp(lackUpPos,lackUpSpe); + lackUp(); } } \ No newline at end of file