ケンタ ミヤザキ
nhk2019
Dependencies: mbed kondoSerialServo
Diff: main.cpp
- Revision:
- 4:e66bd933bafa
- Parent:
- 3:0366c1adc510
--- a/main.cpp Fri May 03 01:45:39 2019 +0000
+++ b/main.cpp Fri May 24 02:25:41 2019 +0000
@@ -31,6 +31,11 @@
int flatCurve2;
float slopeLack1;
float slopeLack2;
+int ropeCurve1;
+int ropeCurve2;
+int ropeClab1;
+int ropeClab2;
+int clabCount;
//-----------------------------
const float PI = 3.1415926;
@@ -65,10 +70,15 @@
void initRed(){//左側
initLack1 = 7600;
initLack2 = 3500;
- flatCurve1 = 5;
+ flatCurve1 = 0;
flatCurve2 = 15;
- slopeLack1 = 9700;
- slopeLack2 = 4000;
+ slopeLack1 = 9700;//9700
+ slopeLack2 = 3800;
+ ropeCurve1 = -20;
+ ropeCurve2 = -50;
+ ropeClab1 = 70;
+ ropeClab2 = 1;
+ clabCount = 2;
}
void initBlue(){//右側
@@ -77,7 +87,12 @@
flatCurve1 = -7;
flatCurve2 = -17;
slopeLack1 = 9700;
- slopeLack2 = 11000;
+ slopeLack2 = 11100;
+ ropeCurve1 = 15;
+ ropeCurve2 = 50;
+ ropeClab1 = -40;
+ ropeClab2 = 0;
+ clabCount = 2;
}
//--------------------------------------
@@ -126,20 +141,20 @@
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[0][1] = int(fmap(currentAngle[0][1],-135,135,11500,3500)+100);//+200
+ currentPulse[0][2] = int(fmap(currentAngle[0][2],0,270,3500,11500));//0
//左前足
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[1][1] = int(fmap(currentAngle[1][1],-135,135,11500,3500)-100);//-200
+ currentPulse[1][2] = int(fmap(currentAngle[1][2],0,270,3500,11500)-400);//-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[2][0] = int(fmap(currentAngle[2][0],-135,135,11500,3500)+500);
+ currentPulse[2][1] = int(fmap(currentAngle[2][1],-135,135,11500,3500));//0
+ currentPulse[2][2] = int(fmap(currentAngle[2][2],0,270,3500,11500)-100);//0
//左後足
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));
+ currentPulse[3][1] = int(fmap(currentAngle[3][1],-135,135,11500,3500));//0
+ currentPulse[3][2] = int(fmap(currentAngle[3][2],0,270,3500,11500)-100);//0
for(int i=0;i<3;i++){
for(int j=0;j<4;j++){
@@ -152,18 +167,18 @@
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;
- }
+ if(touch.read() == 0){
+ ropeCount++;
+ }else{
+ ropeCount--;
+ }
+ if(ropeCount >= 25){
+ 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++){
@@ -219,17 +234,14 @@
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];
+void Turn(unsigned int count,float *pos, float *speed,bool direction){
moveSpeed = speed[0];
- if(turn > 0){
- stepRY += turn;
- stepLY -= turn;
- }else if(turn < 0){
- stepRY += turn;
- stepLY -= turn;
+ float stepRY = 0;
+ float stepLY = 0;
+ if(direction == 0){
+ stepRY = pos[5];
+ }else{
+ stepLY = pos[5];
}
while(count-- > 0){
//0,3
@@ -249,10 +261,105 @@
allMotionWait();
setSite(1, pos[0], pos[1] + stepLY, pos[3]);
setSite(2, pos[0], pos[2] + stepRY, pos[4]);
- allMotionWait();
+ allMotionWait();
}
}
-void rightMove(unsigned int count, float *pos, float *speed, float turn){
+//トロット歩容
+void forwardTrot(unsigned int count, float *pos, float *speed, float turn, bool alta = 1){
+ 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){
+ if(alta == 1){
+ //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();
+ }else{
+ //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();
+ //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();
+ }
+ }
+}
+bool forwardTrot2(float *pos, float *speed, float turn){
+ float stepRY = pos[5];
+ float stepLY = pos[5];
+ bool alta = 0;
+ moveSpeed = speed[0];
+ if(turn > 0){
+ stepRY += turn;
+ stepLY -= turn;
+ }else if(turn < 0){
+ stepRY += turn;
+ stepLY -= turn;
+ }
+ while(1){
+ if(alta == 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();
+ }else{
+ //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();
+ }
+ if(ropeFlag == true){
+ return alta;
+ }
+ alta = !alta;
+ }
+}
+
+void clabWalk(unsigned int count, float *pos, float *speed, float turn){
moveSpeed = speed[0];
while(count-- > 0){
//0,3
@@ -382,79 +489,98 @@
}
-void ropeOver(){
- float levelPosTest[8] = {200,40,-110,150,170,110,35,80};
- float levelSpeTest[2] = {7,8};
+bool ropeOver(){
+ float levelPosTest[8] = {200,30,-160,160,170,145,60,70};
+ float levelSpeTest[2] = {7.5,8};
+
ropeFlag = false;
ropeSign = true;
- while(ropeFlag == false){
- forwardTrot(1,levelPosTest,levelSpeTest,0);
- }
+ bool alta;
+ alta = forwardTrot2(levelPosTest,levelSpeTest,0);
+ ropeTouch.pulsewidth_us(2400);
ropeSign = false;
moveSpeed = 6;
- setSite(0, 200, 60, 180);
- setSite(1, 200, 160, 180);
- setSite(2, 200, -10, 180);
- setSite(3, 200, -110, 180);
+ int f1;
+ int f2;
+ int b1;
+ int b2;
+ if(alta == true){
+ f1 = 0;
+ f2 = 1;
+ b1 = 3;
+ b2 = 2;
+ }else{
+ f1 = 1;
+ f2 = 0;
+ b1 = 2;
+ b2 = 3;
+ }
+
+ setSite(f1, 200, 60, 180);
+ setSite(f2, 200, 160, 180);
+
+ setSite(b2, 200, -10, 180);
+ setSite(b1, 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);
+ setSite(f1, 200, 0, 30);
+ setSite(f2, 200, 160, 180);
+ setSite(b2, 200, -10, 180);
+ setSite(b1, 200, -110, 30);
allMotionWait();
- setSite(0, 200, 60, 180);
- setSite(1, 200, 220, 30);
- setSite(2, 200, -10, 180);
- setSite(3, 200, -110, 180);
+ setSite(f1, 250, 160, 30);
+ setSite(f2, 200, 60, 180);
+ setSite(b2, 200, -110, 180);
+ setSite(b1, 200, -10, 180);
+ allMotionWait();
+
+ setSite(f1, 250, 100, 180);
+
+ setSite(f1, 250, 100, 200);
+ setSite(f2, 200, 0, 30);
+ setSite(b2, 200, -110, 30);
+ setSite(b1, 200, -10, 200);
allMotionWait();
- setSite(1, 200, 300, 180);
+ setSite(f1, 200, 60, 180);
+ setSite(f2, 200, 220, 30);
+ setSite(b2, 200, -10, 180);
+ setSite(b1, 200, -110, 180);
+ allMotionWait();
+
+ setSite(f2, 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);
+
+ forwardTrot(1,levelPosTest,levelSpeTest,0,alta);
+ moveSpeed = 9.0;
+ setSite(f1, 200, -20, 240);
+ setSite(f2, 200, 80, 240);
+ setSite(b2, 200, -190, 260);
+ setSite(b1, 200, -190, 260);
allMotionWait();
- setSite(3, 200, -300, 30);
+ setSite(b1, 200, -300, 30);
allMotionWait();
- setSite(3, 200, 0, 30);
+ setSite(b1, 200, 0, 30);
allMotionWait();
- setSite(3, 200, 0, 260);
+ setSite(b1, 200, 0, 260);
allMotionWait();
- setSite(0, 200, -40, 230);
- setSite(1, 200, 60, 230);
- setSite(2, 200, -180, 260);
- setSite(3, 200, -100, 260);
+ setSite(f1, 230, -40, 230);
+ setSite(f2, 200, 60, 230);
+ setSite(b2, 200, -180, 260);
+ setSite(b1, 200, -100, 260);
allMotionWait();
- setSite(2, 200, -300, 30);
+ setSite(b2, 200, -300, 30);
allMotionWait();
- setSite(2, 200, 0, 30);
+ setSite(b2, 200, 0, 30);
allMotionWait();
- setSite(2, 200, 0, 260);
+ setSite(b2, 200, 0, 260);
allMotionWait();
ropeFlag = false;
+ return alta;
}
//初期姿勢
void initPos(){
@@ -537,8 +663,8 @@
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(1, 200, 175, 160);
+ setSite(2, 200, -15, 170);
setSite(3, 200, -160, 170);
allMotionWait();
//while(1){};
@@ -565,49 +691,74 @@
float levelPosTest[8] = {200,30,-160,160,170,145,60,70};
float levelSpeTest[2] = {9.0,10};
if(retry.read() == false){
- //段差前
- forwardTrot(10,levelPosTest,levelSpeTest,flatCurve1);
- forwardTrot(2,levelPosTest,levelSpeTest,flatCurve2);
- //段差
- 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;
+ //段差前
+ forwardTrot(10,levelPosTest,levelSpeTest,flatCurve1);
+ forwardTrot(2,levelPosTest,levelSpeTest,flatCurve2);
+ //段差乗り越え
+ stepOver();
+ forwardTrot(3,levelPosTest,levelSpeTest,0);
+ moveSpeed = 6;
+ //リトライ待機
+ setSite(0, 200, 30, 160);
+ setSite(1, 200, 175, 160);
+ setSite(2, 200, -15, 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);
+ //ロープ
+ ropeTouch.pulsewidth_us(700);
+ forwardTrot(2,levelPosTest,levelSpeTest,ropeClab2);
+ Turn(2,levelPosTest,levelSpeTest,1);
ropeOver();
- float levelPosTest2[8] = {200,60,-140,160,170,80,70,80};
- rightMove(2,levelPosTest2,levelSpeTest,85);
- forwardTrot(1,levelPosTest,levelSpeTest,10);
- ropeFlag = false;
+ ropeTouch.pulsewidth_us(700);
+ float levelPosTest2[8] = {200,60,-140,160,170,0,60,70};
+ float levelSpeTest2[2] = {8.0,10};
+ clabWalk(clabCount,levelPosTest2,levelSpeTest2,ropeClab1);
+ Turn(1,levelPosTest,levelSpeTest,ropeClab2);
ropeOver();
- forwardTrot(7,levelPosTest,levelSpeTest,0);
+ forwardTrot(1,levelPosTest,levelSpeTest,0);
+ //坂
+ while(true){
+ if(sw.read() == false){
+ break;
+ }
}
- else{
- //forwardTrot(2,levelPosTest,levelSpeTest,17);
- ropeTouch.pulsewidth_us(600);
- forwardTrot(4,levelPosTest,levelSpeTest,-35);
- //forwardTrot(2,levelPosTest,levelSpeTest,4);
+ wait(1.0);
+ setSite(0, 200, 30, 150);
+ setSite(1, 200, 170, 150);
+ setSite(2, 200, -30, 180);//-70
+ setSite(3, 200, -30, 180);//-70
+ allMotionWait();
+ update.detach();
+ wait(0.1);
+ servo[0].move(5,slopeLack2);//持ち上げ機構初期位置へ
+ wait(0.1);
+ servo[0].speed(4,40);
+ wait(0.005);
+ servo[0].move(4,slopeLack1);
+ wait(0.1);
+
+
+ }else{
+ ropeTouch.pulsewidth_us(700);
+ forwardTrot(2,levelPosTest,levelSpeTest,0);
+ Turn(2,levelPosTest,levelSpeTest,ropeClab2);
ropeOver();
- float levelPosTest2[8] = {200,60,-140,160,170,80,70,80};
- rightMove(2,levelPosTest2,levelSpeTest,85);
- forwardTrot(1,levelPosTest,levelSpeTest,10);
- ropeFlag = false;
+ ropeTouch.pulsewidth_us(700);
+ float levelPosTest2[8] = {200,60,-140,160,170,0,60,70};
+ float levelSpeTest2[2] = {8.0,10};
+ clabWalk(clabCount,levelPosTest2,levelSpeTest2,ropeClab1);
+ Turn(1,levelPosTest,levelSpeTest,ropeClab2);
ropeOver();
- forwardTrot(7,levelPosTest,levelSpeTest,0);
+ forwardTrot(1,levelPosTest,levelSpeTest,0);
}
+
//----------------------------------------------------------
wait(10);
}
@@ -674,13 +825,14 @@
allMotionWait();
- float slopePosTest[8] = {210,170,-20,150,130,-130,110,50};
+ float slopePosTest[8] = {210,170,-20,140,130,-130,110,60};
float slopeSpeTest[8] = {7,7};
float lackUpPos[8] = {170,150,-150,390,390,0,0,0};
float lackUpSpe[2] = {4.3,4};
- backTrot(4,slopePosTest,slopeSpeTest,25);
- backTrot(6,slopePosTest,slopeSpeTest,2);
+ //backTrot(10,slopePosTest,slopeSpeTest,0);
+ backTrot(4,slopePosTest,slopeSpeTest,15);
+ backTrot(6,slopePosTest,slopeSpeTest,0);
standUp(lackUpPos,lackUpSpe);
lackUp(slopeLack1,slopeLack2);