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

nhk2019

Dependencies:   mbed kondoSerialServo

main.cpp@2:6140746e19b1, 2019-05-01 (annotated)

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