ケンタ ミヤザキ
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