ケンタ ミヤザキ
nhk2019
Dependencies: mbed kondoSerialServo
Diff: main.cpp
- Revision:
- 3:0366c1adc510
- Parent:
- 2:6140746e19b1
- Child:
- 4:e66bd933bafa
--- a/main.cpp Wed May 01 03:47:34 2019 +0000
+++ b/main.cpp Fri May 03 01:45:39 2019 +0000
@@ -22,7 +22,16 @@
DigitalOut trig(PA_4);
InterruptIn echo(PB_0);
Timer timer;
-DigitalOut Led(PA_5);//スタート合図
+//DigitalOut Led(PA_5);//スタート合図
+
+//-----------------------------
+float initLack1;
+float initLack2;
+int flatCurve1;
+int flatCurve2;
+float slopeLack1;
+float slopeLack2;
+//-----------------------------
const float PI = 3.1415926;
bool modeFlag = true;
@@ -52,6 +61,26 @@
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
+//----------色に合わせて初期化-------------
+void initRed(){//左側
+ initLack1 = 7600;
+ initLack2 = 3500;
+ flatCurve1 = 5;
+ flatCurve2 = 15;
+ slopeLack1 = 9700;
+ slopeLack2 = 4000;
+}
+
+void initBlue(){//右側
+ initLack1 = 7600;
+ initLack2 = 11500;
+ flatCurve1 = -7;
+ flatCurve2 = -17;
+ slopeLack1 = 9700;
+ slopeLack2 = 11000;
+}
+
+//--------------------------------------
//----------超音波センサー用関数------------
float Distance = 0;
void pulseRise(){
@@ -440,12 +469,13 @@
}
//7680,10100
//5800,9600
-void lackUp(){
+void lackUp(float initPos1,float initPos2){
+ //1->10170,2->7500
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;
+ float pos1 = initPos1;
+ float pos2 = initPos2;
+ float sum1 = float((10170.0 - float(initPos1))/1000.0);
+ float sum2 = float((7500.0 - float(initPos2))/1000.0);
for(int i=0;i<1000;i++){
pos1 = pos1 + sum1;
pos2 = pos2 + sum2;
@@ -458,11 +488,26 @@
//---------------------------------------------------------------
int main() {
- Led = 0;
+ //Led = 0;
ropeTouch.pulsewidth_us(2400);
for(int i=0;i<4;i++){
servo[i].init();
}
+ //------赤、青モード決定-------------
+ color.mode(PullUp);
+ int colorCount = 0;
+ for(int i=0;i<100;i++){
+ if(color.read() == true){
+ colorCount++;
+ }else{
+ colorCount--;
+ }
+ if(colorCount >= 0)
+ initRed();
+ else
+ initBlue();
+ }
+ //--------------------------------
//------平地と坂のモード決定----------
sw.mode(PullUp);
int modeCount = 0;
@@ -485,9 +530,9 @@
//---------平地モード---------------
if(modeFlag == true){
retry.mode(PullUp);
- servo[0].move(4,7600);//持ち上げ機構初期位置へ
+ servo[0].move(4,initLack1);//持ち上げ機構初期位置へ
wait(0.1);
- servo[0].move(5,5100);//持ち上げ機構初期位置へ
+ servo[0].move(5,initLack2);//持ち上げ機構初期位置へ
initPos();//スタート姿勢
update.attach(&siteUpdate,0.02);//20msごとに座標値更新
//wait(20.0);
@@ -497,12 +542,12 @@
setSite(3, 200, -160, 170);
allMotionWait();
//while(1){};
- wait(1.0);
+ wait(5.0);
//----------ゲルゲ受け取り待機-------------
bool flag = false;
int count = 0;
while(flag == false){
- if(start.read() == 0){
+ if(int(start.read()) == false){
count++;
}else{
count--;
@@ -515,14 +560,15 @@
}
}
wait(0.2);
- Led = 1;
+ //Led = 1;
//-------スタート--------------------
float levelPosTest[8] = {200,30,-160,160,170,145,60,70};
- float levelSpeTest[2] = {8.0,10};
+ float levelSpeTest[2] = {9.0,10};
if(retry.read() == false){
//段差前
- forwardTrot(9,levelPosTest,levelSpeTest,-1);
- forwardTrot(2,levelPosTest,levelSpeTest,18);
+ forwardTrot(10,levelPosTest,levelSpeTest,flatCurve1);
+ forwardTrot(2,levelPosTest,levelSpeTest,flatCurve2);
+ //段差
stepOver();
forwardTrot(3,levelPosTest,levelSpeTest,0);
moveSpeed = 6;
@@ -562,58 +608,25 @@
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
+ servo[0].move(4,initLack1);//持ち上げ機構初期位置へ
wait(0.1);
- servo[0].move(5,5800);//持ち上げ機構初期位置へ//8500
+ servo[0].move(5,slopeLack2);//持ち上げ機構初期位置へ
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
+ setSite(2, 200, -30, 180);//-70
+ setSite(3, 200, -30, 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;
@@ -637,23 +650,14 @@
}
echo.rise(NULL);
echo.fall(NULL);
- Led = 1;
+ //Led = 1;
servo[0].speed(4,40);
wait(0.005);
- servo[0].move(4,9700);//持ち上げ機構初期位置へ//5500
+ servo[0].move(4,slopeLack1);
wait(0.1);
+ //while(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);
@@ -671,14 +675,14 @@
float slopePosTest[8] = {210,170,-20,150,130,-130,110,50};
- float slopeSpeTest[8] = {6,4};
+ 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,20);
- backTrot(6,slopePosTest,slopeSpeTest,0);
+ backTrot(4,slopePosTest,slopeSpeTest,25);
+ backTrot(6,slopePosTest,slopeSpeTest,2);
standUp(lackUpPos,lackUpSpe);
- lackUp();
+ lackUp(slopeLack1,slopeLack2);
}
}
\ No newline at end of file