ケンタ ミヤザキ / Mbed 2 deprecated quad_spider_nhk2019_04

nhk2019

Dependencies:   mbed kondoSerialServo

main.cpp@3:0366c1adc510, 2019-05-03 (annotated)

Committer:
kenken0721
Date:
Fri May 03 01:45:39 2019 +0000
Revision:
3:0366c1adc510
Parent:
2:6140746e19b1
Child:
4:e66bd933bafa
nhk2019

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kenken0721 0:ffe56c419abd 1 #include "mbed.h"
kenken0721 0:ffe56c419abd 2 #include "SerialServo.h"
kenken0721 0:ffe56c419abd 3
kenken0721 1:5fb6895ad8e6 4 //Serial pc(USBTX,USBRX);//デバック用シリアル
kenken0721 0:ffe56c419abd 5 Ticker update;//現在地座標の更新割り込み
kenken0721 0:ffe56c419abd 6
kenken0721 1:5fb6895ad8e6 7 SerialServo servo[4] = {SerialServo(PC_4,PC_5),//serial1
kenken0721 1:5fb6895ad8e6 8 SerialServo(PA_2,PA_3),//serial5
kenken0721 1:5fb6895ad8e6 9 //SerialServo(PC_10,PC_11),//serial5
kenken0721 1:5fb6895ad8e6 10 SerialServo(PB_10,PB_11),//serial3
kenken0721 1:5fb6895ad8e6 11 SerialServo(PC_12,PD_2)};//serial4
kenken0721 0:ffe56c419abd 12
kenken0721 0:ffe56c419abd 13
kenken0721 2:6140746e19b1 14 PwmOut ropeTouch(PA_13);//タッチセンサ位置切り替え
kenken0721 2:6140746e19b1 15 DigitalIn sw(PC_1);//平地と坂の切り替え
kenken0721 2:6140746e19b1 16 DigitalIn start(PA_0);//gerege受け取り時の合図
kenken0721 2:6140746e19b1 17 DigitalIn touch(PA_1);//糸探知用タッチセンサ
kenken0721 2:6140746e19b1 18 DigitalIn retry(PC_13);
kenken0721 2:6140746e19b1 19 DigitalIn color(PC_0);//赤、青モード切替
kenken0721 2:6140746e19b1 20
kenken0721 2:6140746e19b1 21 //---非接触合図(超音波センサー)---
kenken0721 2:6140746e19b1 22 DigitalOut trig(PA_4);
kenken0721 2:6140746e19b1 23 InterruptIn echo(PB_0);
kenken0721 2:6140746e19b1 24 Timer timer;
kenken0721 3:0366c1adc510 25 //DigitalOut Led(PA_5);//スタート合図
kenken0721 3:0366c1adc510 26
kenken0721 3:0366c1adc510 27 //-----------------------------
kenken0721 3:0366c1adc510 28 float initLack1;
kenken0721 3:0366c1adc510 29 float initLack2;
kenken0721 3:0366c1adc510 30 int flatCurve1;
kenken0721 3:0366c1adc510 31 int flatCurve2;
kenken0721 3:0366c1adc510 32 float slopeLack1;
kenken0721 3:0366c1adc510 33 float slopeLack2;
kenken0721 3:0366c1adc510 34 //-----------------------------
kenken0721 2:6140746e19b1 35
kenken0721 1:5fb6895ad8e6 36 const float PI = 3.1415926;
kenken0721 2:6140746e19b1 37 bool modeFlag = true;
kenken0721 2:6140746e19b1 38 bool ropeFlag = false;
kenken0721 2:6140746e19b1 39 bool ropeSign = false;
kenken0721 2:6140746e19b1 40 int ropeCount = 0;
kenken0721 0:ffe56c419abd 41
kenken0721 0:ffe56c419abd 42 //---各足の長さ----
kenken0721 0:ffe56c419abd 43 const float L1 = 76;
kenken0721 1:5fb6895ad8e6 44 const float L2 = 168;
kenken0721 1:5fb6895ad8e6 45 const float L3 = 252;
kenken0721 0:ffe56c419abd 46
kenken0721 2:6140746e19b1 47 //移動速度
kenken0721 1:5fb6895ad8e6 48 float moveSpeed = 1;
kenken0721 0:ffe56c419abd 49 //現在の座標
kenken0721 0:ffe56c419abd 50 float currentSite[4][3];
kenken0721 0:ffe56c419abd 51 //次の座標
kenken0721 0:ffe56c419abd 52 float nextSite[4][3];
kenken0721 0:ffe56c419abd 53 //値更新用スピード
kenken0721 0:ffe56c419abd 54 float tempSpeed[4][3];
kenken0721 0:ffe56c419abd 55 //現在の角度
kenken0721 0:ffe56c419abd 56 float currentAngle[4][3];
kenken0721 0:ffe56c419abd 57 //現在の周波数
kenken0721 0:ffe56c419abd 58 int currentPulse[4][3];
kenken0721 0:ffe56c419abd 59
kenken0721 0:ffe56c419abd 60 float fmap(float x, float in_min, float in_max, float out_min, float out_max){
kenken0721 0:ffe56c419abd 61 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
kenken0721 0:ffe56c419abd 62 }
kenken0721 0:ffe56c419abd 63
kenken0721 3:0366c1adc510 64 //----------色に合わせて初期化-------------
kenken0721 3:0366c1adc510 65 void initRed(){//左側
kenken0721 3:0366c1adc510 66 initLack1 = 7600;
kenken0721 3:0366c1adc510 67 initLack2 = 3500;
kenken0721 3:0366c1adc510 68 flatCurve1 = 5;
kenken0721 3:0366c1adc510 69 flatCurve2 = 15;
kenken0721 3:0366c1adc510 70 slopeLack1 = 9700;
kenken0721 3:0366c1adc510 71 slopeLack2 = 4000;
kenken0721 3:0366c1adc510 72 }
kenken0721 3:0366c1adc510 73
kenken0721 3:0366c1adc510 74 void initBlue(){//右側
kenken0721 3:0366c1adc510 75 initLack1 = 7600;
kenken0721 3:0366c1adc510 76 initLack2 = 11500;
kenken0721 3:0366c1adc510 77 flatCurve1 = -7;
kenken0721 3:0366c1adc510 78 flatCurve2 = -17;
kenken0721 3:0366c1adc510 79 slopeLack1 = 9700;
kenken0721 3:0366c1adc510 80 slopeLack2 = 11000;
kenken0721 3:0366c1adc510 81 }
kenken0721 3:0366c1adc510 82
kenken0721 3:0366c1adc510 83 //--------------------------------------
kenken0721 2:6140746e19b1 84 //----------超音波センサー用関数------------
kenken0721 2:6140746e19b1 85 float Distance = 0;
kenken0721 2:6140746e19b1 86 void pulseRise(){
kenken0721 2:6140746e19b1 87 timer.start();
kenken0721 2:6140746e19b1 88 }
kenken0721 2:6140746e19b1 89 void pulseFall(){
kenken0721 2:6140746e19b1 90 timer.stop();
kenken0721 2:6140746e19b1 91 Distance = timer.read_us();
kenken0721 2:6140746e19b1 92 timer.reset();
kenken0721 2:6140746e19b1 93 }
kenken0721 2:6140746e19b1 94 //--------------------------------------
kenken0721 0:ffe56c419abd 95 //座標指定
kenken0721 1:5fb6895ad8e6 96 void setSite(int leg, float x, float y, float z){
kenken0721 0:ffe56c419abd 97 float length[3] = {0.0,0.0,0.0};
kenken0721 0:ffe56c419abd 98 length[0] = x - currentSite[leg][0];
kenken0721 0:ffe56c419abd 99 length[1] = y - currentSite[leg][1];
kenken0721 0:ffe56c419abd 100 length[2] = z - currentSite[leg][2];
kenken0721 0:ffe56c419abd 101 float L = sqrt(pow(length[0],2)+pow(length[1],2)+pow(length[2],2));
kenken0721 0:ffe56c419abd 102 tempSpeed[leg][0] = (length[0]/L)*moveSpeed;
kenken0721 0:ffe56c419abd 103 tempSpeed[leg][1] = (length[1]/L)*moveSpeed;
kenken0721 0:ffe56c419abd 104 tempSpeed[leg][2] = (length[2]/L)*moveSpeed;
kenken0721 0:ffe56c419abd 105 nextSite[leg][0] = x;
kenken0721 0:ffe56c419abd 106 nextSite[leg][1] = y;
kenken0721 0:ffe56c419abd 107 nextSite[leg][2] = z;
kenken0721 0:ffe56c419abd 108 }
kenken0721 0:ffe56c419abd 109
kenken0721 0:ffe56c419abd 110 void toAngle(int leg){
kenken0721 0:ffe56c419abd 111 float x = currentSite[leg][0];
kenken0721 0:ffe56c419abd 112 float y = currentSite[leg][1];
kenken0721 0:ffe56c419abd 113 float z = currentSite[leg][2];
kenken0721 0:ffe56c419abd 114 float L = sqrt((x*x)+(y*y));
kenken0721 0:ffe56c419abd 115 float w = L - L1;
kenken0721 0:ffe56c419abd 116 float v = sqrt((w*w)+(z*z));
kenken0721 0:ffe56c419abd 117 float alpha = atan2(z,w) - acos(((v*v)+(L2*L2)-(L3*L3))/(2*L2*v));
kenken0721 0:ffe56c419abd 118 float beta = PI - acos(((L2*L2) + (L3*L3) - (v*v))/(2*L2*L3));
kenken0721 0:ffe56c419abd 119 float gamma = atan2(y, x);
kenken0721 0:ffe56c419abd 120 currentAngle[leg][0] = gamma = (gamma / PI * 180);
kenken0721 0:ffe56c419abd 121 currentAngle[leg][1] = alpha = (alpha / PI * 180);
kenken0721 0:ffe56c419abd 122 currentAngle[leg][2] = beta = (beta / PI * 180);
kenken0721 0:ffe56c419abd 123 }
kenken0721 0:ffe56c419abd 124
kenken0721 1:5fb6895ad8e6 125
kenken0721 1:5fb6895ad8e6 126 void toPulse(){
kenken0721 1:5fb6895ad8e6 127 //右前足
kenken0721 2:6140746e19b1 128 currentPulse[0][0] = int(fmap(currentAngle[0][0],-135,135,11500,3500));
kenken0721 2:6140746e19b1 129 currentPulse[0][1] = int(fmap(currentAngle[0][1],-135,135,11500,3500)+200);
kenken0721 1:5fb6895ad8e6 130 currentPulse[0][2] = int(fmap(currentAngle[0][2],0,270,3500,11500));
kenken0721 1:5fb6895ad8e6 131 //左前足
kenken0721 2:6140746e19b1 132 currentPulse[1][0] = int(fmap(currentAngle[1][0],-135,135,3500,11500)-100);
kenken0721 1:5fb6895ad8e6 133 currentPulse[1][1] = int(fmap(currentAngle[1][1],-135,135,11500,3500)-200);
kenken0721 2:6140746e19b1 134 currentPulse[1][2] = int(fmap(currentAngle[1][2],0,270,3500,11500)-350);
kenken0721 1:5fb6895ad8e6 135 //右後足
kenken0721 2:6140746e19b1 136 currentPulse[2][0] = int(fmap(currentAngle[2][0],-135,135,11500,3500));
kenken0721 2:6140746e19b1 137 currentPulse[2][1] = int(fmap(currentAngle[2][1],-135,135,11500,3500));
kenken0721 1:5fb6895ad8e6 138 currentPulse[2][2] = int(fmap(currentAngle[2][2],0,270,3500,11500));
kenken0721 1:5fb6895ad8e6 139 //左後足
kenken0721 2:6140746e19b1 140 currentPulse[3][0] = int(fmap(currentAngle[3][0],-135,135,3500,11500)+100);
kenken0721 1:5fb6895ad8e6 141 currentPulse[3][1] = int(fmap(currentAngle[3][1],-135,135,11500,3500));
kenken0721 2:6140746e19b1 142 currentPulse[3][2] = int(fmap(currentAngle[3][2],0,270,3500,11500));
kenken0721 1:5fb6895ad8e6 143
kenken0721 1:5fb6895ad8e6 144 for(int i=0;i<3;i++){
kenken0721 1:5fb6895ad8e6 145 for(int j=0;j<4;j++){
kenken0721 1:5fb6895ad8e6 146 servo[j].move(i+1,int(currentPulse[j][i]));
kenken0721 1:5fb6895ad8e6 147 wait_us(100);
kenken0721 0:ffe56c419abd 148 }
kenken0721 1:5fb6895ad8e6 149 wait_us(400);
kenken0721 0:ffe56c419abd 150 }
kenken0721 0:ffe56c419abd 151 }
kenken0721 0:ffe56c419abd 152
kenken0721 0:ffe56c419abd 153 void siteUpdate(){
kenken0721 2:6140746e19b1 154 if(ropeSign == true){
kenken0721 2:6140746e19b1 155 if(touch.read() == 0){
kenken0721 2:6140746e19b1 156 ropeCount++;
kenken0721 2:6140746e19b1 157 }else{
kenken0721 2:6140746e19b1 158 ropeCount--;
kenken0721 2:6140746e19b1 159 }
kenken0721 2:6140746e19b1 160 if(ropeCount >= 40){
kenken0721 2:6140746e19b1 161 ropeFlag = true;
kenken0721 2:6140746e19b1 162 ropeCount = 0;
kenken0721 2:6140746e19b1 163 }else if(ropeCount <= 0){
kenken0721 2:6140746e19b1 164 ropeFlag = false;
kenken0721 2:6140746e19b1 165 ropeCount = 0;
kenken0721 2:6140746e19b1 166 }
kenken0721 2:6140746e19b1 167 }
kenken0721 0:ffe56c419abd 168 for(int i=0;i<4;i++){
kenken0721 0:ffe56c419abd 169 for(int j=0;j<3;j++){
kenken0721 0:ffe56c419abd 170 float siteLength = abs(currentSite[i][j] - nextSite[i][j]);
kenken0721 0:ffe56c419abd 171 if( siteLength >= abs(tempSpeed[i][j]))
kenken0721 0:ffe56c419abd 172 currentSite[i][j] += tempSpeed[i][j];
kenken0721 0:ffe56c419abd 173 else
kenken0721 0:ffe56c419abd 174 currentSite[i][j] = nextSite[i][j];
kenken0721 0:ffe56c419abd 175 }
kenken0721 0:ffe56c419abd 176 toAngle(i);
kenken0721 0:ffe56c419abd 177 }
kenken0721 1:5fb6895ad8e6 178 toPulse();
kenken0721 0:ffe56c419abd 179 }
kenken0721 0:ffe56c419abd 180
kenken0721 0:ffe56c419abd 181 void motionWait(int leg){
kenken0721 0:ffe56c419abd 182 bool flag = false;
kenken0721 0:ffe56c419abd 183 while(flag == false){
kenken0721 1:5fb6895ad8e6 184 wait_us(1);
kenken0721 0:ffe56c419abd 185 if(currentSite[leg][0] == nextSite[leg][0]){
kenken0721 0:ffe56c419abd 186 if(currentSite[leg][1] == nextSite[leg][1]){
kenken0721 0:ffe56c419abd 187 if(currentSite[leg][2] == nextSite[leg][2]){
kenken0721 0:ffe56c419abd 188 flag = true;
kenken0721 0:ffe56c419abd 189 }
kenken0721 0:ffe56c419abd 190 }
kenken0721 0:ffe56c419abd 191 }
kenken0721 0:ffe56c419abd 192 }
kenken0721 0:ffe56c419abd 193 }
kenken0721 0:ffe56c419abd 194
kenken0721 0:ffe56c419abd 195 //すべての足が動き終わるまで待機
kenken0721 0:ffe56c419abd 196 void allMotionWait(){
kenken0721 0:ffe56c419abd 197 for(int i=0;i<4;i++){
kenken0721 0:ffe56c419abd 198 motionWait(i);
kenken0721 0:ffe56c419abd 199 }
kenken0721 0:ffe56c419abd 200 }
kenken0721 1:5fb6895ad8e6 201 //------------------歩行モーション------------------------------------
kenken0721 0:ffe56c419abd 202 //静歩行
kenken0721 2:6140746e19b1 203
kenken0721 1:5fb6895ad8e6 204 //座位
kenken0721 1:5fb6895ad8e6 205 void standDown(float *pos,float *speed){
kenken0721 1:5fb6895ad8e6 206 moveSpeed = speed[0];
kenken0721 1:5fb6895ad8e6 207 setSite(0, pos[0], pos[1], pos[3]);
kenken0721 1:5fb6895ad8e6 208 setSite(1, pos[0], pos[1], pos[3]);
kenken0721 1:5fb6895ad8e6 209 setSite(2, pos[0], pos[2], pos[4]);
kenken0721 1:5fb6895ad8e6 210 setSite(3, pos[0], pos[2], pos[4]);
kenken0721 1:5fb6895ad8e6 211 allMotionWait();
kenken0721 1:5fb6895ad8e6 212 }
kenken0721 1:5fb6895ad8e6 213 //直立姿勢
kenken0721 1:5fb6895ad8e6 214 void standUp(float *pos,float *speed){
kenken0721 1:5fb6895ad8e6 215 moveSpeed = speed[0];
kenken0721 1:5fb6895ad8e6 216 setSite(0, pos[0], pos[1], pos[3]);
kenken0721 1:5fb6895ad8e6 217 setSite(1, pos[0], pos[1], pos[3]);
kenken0721 1:5fb6895ad8e6 218 setSite(2, pos[0], pos[2], pos[4]);
kenken0721 1:5fb6895ad8e6 219 setSite(3, pos[0], pos[2], pos[4]);
kenken0721 1:5fb6895ad8e6 220 allMotionWait();
kenken0721 0:ffe56c419abd 221 }
kenken0721 0:ffe56c419abd 222 //トロット歩容
kenken0721 1:5fb6895ad8e6 223 void forwardTrot(unsigned int count, float *pos, float *speed, float turn){
kenken0721 1:5fb6895ad8e6 224 float stepRY = pos[5];
kenken0721 1:5fb6895ad8e6 225 float stepLY = pos[5];
kenken0721 1:5fb6895ad8e6 226 moveSpeed = speed[0];
kenken0721 1:5fb6895ad8e6 227 if(turn > 0){
kenken0721 1:5fb6895ad8e6 228 stepRY += turn;
kenken0721 1:5fb6895ad8e6 229 stepLY -= turn;
kenken0721 1:5fb6895ad8e6 230 }else if(turn < 0){
kenken0721 1:5fb6895ad8e6 231 stepRY += turn;
kenken0721 1:5fb6895ad8e6 232 stepLY -= turn;
kenken0721 0:ffe56c419abd 233 }
kenken0721 0:ffe56c419abd 234 while(count-- > 0){
kenken0721 1:5fb6895ad8e6 235 //0,3
kenken0721 1:5fb6895ad8e6 236 setSite(0, pos[0], pos[1] + stepRY, pos[3]-pos[6]);
kenken0721 1:5fb6895ad8e6 237 setSite(1, pos[0], pos[1], pos[3]);
kenken0721 1:5fb6895ad8e6 238 setSite(2, pos[0], pos[2], pos[4]);
kenken0721 1:5fb6895ad8e6 239 setSite(3, pos[0], pos[2] + stepLY, pos[4]-pos[7]);
kenken0721 1:5fb6895ad8e6 240 allMotionWait();
kenken0721 1:5fb6895ad8e6 241 setSite(0, pos[0], pos[1] + stepRY, pos[3]);
kenken0721 1:5fb6895ad8e6 242 setSite(3, pos[0], pos[2] + stepLY, pos[4]);
kenken0721 0:ffe56c419abd 243 allMotionWait();
kenken0721 1:5fb6895ad8e6 244 //1,2
kenken0721 1:5fb6895ad8e6 245 setSite(0, pos[0], pos[1], pos[3]);
kenken0721 1:5fb6895ad8e6 246 setSite(1, pos[0], pos[1] + stepLY, pos[3]-pos[6]);
kenken0721 1:5fb6895ad8e6 247 setSite(2, pos[0], pos[2] + stepRY, pos[4]-pos[7]);
kenken0721 1:5fb6895ad8e6 248 setSite(3, pos[0], pos[2], pos[4]);
kenken0721 0:ffe56c419abd 249 allMotionWait();
kenken0721 1:5fb6895ad8e6 250 setSite(1, pos[0], pos[1] + stepLY, pos[3]);
kenken0721 1:5fb6895ad8e6 251 setSite(2, pos[0], pos[2] + stepRY, pos[4]);
kenken0721 0:ffe56c419abd 252 allMotionWait();
kenken0721 0:ffe56c419abd 253 }
kenken0721 0:ffe56c419abd 254 }
kenken0721 2:6140746e19b1 255 void rightMove(unsigned int count, float *pos, float *speed, float turn){
kenken0721 2:6140746e19b1 256 moveSpeed = speed[0];
kenken0721 2:6140746e19b1 257 while(count-- > 0){
kenken0721 2:6140746e19b1 258 //0,3
kenken0721 2:6140746e19b1 259 setSite(0, pos[0]+turn, pos[1] + pos[5], pos[3]-pos[6]);
kenken0721 2:6140746e19b1 260 setSite(1, pos[0], pos[1], pos[3]);
kenken0721 2:6140746e19b1 261 setSite(2, pos[0], pos[2], pos[4]);
kenken0721 2:6140746e19b1 262 setSite(3, pos[0]-turn, pos[2] + pos[5], pos[4]-pos[7]);
kenken0721 2:6140746e19b1 263 allMotionWait();
kenken0721 2:6140746e19b1 264 setSite(0, pos[0]+turn, pos[1] + pos[5], pos[3]);
kenken0721 2:6140746e19b1 265 setSite(3, pos[0]-turn, pos[2] + pos[5], pos[4]);
kenken0721 2:6140746e19b1 266 allMotionWait();
kenken0721 2:6140746e19b1 267 //1,2
kenken0721 2:6140746e19b1 268 setSite(0, pos[0], pos[1], pos[3]);
kenken0721 2:6140746e19b1 269 setSite(1, pos[0]-turn, pos[1] + pos[5], pos[3]-pos[6]);
kenken0721 2:6140746e19b1 270 setSite(2, pos[0]+turn, pos[2] + pos[5], pos[4]-pos[7]);
kenken0721 2:6140746e19b1 271 setSite(3, pos[0], pos[2], pos[4]);
kenken0721 2:6140746e19b1 272 allMotionWait();
kenken0721 2:6140746e19b1 273 setSite(1, pos[0]-turn, pos[1] + pos[5], pos[3]);
kenken0721 2:6140746e19b1 274 setSite(2, pos[0]+turn, pos[2] + pos[5], pos[4]);
kenken0721 2:6140746e19b1 275 allMotionWait();
kenken0721 2:6140746e19b1 276 }
kenken0721 2:6140746e19b1 277 }
kenken0721 0:ffe56c419abd 278
kenken0721 2:6140746e19b1 279 void backTrot(unsigned int count,float *pos,float *speed, float turn){
kenken0721 1:5fb6895ad8e6 280 float stepRY = pos[5];
kenken0721 1:5fb6895ad8e6 281 float stepLY = pos[5];
kenken0721 1:5fb6895ad8e6 282 if(turn > 0){
kenken0721 1:5fb6895ad8e6 283 stepRY += turn;
kenken0721 1:5fb6895ad8e6 284 stepLY -= turn;
kenken0721 1:5fb6895ad8e6 285 }else if(turn < 0){
kenken0721 1:5fb6895ad8e6 286 stepRY += turn;
kenken0721 1:5fb6895ad8e6 287 stepLY -= turn;
kenken0721 0:ffe56c419abd 288 }
kenken0721 1:5fb6895ad8e6 289 moveSpeed = speed[0];
kenken0721 1:5fb6895ad8e6 290 while(count-- > 0){
kenken0721 1:5fb6895ad8e6 291 //0,3
kenken0721 1:5fb6895ad8e6 292 setSite(0, pos[0], pos[1] + stepRY, pos[3]-pos[6]);
kenken0721 1:5fb6895ad8e6 293 setSite(1, pos[0], pos[1], pos[3]);
kenken0721 2:6140746e19b1 294 setSite(2, pos[0], pos[2], pos[4]);
kenken0721 2:6140746e19b1 295 setSite(3, pos[0], pos[2] + stepLY, pos[4]-pos[7]);
kenken0721 1:5fb6895ad8e6 296 allMotionWait();
kenken0721 1:5fb6895ad8e6 297 setSite(0, pos[0], pos[1] + stepRY, pos[3]);
kenken0721 1:5fb6895ad8e6 298 setSite(3, pos[0], pos[2] + stepLY, pos[4]);
kenken0721 1:5fb6895ad8e6 299 allMotionWait();
kenken0721 1:5fb6895ad8e6 300 //1,2
kenken0721 1:5fb6895ad8e6 301 setSite(0, pos[0], pos[1], pos[3]);
kenken0721 1:5fb6895ad8e6 302 setSite(1, pos[0], pos[1] + stepLY, pos[3]-pos[6]);
kenken0721 1:5fb6895ad8e6 303 setSite(2, pos[0], pos[2] + stepRY, pos[4]-pos[7]);
kenken0721 2:6140746e19b1 304 setSite(3, pos[0], pos[2], pos[4]);
kenken0721 1:5fb6895ad8e6 305 allMotionWait();
kenken0721 1:5fb6895ad8e6 306 setSite(1, pos[0], pos[1] + stepLY, pos[3]);
kenken0721 1:5fb6895ad8e6 307 setSite(2, pos[0], pos[2] + stepRY, pos[4]);
kenken0721 1:5fb6895ad8e6 308 allMotionWait();
kenken0721 1:5fb6895ad8e6 309 }
kenken0721 0:ffe56c419abd 310 }
kenken0721 2:6140746e19b1 311 void stepOver(){
kenken0721 2:6140746e19b1 312 moveSpeed = 8;//-30
kenken0721 2:6140746e19b1 313 setSite(0, 200, 20, 250);
kenken0721 2:6140746e19b1 314 setSite(1, 200, 155, 250);
kenken0721 2:6140746e19b1 315 setSite(2, 200, -25, 250);
kenken0721 2:6140746e19b1 316 setSite(3, 200, -160, 250);
kenken0721 2:6140746e19b1 317 allMotionWait();
kenken0721 2:6140746e19b1 318
kenken0721 2:6140746e19b1 319 setSite(0, 200, -20, 30);
kenken0721 2:6140746e19b1 320 allMotionWait();
kenken0721 2:6140746e19b1 321 setSite(0, 200, 200, 30);
kenken0721 2:6140746e19b1 322 allMotionWait();
kenken0721 2:6140746e19b1 323 setSite(0, 200, 200, 140);
kenken0721 2:6140746e19b1 324 allMotionWait();
kenken0721 2:6140746e19b1 325
kenken0721 2:6140746e19b1 326 setSite(1, 200, 30, 30);
kenken0721 2:6140746e19b1 327 allMotionWait();
kenken0721 2:6140746e19b1 328 setSite(1, 200, 270, 30);
kenken0721 2:6140746e19b1 329 allMotionWait();
kenken0721 2:6140746e19b1 330 setSite(1, 200, 270, 140);
kenken0721 2:6140746e19b1 331 allMotionWait();
kenken0721 2:6140746e19b1 332
kenken0721 2:6140746e19b1 333 float stepPosTest[8] = {200,10,-150,110,250,70,60,80};
kenken0721 2:6140746e19b1 334 float stepSpeTest[2] = {7,5};
kenken0721 2:6140746e19b1 335 forwardTrot(2,stepPosTest,stepSpeTest,0);
kenken0721 2:6140746e19b1 336
kenken0721 2:6140746e19b1 337 moveSpeed = 8;
kenken0721 2:6140746e19b1 338 setSite(2, 200, -200, 30);
kenken0721 2:6140746e19b1 339 allMotionWait();
kenken0721 2:6140746e19b1 340 setSite(2, 200, 40, 30);
kenken0721 2:6140746e19b1 341 allMotionWait();
kenken0721 2:6140746e19b1 342 setSite(2, 200, 40, 140);
kenken0721 2:6140746e19b1 343 allMotionWait();
kenken0721 2:6140746e19b1 344
kenken0721 2:6140746e19b1 345 setSite(0, 200, 105, 30);
kenken0721 2:6140746e19b1 346 allMotionWait();
kenken0721 2:6140746e19b1 347 setSite(0, 200, 280, 30);
kenken0721 2:6140746e19b1 348 allMotionWait();
kenken0721 2:6140746e19b1 349 setSite(0, 200, 280, 240);
kenken0721 2:6140746e19b1 350 allMotionWait();
kenken0721 2:6140746e19b1 351
kenken0721 2:6140746e19b1 352 moveSpeed = 7;
kenken0721 2:6140746e19b1 353 setSite(0, 200, 180, 230);//80
kenken0721 2:6140746e19b1 354 setSite(1, 200, -20, 140);
kenken0721 2:6140746e19b1 355 setSite(2, 200, -70, 140);//-80
kenken0721 2:6140746e19b1 356 setSite(3, 200, -240, 240);
kenken0721 2:6140746e19b1 357 allMotionWait();
kenken0721 2:6140746e19b1 358
kenken0721 2:6140746e19b1 359 moveSpeed = 8;
kenken0721 2:6140746e19b1 360 setSite(3, 200, -250, 30);
kenken0721 2:6140746e19b1 361 allMotionWait();
kenken0721 2:6140746e19b1 362 setSite(3, 200, -150, 30);
kenken0721 2:6140746e19b1 363 allMotionWait();
kenken0721 2:6140746e19b1 364 setSite(3, 200, -150, 140);//-150
kenken0721 2:6140746e19b1 365 allMotionWait();
kenken0721 2:6140746e19b1 366
kenken0721 2:6140746e19b1 367 setSite(1, 200, -10, 30);
kenken0721 2:6140746e19b1 368 allMotionWait();
kenken0721 2:6140746e19b1 369 setSite(1, 200, 250, 30);
kenken0721 2:6140746e19b1 370 allMotionWait();
kenken0721 2:6140746e19b1 371 setSite(1, 200, 250, 240);//150
kenken0721 2:6140746e19b1 372 allMotionWait();
kenken0721 2:6140746e19b1 373
kenken0721 2:6140746e19b1 374 float stepPosTest2[8] = {200,30,-150,250,140,100,60,120};
kenken0721 2:6140746e19b1 375 float stepSpeTest2[2] = {7,6};
kenken0721 2:6140746e19b1 376 forwardTrot(1,stepPosTest2,stepSpeTest2,0);
kenken0721 2:6140746e19b1 377
kenken0721 2:6140746e19b1 378 float stepPosTest3[8] = {200,30,-120,200,180,100,60,140};
kenken0721 2:6140746e19b1 379 float stepSpeTest3[2] = {7,6};
kenken0721 2:6140746e19b1 380 forwardTrot(1,stepPosTest3,stepSpeTest3,0);
kenken0721 2:6140746e19b1 381
kenken0721 2:6140746e19b1 382
kenken0721 2:6140746e19b1 383 }
kenken0721 2:6140746e19b1 384
kenken0721 2:6140746e19b1 385 void ropeOver(){
kenken0721 2:6140746e19b1 386 float levelPosTest[8] = {200,40,-110,150,170,110,35,80};
kenken0721 2:6140746e19b1 387 float levelSpeTest[2] = {7,8};
kenken0721 2:6140746e19b1 388 ropeFlag = false;
kenken0721 2:6140746e19b1 389 ropeSign = true;
kenken0721 2:6140746e19b1 390 while(ropeFlag == false){
kenken0721 2:6140746e19b1 391 forwardTrot(1,levelPosTest,levelSpeTest,0);
kenken0721 2:6140746e19b1 392 }
kenken0721 2:6140746e19b1 393 ropeSign = false;
kenken0721 2:6140746e19b1 394 moveSpeed = 6;
kenken0721 2:6140746e19b1 395 setSite(0, 200, 60, 180);
kenken0721 2:6140746e19b1 396 setSite(1, 200, 160, 180);
kenken0721 2:6140746e19b1 397 setSite(2, 200, -10, 180);
kenken0721 2:6140746e19b1 398 setSite(3, 200, -110, 180);
kenken0721 2:6140746e19b1 399 allMotionWait();
kenken0721 2:6140746e19b1 400
kenken0721 2:6140746e19b1 401 setSite(0, 200, 0, 30);
kenken0721 2:6140746e19b1 402 setSite(1, 200, 160, 180);
kenken0721 2:6140746e19b1 403 setSite(2, 200, -10, 180);
kenken0721 2:6140746e19b1 404 setSite(3, 200, -110, 30);
kenken0721 2:6140746e19b1 405 allMotionWait();
kenken0721 2:6140746e19b1 406
kenken0721 2:6140746e19b1 407 setSite(0, 250, 160, 30);
kenken0721 2:6140746e19b1 408 setSite(1, 200, 60, 180);
kenken0721 2:6140746e19b1 409 setSite(2, 200, -110, 180);
kenken0721 2:6140746e19b1 410 setSite(3, 200, -10, 180);
kenken0721 2:6140746e19b1 411 allMotionWait();
kenken0721 2:6140746e19b1 412
kenken0721 2:6140746e19b1 413 setSite(0, 250, 100, 180);
kenken0721 2:6140746e19b1 414
kenken0721 2:6140746e19b1 415 setSite(0, 250, 100, 200);
kenken0721 2:6140746e19b1 416 setSite(1, 200, 0, 30);
kenken0721 2:6140746e19b1 417 setSite(2, 200, -110, 30);
kenken0721 2:6140746e19b1 418 setSite(3, 200, -10, 200);
kenken0721 2:6140746e19b1 419 allMotionWait();
kenken0721 2:6140746e19b1 420
kenken0721 2:6140746e19b1 421 setSite(0, 200, 60, 180);
kenken0721 2:6140746e19b1 422 setSite(1, 200, 220, 30);
kenken0721 2:6140746e19b1 423 setSite(2, 200, -10, 180);
kenken0721 2:6140746e19b1 424 setSite(3, 200, -110, 180);
kenken0721 2:6140746e19b1 425 allMotionWait();
kenken0721 2:6140746e19b1 426
kenken0721 2:6140746e19b1 427 setSite(1, 200, 300, 180);
kenken0721 2:6140746e19b1 428 allMotionWait();
kenken0721 2:6140746e19b1 429
kenken0721 2:6140746e19b1 430 forwardTrot(1,levelPosTest,levelSpeTest,0);
kenken0721 2:6140746e19b1 431 moveSpeed = 6.5;
kenken0721 2:6140746e19b1 432 setSite(0, 200, -20, 240);
kenken0721 2:6140746e19b1 433 setSite(1, 200, 80, 240);
kenken0721 2:6140746e19b1 434 setSite(2, 200, -190, 260);
kenken0721 2:6140746e19b1 435 setSite(3, 200, -190, 260);
kenken0721 2:6140746e19b1 436 allMotionWait();
kenken0721 2:6140746e19b1 437
kenken0721 2:6140746e19b1 438 setSite(3, 200, -300, 30);
kenken0721 2:6140746e19b1 439 allMotionWait();
kenken0721 2:6140746e19b1 440 setSite(3, 200, 0, 30);
kenken0721 2:6140746e19b1 441 allMotionWait();
kenken0721 2:6140746e19b1 442 setSite(3, 200, 0, 260);
kenken0721 2:6140746e19b1 443 allMotionWait();
kenken0721 2:6140746e19b1 444
kenken0721 2:6140746e19b1 445 setSite(0, 200, -40, 230);
kenken0721 2:6140746e19b1 446 setSite(1, 200, 60, 230);
kenken0721 2:6140746e19b1 447 setSite(2, 200, -180, 260);
kenken0721 2:6140746e19b1 448 setSite(3, 200, -100, 260);
kenken0721 2:6140746e19b1 449 allMotionWait();
kenken0721 2:6140746e19b1 450
kenken0721 2:6140746e19b1 451 setSite(2, 200, -300, 30);
kenken0721 2:6140746e19b1 452 allMotionWait();
kenken0721 2:6140746e19b1 453 setSite(2, 200, 0, 30);
kenken0721 2:6140746e19b1 454 allMotionWait();
kenken0721 2:6140746e19b1 455 setSite(2, 200, 0, 260);
kenken0721 2:6140746e19b1 456 allMotionWait();
kenken0721 2:6140746e19b1 457 ropeFlag = false;
kenken0721 2:6140746e19b1 458 }
kenken0721 1:5fb6895ad8e6 459 //初期姿勢
kenken0721 1:5fb6895ad8e6 460 void initPos(){
kenken0721 1:5fb6895ad8e6 461 moveSpeed = 1;
kenken0721 1:5fb6895ad8e6 462 float pos[3] = {150,0,100};
kenken0721 0:ffe56c419abd 463 for(int i=0;i<4;i++){
kenken0721 1:5fb6895ad8e6 464 for(int j=0;j<3;j++){
kenken0721 1:5fb6895ad8e6 465 currentSite[i][j] = pos[j];
kenken0721 1:5fb6895ad8e6 466 nextSite[i][j] = pos[j];
kenken0721 1:5fb6895ad8e6 467 }
kenken0721 0:ffe56c419abd 468 }
kenken0721 0:ffe56c419abd 469 }
kenken0721 2:6140746e19b1 470 //7680,10100
kenken0721 2:6140746e19b1 471 //5800,9600
kenken0721 3:0366c1adc510 472 void lackUp(float initPos1,float initPos2){
kenken0721 3:0366c1adc510 473 //1->10170,2->7500
kenken0721 2:6140746e19b1 474 update.detach();
kenken0721 3:0366c1adc510 475 float pos1 = initPos1;
kenken0721 3:0366c1adc510 476 float pos2 = initPos2;
kenken0721 3:0366c1adc510 477 float sum1 = float((10170.0 - float(initPos1))/1000.0);
kenken0721 3:0366c1adc510 478 float sum2 = float((7500.0 - float(initPos2))/1000.0);
kenken0721 2:6140746e19b1 479 for(int i=0;i<1000;i++){
kenken0721 2:6140746e19b1 480 pos1 = pos1 + sum1;
kenken0721 2:6140746e19b1 481 pos2 = pos2 + sum2;
kenken0721 2:6140746e19b1 482 servo[0].move(4,int(pos1));
kenken0721 2:6140746e19b1 483 servo[0].move(5,int(pos2));
kenken0721 2:6140746e19b1 484 wait(0.0008);
kenken0721 2:6140746e19b1 485 }
kenken0721 2:6140746e19b1 486 }
kenken0721 2:6140746e19b1 487
kenken0721 1:5fb6895ad8e6 488 //---------------------------------------------------------------
kenken0721 0:ffe56c419abd 489
kenken0721 0:ffe56c419abd 490 int main() {
kenken0721 3:0366c1adc510 491 //Led = 0;
kenken0721 2:6140746e19b1 492 ropeTouch.pulsewidth_us(2400);
kenken0721 0:ffe56c419abd 493 for(int i=0;i<4;i++){
kenken0721 1:5fb6895ad8e6 494 servo[i].init();
kenken0721 0:ffe56c419abd 495 }
kenken0721 3:0366c1adc510 496 //------赤、青モード決定-------------
kenken0721 3:0366c1adc510 497 color.mode(PullUp);
kenken0721 3:0366c1adc510 498 int colorCount = 0;
kenken0721 3:0366c1adc510 499 for(int i=0;i<100;i++){
kenken0721 3:0366c1adc510 500 if(color.read() == true){
kenken0721 3:0366c1adc510 501 colorCount++;
kenken0721 3:0366c1adc510 502 }else{
kenken0721 3:0366c1adc510 503 colorCount--;
kenken0721 3:0366c1adc510 504 }
kenken0721 3:0366c1adc510 505 if(colorCount >= 0)
kenken0721 3:0366c1adc510 506 initRed();
kenken0721 3:0366c1adc510 507 else
kenken0721 3:0366c1adc510 508 initBlue();
kenken0721 3:0366c1adc510 509 }
kenken0721 3:0366c1adc510 510 //--------------------------------
kenken0721 2:6140746e19b1 511 //------平地と坂のモード決定----------
kenken0721 2:6140746e19b1 512 sw.mode(PullUp);
kenken0721 2:6140746e19b1 513 int modeCount = 0;
kenken0721 2:6140746e19b1 514 while(1){
kenken0721 2:6140746e19b1 515 if(sw.read() == true){
kenken0721 2:6140746e19b1 516 modeCount++;
kenken0721 2:6140746e19b1 517 }else{
kenken0721 2:6140746e19b1 518 modeCount--;
kenken0721 2:6140746e19b1 519 }
kenken0721 2:6140746e19b1 520 if(modeCount >= 100){
kenken0721 2:6140746e19b1 521 modeFlag = true;
kenken0721 2:6140746e19b1 522 break;
kenken0721 2:6140746e19b1 523 }else if(modeCount <= -100){
kenken0721 2:6140746e19b1 524 modeFlag = false;
kenken0721 2:6140746e19b1 525 break;
kenken0721 2:6140746e19b1 526 }
kenken0721 2:6140746e19b1 527 }
kenken0721 2:6140746e19b1 528 //--------------------------------
kenken0721 1:5fb6895ad8e6 529
kenken0721 2:6140746e19b1 530 //---------平地モード---------------
kenken0721 2:6140746e19b1 531 if(modeFlag == true){
kenken0721 2:6140746e19b1 532 retry.mode(PullUp);
kenken0721 3:0366c1adc510 533 servo[0].move(4,initLack1);//持ち上げ機構初期位置へ
kenken0721 2:6140746e19b1 534 wait(0.1);
kenken0721 3:0366c1adc510 535 servo[0].move(5,initLack2);//持ち上げ機構初期位置へ
kenken0721 2:6140746e19b1 536 initPos();//スタート姿勢
kenken0721 2:6140746e19b1 537 update.attach(&siteUpdate,0.02);//20msごとに座標値更新
kenken0721 2:6140746e19b1 538 //wait(20.0);
kenken0721 2:6140746e19b1 539 setSite(0, 200, 30, 160);
kenken0721 2:6140746e19b1 540 setSite(1, 200, 160, 160);
kenken0721 2:6140746e19b1 541 setSite(2, 200, -30, 170);
kenken0721 2:6140746e19b1 542 setSite(3, 200, -160, 170);
kenken0721 1:5fb6895ad8e6 543 allMotionWait();
kenken0721 2:6140746e19b1 544 //while(1){};
kenken0721 3:0366c1adc510 545 wait(5.0);
kenken0721 2:6140746e19b1 546 //----------ゲルゲ受け取り待機-------------
kenken0721 2:6140746e19b1 547 bool flag = false;
kenken0721 2:6140746e19b1 548 int count = 0;
kenken0721 2:6140746e19b1 549 while(flag == false){
kenken0721 3:0366c1adc510 550 if(int(start.read()) == false){
kenken0721 2:6140746e19b1 551 count++;
kenken0721 2:6140746e19b1 552 }else{
kenken0721 2:6140746e19b1 553 count--;
kenken0721 2:6140746e19b1 554 }
kenken0721 2:6140746e19b1 555 if(count >= 5000){
kenken0721 2:6140746e19b1 556 flag = true;
kenken0721 2:6140746e19b1 557 }else if(count <= 0){
kenken0721 2:6140746e19b1 558 flag = false;
kenken0721 2:6140746e19b1 559 count = 0;
kenken0721 2:6140746e19b1 560 }
kenken0721 2:6140746e19b1 561 }
kenken0721 2:6140746e19b1 562 wait(0.2);
kenken0721 3:0366c1adc510 563 //Led = 1;
kenken0721 2:6140746e19b1 564 //-------スタート--------------------
kenken0721 2:6140746e19b1 565 float levelPosTest[8] = {200,30,-160,160,170,145,60,70};
kenken0721 3:0366c1adc510 566 float levelSpeTest[2] = {9.0,10};
kenken0721 2:6140746e19b1 567 if(retry.read() == false){
kenken0721 2:6140746e19b1 568 //段差前
kenken0721 3:0366c1adc510 569 forwardTrot(10,levelPosTest,levelSpeTest,flatCurve1);
kenken0721 3:0366c1adc510 570 forwardTrot(2,levelPosTest,levelSpeTest,flatCurve2);
kenken0721 3:0366c1adc510 571 //段差
kenken0721 2:6140746e19b1 572 stepOver();
kenken0721 2:6140746e19b1 573 forwardTrot(3,levelPosTest,levelSpeTest,0);
kenken0721 2:6140746e19b1 574 moveSpeed = 6;
kenken0721 2:6140746e19b1 575 setSite(0, 200, 30, 160);
kenken0721 2:6140746e19b1 576 setSite(1, 200, 160, 160);
kenken0721 2:6140746e19b1 577 setSite(2, 200, -30, 170);
kenken0721 2:6140746e19b1 578 setSite(3, 200, -160, 170);
kenken0721 1:5fb6895ad8e6 579 allMotionWait();
kenken0721 2:6140746e19b1 580 while(1){
kenken0721 2:6140746e19b1 581 if(retry.read() == true){
kenken0721 2:6140746e19b1 582 break;
kenken0721 2:6140746e19b1 583 }
kenken0721 2:6140746e19b1 584 }
kenken0721 2:6140746e19b1 585 wait(0.5);
kenken0721 2:6140746e19b1 586 ropeTouch.pulsewidth_us(600);
kenken0721 2:6140746e19b1 587 forwardTrot(4,levelPosTest,levelSpeTest,-35);
kenken0721 2:6140746e19b1 588 //forwardTrot(2,levelPosTest,levelSpeTest,4);
kenken0721 2:6140746e19b1 589 ropeOver();
kenken0721 2:6140746e19b1 590 float levelPosTest2[8] = {200,60,-140,160,170,80,70,80};
kenken0721 2:6140746e19b1 591 rightMove(2,levelPosTest2,levelSpeTest,85);
kenken0721 2:6140746e19b1 592 forwardTrot(1,levelPosTest,levelSpeTest,10);
kenken0721 2:6140746e19b1 593 ropeFlag = false;
kenken0721 2:6140746e19b1 594 ropeOver();
kenken0721 2:6140746e19b1 595 forwardTrot(7,levelPosTest,levelSpeTest,0);
kenken0721 2:6140746e19b1 596 }
kenken0721 2:6140746e19b1 597 else{
kenken0721 2:6140746e19b1 598 //forwardTrot(2,levelPosTest,levelSpeTest,17);
kenken0721 2:6140746e19b1 599 ropeTouch.pulsewidth_us(600);
kenken0721 2:6140746e19b1 600 forwardTrot(4,levelPosTest,levelSpeTest,-35);
kenken0721 2:6140746e19b1 601 //forwardTrot(2,levelPosTest,levelSpeTest,4);
kenken0721 2:6140746e19b1 602 ropeOver();
kenken0721 2:6140746e19b1 603 float levelPosTest2[8] = {200,60,-140,160,170,80,70,80};
kenken0721 2:6140746e19b1 604 rightMove(2,levelPosTest2,levelSpeTest,85);
kenken0721 2:6140746e19b1 605 forwardTrot(1,levelPosTest,levelSpeTest,10);
kenken0721 2:6140746e19b1 606 ropeFlag = false;
kenken0721 2:6140746e19b1 607 ropeOver();
kenken0721 2:6140746e19b1 608 forwardTrot(7,levelPosTest,levelSpeTest,0);
kenken0721 2:6140746e19b1 609 }
kenken0721 2:6140746e19b1 610
kenken0721 2:6140746e19b1 611 //----------------------------------------------------------
kenken0721 2:6140746e19b1 612 wait(10);
kenken0721 2:6140746e19b1 613 }
kenken0721 2:6140746e19b1 614 //---------坂モード-----------------
kenken0721 2:6140746e19b1 615 else{
kenken0721 3:0366c1adc510 616 servo[0].move(4,initLack1);//持ち上げ機構初期位置へ
kenken0721 2:6140746e19b1 617 wait(0.1);
kenken0721 3:0366c1adc510 618 servo[0].move(5,slopeLack2);//持ち上げ機構初期位置へ
kenken0721 2:6140746e19b1 619
kenken0721 2:6140746e19b1 620 initPos();//スタート姿勢
kenken0721 2:6140746e19b1 621 update.attach(&siteUpdate,0.02);//20msごとに座標値更新
kenken0721 2:6140746e19b1 622
kenken0721 2:6140746e19b1 623 wait(1.0);
kenken0721 2:6140746e19b1 624 setSite(0, 200, 30, 150);
kenken0721 2:6140746e19b1 625 setSite(1, 200, 170, 150);
kenken0721 3:0366c1adc510 626 setSite(2, 200, -30, 180);//-70
kenken0721 3:0366c1adc510 627 setSite(3, 200, -30, 180);//-70
kenken0721 0:ffe56c419abd 628 allMotionWait();
kenken0721 3:0366c1adc510 629
kenken0721 2:6140746e19b1 630 update.detach();
kenken0721 2:6140746e19b1 631 //----------非接触合図待機-----------------
kenken0721 2:6140746e19b1 632 trig = 0;
kenken0721 2:6140746e19b1 633 echo.rise(&pulseRise);
kenken0721 2:6140746e19b1 634 echo.fall(&pulseFall);
kenken0721 2:6140746e19b1 635 bool flag = false;
kenken0721 2:6140746e19b1 636 int count = 0;
kenken0721 2:6140746e19b1 637 timer.reset();
kenken0721 2:6140746e19b1 638 while(flag == false){
kenken0721 2:6140746e19b1 639 trig = 0;
kenken0721 2:6140746e19b1 640 wait_us(1);
kenken0721 2:6140746e19b1 641 trig = 1;
kenken0721 2:6140746e19b1 642 wait_us(11);
kenken0721 2:6140746e19b1 643 trig = 0;
kenken0721 2:6140746e19b1 644 wait_us(1);
kenken0721 2:6140746e19b1 645 wait(0.01);
kenken0721 2:6140746e19b1 646 if(Distance <= 250)
kenken0721 2:6140746e19b1 647 count++;
kenken0721 2:6140746e19b1 648 if(count >= 30)
kenken0721 2:6140746e19b1 649 flag = true;
kenken0721 2:6140746e19b1 650 }
kenken0721 2:6140746e19b1 651 echo.rise(NULL);
kenken0721 2:6140746e19b1 652 echo.fall(NULL);
kenken0721 3:0366c1adc510 653 //Led = 1;
kenken0721 2:6140746e19b1 654 servo[0].speed(4,40);
kenken0721 2:6140746e19b1 655 wait(0.005);
kenken0721 3:0366c1adc510 656 servo[0].move(4,slopeLack1);
kenken0721 2:6140746e19b1 657 wait(0.1);
kenken0721 3:0366c1adc510 658 //while(1){}
kenken0721 2:6140746e19b1 659 update.attach(&siteUpdate,0.02);//20msごとに座標値更新
kenken0721 2:6140746e19b1 660 //-----スタート-------------------
kenken0721 2:6140746e19b1 661
kenken0721 2:6140746e19b1 662 moveSpeed = 8;
kenken0721 2:6140746e19b1 663 setSite(2, 200, 0, 50);
kenken0721 2:6140746e19b1 664 allMotionWait();
kenken0721 2:6140746e19b1 665 setSite(2, 200, -200, 50);
kenken0721 2:6140746e19b1 666 allMotionWait();
kenken0721 2:6140746e19b1 667 setSite(2, 200, -200, 160);
kenken0721 2:6140746e19b1 668 allMotionWait();
kenken0721 2:6140746e19b1 669 setSite(3, 200, 0, 50);
kenken0721 2:6140746e19b1 670 allMotionWait();
kenken0721 2:6140746e19b1 671 setSite(3, 200, -120, 50);
kenken0721 2:6140746e19b1 672 allMotionWait();
kenken0721 2:6140746e19b1 673 setSite(3, 200, -90, 160);
kenken0721 0:ffe56c419abd 674 allMotionWait();
kenken0721 0:ffe56c419abd 675
kenken0721 0:ffe56c419abd 676
kenken0721 2:6140746e19b1 677 float slopePosTest[8] = {210,170,-20,150,130,-130,110,50};
kenken0721 3:0366c1adc510 678 float slopeSpeTest[8] = {7,7};
kenken0721 0:ffe56c419abd 679
kenken0721 2:6140746e19b1 680 float lackUpPos[8] = {170,150,-150,390,390,0,0,0};
kenken0721 2:6140746e19b1 681 float lackUpSpe[2] = {4.3,4};
kenken0721 3:0366c1adc510 682 backTrot(4,slopePosTest,slopeSpeTest,25);
kenken0721 3:0366c1adc510 683 backTrot(6,slopePosTest,slopeSpeTest,2);
kenken0721 0:ffe56c419abd 684
kenken0721 2:6140746e19b1 685 standUp(lackUpPos,lackUpSpe);
kenken0721 3:0366c1adc510 686 lackUp(slopeLack1,slopeLack2);
kenken0721 0:ffe56c419abd 687 }
kenken0721 0:ffe56c419abd 688 }