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

nhk2019

Dependencies:   mbed kondoSerialServo

main.cpp@4:e66bd933bafa, 2019-05-24 (annotated)

Committer:
kenken0721
Date:
Fri May 24 02:25:41 2019 +0000
Revision:
4:e66bd933bafa
Parent:
3:0366c1adc510 
robocon

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 4:e66bd933bafa 34 int ropeCurve1;
kenken0721 4:e66bd933bafa 35 int ropeCurve2;
kenken0721 4:e66bd933bafa 36 int ropeClab1;
kenken0721 4:e66bd933bafa 37 int ropeClab2;
kenken0721 4:e66bd933bafa 38 int clabCount;
kenken0721 3:0366c1adc510 39 //-----------------------------
kenken0721 2:6140746e19b1 40
kenken0721 1:5fb6895ad8e6 41 const float PI = 3.1415926;
kenken0721 2:6140746e19b1 42 bool modeFlag = true;
kenken0721 2:6140746e19b1 43 bool ropeFlag = false;
kenken0721 2:6140746e19b1 44 bool ropeSign = false;
kenken0721 2:6140746e19b1 45 int ropeCount = 0;
kenken0721 0:ffe56c419abd 46
kenken0721 0:ffe56c419abd 47 //---各足の長さ----
kenken0721 0:ffe56c419abd 48 const float L1 = 76;
kenken0721 1:5fb6895ad8e6 49 const float L2 = 168;
kenken0721 1:5fb6895ad8e6 50 const float L3 = 252;
kenken0721 0:ffe56c419abd 51
kenken0721 2:6140746e19b1 52 //移動速度
kenken0721 1:5fb6895ad8e6 53 float moveSpeed = 1;
kenken0721 0:ffe56c419abd 54 //現在の座標
kenken0721 0:ffe56c419abd 55 float currentSite[4][3];
kenken0721 0:ffe56c419abd 56 //次の座標
kenken0721 0:ffe56c419abd 57 float nextSite[4][3];
kenken0721 0:ffe56c419abd 58 //値更新用スピード
kenken0721 0:ffe56c419abd 59 float tempSpeed[4][3];
kenken0721 0:ffe56c419abd 60 //現在の角度
kenken0721 0:ffe56c419abd 61 float currentAngle[4][3];
kenken0721 0:ffe56c419abd 62 //現在の周波数
kenken0721 0:ffe56c419abd 63 int currentPulse[4][3];
kenken0721 0:ffe56c419abd 64
kenken0721 0:ffe56c419abd 65 float fmap(float x, float in_min, float in_max, float out_min, float out_max){
kenken0721 0:ffe56c419abd 66 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
kenken0721 0:ffe56c419abd 67 }
kenken0721 0:ffe56c419abd 68
kenken0721 3:0366c1adc510 69 //----------色に合わせて初期化-------------
kenken0721 3:0366c1adc510 70 void initRed(){//左側
kenken0721 3:0366c1adc510 71 initLack1 = 7600;
kenken0721 3:0366c1adc510 72 initLack2 = 3500;
kenken0721 4:e66bd933bafa 73 flatCurve1 = 0;
kenken0721 3:0366c1adc510 74 flatCurve2 = 15;
kenken0721 4:e66bd933bafa 75 slopeLack1 = 9700;//9700
kenken0721 4:e66bd933bafa 76 slopeLack2 = 3800;
kenken0721 4:e66bd933bafa 77 ropeCurve1 = -20;
kenken0721 4:e66bd933bafa 78 ropeCurve2 = -50;
kenken0721 4:e66bd933bafa 79 ropeClab1 = 70;
kenken0721 4:e66bd933bafa 80 ropeClab2 = 1;
kenken0721 4:e66bd933bafa 81 clabCount = 2;
kenken0721 3:0366c1adc510 82 }
kenken0721 3:0366c1adc510 83
kenken0721 3:0366c1adc510 84 void initBlue(){//右側
kenken0721 3:0366c1adc510 85 initLack1 = 7600;
kenken0721 3:0366c1adc510 86 initLack2 = 11500;
kenken0721 3:0366c1adc510 87 flatCurve1 = -7;
kenken0721 3:0366c1adc510 88 flatCurve2 = -17;
kenken0721 3:0366c1adc510 89 slopeLack1 = 9700;
kenken0721 4:e66bd933bafa 90 slopeLack2 = 11100;
kenken0721 4:e66bd933bafa 91 ropeCurve1 = 15;
kenken0721 4:e66bd933bafa 92 ropeCurve2 = 50;
kenken0721 4:e66bd933bafa 93 ropeClab1 = -40;
kenken0721 4:e66bd933bafa 94 ropeClab2 = 0;
kenken0721 4:e66bd933bafa 95 clabCount = 2;
kenken0721 3:0366c1adc510 96 }
kenken0721 3:0366c1adc510 97
kenken0721 3:0366c1adc510 98 //--------------------------------------
kenken0721 2:6140746e19b1 99 //----------超音波センサー用関数------------
kenken0721 2:6140746e19b1 100 float Distance = 0;
kenken0721 2:6140746e19b1 101 void pulseRise(){
kenken0721 2:6140746e19b1 102 timer.start();
kenken0721 2:6140746e19b1 103 }
kenken0721 2:6140746e19b1 104 void pulseFall(){
kenken0721 2:6140746e19b1 105 timer.stop();
kenken0721 2:6140746e19b1 106 Distance = timer.read_us();
kenken0721 2:6140746e19b1 107 timer.reset();
kenken0721 2:6140746e19b1 108 }
kenken0721 2:6140746e19b1 109 //--------------------------------------
kenken0721 0:ffe56c419abd 110 //座標指定
kenken0721 1:5fb6895ad8e6 111 void setSite(int leg, float x, float y, float z){
kenken0721 0:ffe56c419abd 112 float length[3] = {0.0,0.0,0.0};
kenken0721 0:ffe56c419abd 113 length[0] = x - currentSite[leg][0];
kenken0721 0:ffe56c419abd 114 length[1] = y - currentSite[leg][1];
kenken0721 0:ffe56c419abd 115 length[2] = z - currentSite[leg][2];
kenken0721 0:ffe56c419abd 116 float L = sqrt(pow(length[0],2)+pow(length[1],2)+pow(length[2],2));
kenken0721 0:ffe56c419abd 117 tempSpeed[leg][0] = (length[0]/L)*moveSpeed;
kenken0721 0:ffe56c419abd 118 tempSpeed[leg][1] = (length[1]/L)*moveSpeed;
kenken0721 0:ffe56c419abd 119 tempSpeed[leg][2] = (length[2]/L)*moveSpeed;
kenken0721 0:ffe56c419abd 120 nextSite[leg][0] = x;
kenken0721 0:ffe56c419abd 121 nextSite[leg][1] = y;
kenken0721 0:ffe56c419abd 122 nextSite[leg][2] = z;
kenken0721 0:ffe56c419abd 123 }
kenken0721 0:ffe56c419abd 124
kenken0721 0:ffe56c419abd 125 void toAngle(int leg){
kenken0721 0:ffe56c419abd 126 float x = currentSite[leg][0];
kenken0721 0:ffe56c419abd 127 float y = currentSite[leg][1];
kenken0721 0:ffe56c419abd 128 float z = currentSite[leg][2];
kenken0721 0:ffe56c419abd 129 float L = sqrt((x*x)+(y*y));
kenken0721 0:ffe56c419abd 130 float w = L - L1;
kenken0721 0:ffe56c419abd 131 float v = sqrt((w*w)+(z*z));
kenken0721 0:ffe56c419abd 132 float alpha = atan2(z,w) - acos(((v*v)+(L2*L2)-(L3*L3))/(2*L2*v));
kenken0721 0:ffe56c419abd 133 float beta = PI - acos(((L2*L2) + (L3*L3) - (v*v))/(2*L2*L3));
kenken0721 0:ffe56c419abd 134 float gamma = atan2(y, x);
kenken0721 0:ffe56c419abd 135 currentAngle[leg][0] = gamma = (gamma / PI * 180);
kenken0721 0:ffe56c419abd 136 currentAngle[leg][1] = alpha = (alpha / PI * 180);
kenken0721 0:ffe56c419abd 137 currentAngle[leg][2] = beta = (beta / PI * 180);
kenken0721 0:ffe56c419abd 138 }
kenken0721 0:ffe56c419abd 139
kenken0721 1:5fb6895ad8e6 140
kenken0721 1:5fb6895ad8e6 141 void toPulse(){
kenken0721 1:5fb6895ad8e6 142 //右前足
kenken0721 2:6140746e19b1 143 currentPulse[0][0] = int(fmap(currentAngle[0][0],-135,135,11500,3500));
kenken0721 4:e66bd933bafa 144 currentPulse[0][1] = int(fmap(currentAngle[0][1],-135,135,11500,3500)+100);//+200
kenken0721 4:e66bd933bafa 145 currentPulse[0][2] = int(fmap(currentAngle[0][2],0,270,3500,11500));//0
kenken0721 1:5fb6895ad8e6 146 //左前足
kenken0721 2:6140746e19b1 147 currentPulse[1][0] = int(fmap(currentAngle[1][0],-135,135,3500,11500)-100);
kenken0721 4:e66bd933bafa 148 currentPulse[1][1] = int(fmap(currentAngle[1][1],-135,135,11500,3500)-100);//-200
kenken0721 4:e66bd933bafa 149 currentPulse[1][2] = int(fmap(currentAngle[1][2],0,270,3500,11500)-400);//-350
kenken0721 1:5fb6895ad8e6 150 //右後足
kenken0721 4:e66bd933bafa 151 currentPulse[2][0] = int(fmap(currentAngle[2][0],-135,135,11500,3500)+500);
kenken0721 4:e66bd933bafa 152 currentPulse[2][1] = int(fmap(currentAngle[2][1],-135,135,11500,3500));//0
kenken0721 4:e66bd933bafa 153 currentPulse[2][2] = int(fmap(currentAngle[2][2],0,270,3500,11500)-100);//0
kenken0721 1:5fb6895ad8e6 154 //左後足
kenken0721 2:6140746e19b1 155 currentPulse[3][0] = int(fmap(currentAngle[3][0],-135,135,3500,11500)+100);
kenken0721 4:e66bd933bafa 156 currentPulse[3][1] = int(fmap(currentAngle[3][1],-135,135,11500,3500));//0
kenken0721 4:e66bd933bafa 157 currentPulse[3][2] = int(fmap(currentAngle[3][2],0,270,3500,11500)-100);//0
kenken0721 1:5fb6895ad8e6 158
kenken0721 1:5fb6895ad8e6 159 for(int i=0;i<3;i++){
kenken0721 1:5fb6895ad8e6 160 for(int j=0;j<4;j++){
kenken0721 1:5fb6895ad8e6 161 servo[j].move(i+1,int(currentPulse[j][i]));
kenken0721 1:5fb6895ad8e6 162 wait_us(100);
kenken0721 0:ffe56c419abd 163 }
kenken0721 1:5fb6895ad8e6 164 wait_us(400);
kenken0721 0:ffe56c419abd 165 }
kenken0721 0:ffe56c419abd 166 }
kenken0721 0:ffe56c419abd 167
kenken0721 0:ffe56c419abd 168 void siteUpdate(){
kenken0721 2:6140746e19b1 169 if(ropeSign == true){
kenken0721 4:e66bd933bafa 170 if(touch.read() == 0){
kenken0721 4:e66bd933bafa 171 ropeCount++;
kenken0721 4:e66bd933bafa 172 }else{
kenken0721 4:e66bd933bafa 173 ropeCount--;
kenken0721 4:e66bd933bafa 174 }
kenken0721 4:e66bd933bafa 175 if(ropeCount >= 25){
kenken0721 4:e66bd933bafa 176 ropeFlag = true;
kenken0721 4:e66bd933bafa 177 ropeCount = 0;
kenken0721 4:e66bd933bafa 178 }else if(ropeCount <= 0){
kenken0721 4:e66bd933bafa 179 ropeFlag = false;
kenken0721 4:e66bd933bafa 180 ropeCount = 0;
kenken0721 4:e66bd933bafa 181 }
kenken0721 2:6140746e19b1 182 }
kenken0721 0:ffe56c419abd 183 for(int i=0;i<4;i++){
kenken0721 0:ffe56c419abd 184 for(int j=0;j<3;j++){
kenken0721 0:ffe56c419abd 185 float siteLength = abs(currentSite[i][j] - nextSite[i][j]);
kenken0721 0:ffe56c419abd 186 if( siteLength >= abs(tempSpeed[i][j]))
kenken0721 0:ffe56c419abd 187 currentSite[i][j] += tempSpeed[i][j];
kenken0721 0:ffe56c419abd 188 else
kenken0721 0:ffe56c419abd 189 currentSite[i][j] = nextSite[i][j];
kenken0721 0:ffe56c419abd 190 }
kenken0721 0:ffe56c419abd 191 toAngle(i);
kenken0721 0:ffe56c419abd 192 }
kenken0721 1:5fb6895ad8e6 193 toPulse();
kenken0721 0:ffe56c419abd 194 }
kenken0721 0:ffe56c419abd 195
kenken0721 0:ffe56c419abd 196 void motionWait(int leg){
kenken0721 0:ffe56c419abd 197 bool flag = false;
kenken0721 0:ffe56c419abd 198 while(flag == false){
kenken0721 1:5fb6895ad8e6 199 wait_us(1);
kenken0721 0:ffe56c419abd 200 if(currentSite[leg][0] == nextSite[leg][0]){
kenken0721 0:ffe56c419abd 201 if(currentSite[leg][1] == nextSite[leg][1]){
kenken0721 0:ffe56c419abd 202 if(currentSite[leg][2] == nextSite[leg][2]){
kenken0721 0:ffe56c419abd 203 flag = true;
kenken0721 0:ffe56c419abd 204 }
kenken0721 0:ffe56c419abd 205 }
kenken0721 0:ffe56c419abd 206 }
kenken0721 0:ffe56c419abd 207 }
kenken0721 0:ffe56c419abd 208 }
kenken0721 0:ffe56c419abd 209
kenken0721 0:ffe56c419abd 210 //すべての足が動き終わるまで待機
kenken0721 0:ffe56c419abd 211 void allMotionWait(){
kenken0721 0:ffe56c419abd 212 for(int i=0;i<4;i++){
kenken0721 0:ffe56c419abd 213 motionWait(i);
kenken0721 0:ffe56c419abd 214 }
kenken0721 0:ffe56c419abd 215 }
kenken0721 1:5fb6895ad8e6 216 //------------------歩行モーション------------------------------------
kenken0721 0:ffe56c419abd 217 //静歩行
kenken0721 2:6140746e19b1 218
kenken0721 1:5fb6895ad8e6 219 //座位
kenken0721 1:5fb6895ad8e6 220 void standDown(float *pos,float *speed){
kenken0721 1:5fb6895ad8e6 221 moveSpeed = speed[0];
kenken0721 1:5fb6895ad8e6 222 setSite(0, pos[0], pos[1], pos[3]);
kenken0721 1:5fb6895ad8e6 223 setSite(1, pos[0], pos[1], pos[3]);
kenken0721 1:5fb6895ad8e6 224 setSite(2, pos[0], pos[2], pos[4]);
kenken0721 1:5fb6895ad8e6 225 setSite(3, pos[0], pos[2], pos[4]);
kenken0721 1:5fb6895ad8e6 226 allMotionWait();
kenken0721 1:5fb6895ad8e6 227 }
kenken0721 1:5fb6895ad8e6 228 //直立姿勢
kenken0721 1:5fb6895ad8e6 229 void standUp(float *pos,float *speed){
kenken0721 1:5fb6895ad8e6 230 moveSpeed = speed[0];
kenken0721 1:5fb6895ad8e6 231 setSite(0, pos[0], pos[1], pos[3]);
kenken0721 1:5fb6895ad8e6 232 setSite(1, pos[0], pos[1], pos[3]);
kenken0721 1:5fb6895ad8e6 233 setSite(2, pos[0], pos[2], pos[4]);
kenken0721 1:5fb6895ad8e6 234 setSite(3, pos[0], pos[2], pos[4]);
kenken0721 1:5fb6895ad8e6 235 allMotionWait();
kenken0721 0:ffe56c419abd 236 }
kenken0721 4:e66bd933bafa 237 void Turn(unsigned int count,float *pos, float *speed,bool direction){
kenken0721 1:5fb6895ad8e6 238 moveSpeed = speed[0];
kenken0721 4:e66bd933bafa 239 float stepRY = 0;
kenken0721 4:e66bd933bafa 240 float stepLY = 0;
kenken0721 4:e66bd933bafa 241 if(direction == 0){
kenken0721 4:e66bd933bafa 242 stepRY = pos[5];
kenken0721 4:e66bd933bafa 243 }else{
kenken0721 4:e66bd933bafa 244 stepLY = pos[5];
kenken0721 0:ffe56c419abd 245 }
kenken0721 0:ffe56c419abd 246 while(count-- > 0){
kenken0721 1:5fb6895ad8e6 247 //0,3
kenken0721 1:5fb6895ad8e6 248 setSite(0, pos[0], pos[1] + stepRY, pos[3]-pos[6]);
kenken0721 1:5fb6895ad8e6 249 setSite(1, pos[0], pos[1], pos[3]);
kenken0721 1:5fb6895ad8e6 250 setSite(2, pos[0], pos[2], pos[4]);
kenken0721 1:5fb6895ad8e6 251 setSite(3, pos[0], pos[2] + stepLY, pos[4]-pos[7]);
kenken0721 1:5fb6895ad8e6 252 allMotionWait();
kenken0721 1:5fb6895ad8e6 253 setSite(0, pos[0], pos[1] + stepRY, pos[3]);
kenken0721 1:5fb6895ad8e6 254 setSite(3, pos[0], pos[2] + stepLY, pos[4]);
kenken0721 0:ffe56c419abd 255 allMotionWait();
kenken0721 1:5fb6895ad8e6 256 //1,2
kenken0721 1:5fb6895ad8e6 257 setSite(0, pos[0], pos[1], pos[3]);
kenken0721 1:5fb6895ad8e6 258 setSite(1, pos[0], pos[1] + stepLY, pos[3]-pos[6]);
kenken0721 1:5fb6895ad8e6 259 setSite(2, pos[0], pos[2] + stepRY, pos[4]-pos[7]);
kenken0721 1:5fb6895ad8e6 260 setSite(3, pos[0], pos[2], pos[4]);
kenken0721 0:ffe56c419abd 261 allMotionWait();
kenken0721 1:5fb6895ad8e6 262 setSite(1, pos[0], pos[1] + stepLY, pos[3]);
kenken0721 1:5fb6895ad8e6 263 setSite(2, pos[0], pos[2] + stepRY, pos[4]);
kenken0721 4:e66bd933bafa 264 allMotionWait();
kenken0721 0:ffe56c419abd 265 }
kenken0721 0:ffe56c419abd 266 }
kenken0721 4:e66bd933bafa 267 //トロット歩容
kenken0721 4:e66bd933bafa 268 void forwardTrot(unsigned int count, float *pos, float *speed, float turn, bool alta = 1){
kenken0721 4:e66bd933bafa 269 float stepRY = pos[5];
kenken0721 4:e66bd933bafa 270 float stepLY = pos[5];
kenken0721 4:e66bd933bafa 271 moveSpeed = speed[0];
kenken0721 4:e66bd933bafa 272 if(turn > 0){
kenken0721 4:e66bd933bafa 273 stepRY += turn;
kenken0721 4:e66bd933bafa 274 stepLY -= turn;
kenken0721 4:e66bd933bafa 275 }else if(turn < 0){
kenken0721 4:e66bd933bafa 276 stepRY += turn;
kenken0721 4:e66bd933bafa 277 stepLY -= turn;
kenken0721 4:e66bd933bafa 278 }
kenken0721 4:e66bd933bafa 279 while(count-- > 0){
kenken0721 4:e66bd933bafa 280 if(alta == 1){
kenken0721 4:e66bd933bafa 281 //0,3
kenken0721 4:e66bd933bafa 282 setSite(0, pos[0], pos[1] + stepRY, pos[3]-pos[6]);
kenken0721 4:e66bd933bafa 283 setSite(1, pos[0], pos[1], pos[3]);
kenken0721 4:e66bd933bafa 284 setSite(2, pos[0], pos[2], pos[4]);
kenken0721 4:e66bd933bafa 285 setSite(3, pos[0], pos[2] + stepLY, pos[4]-pos[7]);
kenken0721 4:e66bd933bafa 286 allMotionWait();
kenken0721 4:e66bd933bafa 287 setSite(0, pos[0], pos[1] + stepRY, pos[3]);
kenken0721 4:e66bd933bafa 288 setSite(3, pos[0], pos[2] + stepLY, pos[4]);
kenken0721 4:e66bd933bafa 289 allMotionWait();
kenken0721 4:e66bd933bafa 290 //1,2
kenken0721 4:e66bd933bafa 291 setSite(0, pos[0], pos[1], pos[3]);
kenken0721 4:e66bd933bafa 292 setSite(1, pos[0], pos[1] + stepLY, pos[3]-pos[6]);
kenken0721 4:e66bd933bafa 293 setSite(2, pos[0], pos[2] + stepRY, pos[4]-pos[7]);
kenken0721 4:e66bd933bafa 294 setSite(3, pos[0], pos[2], pos[4]);
kenken0721 4:e66bd933bafa 295 allMotionWait();
kenken0721 4:e66bd933bafa 296 setSite(1, pos[0], pos[1] + stepLY, pos[3]);
kenken0721 4:e66bd933bafa 297 setSite(2, pos[0], pos[2] + stepRY, pos[4]);
kenken0721 4:e66bd933bafa 298 allMotionWait();
kenken0721 4:e66bd933bafa 299 }else{
kenken0721 4:e66bd933bafa 300 //1,2
kenken0721 4:e66bd933bafa 301 setSite(0, pos[0], pos[1], pos[3]);
kenken0721 4:e66bd933bafa 302 setSite(1, pos[0], pos[1] + stepLY, pos[3]-pos[6]);
kenken0721 4:e66bd933bafa 303 setSite(2, pos[0], pos[2] + stepRY, pos[4]-pos[7]);
kenken0721 4:e66bd933bafa 304 setSite(3, pos[0], pos[2], pos[4]);
kenken0721 4:e66bd933bafa 305 allMotionWait();
kenken0721 4:e66bd933bafa 306 setSite(1, pos[0], pos[1] + stepLY, pos[3]);
kenken0721 4:e66bd933bafa 307 setSite(2, pos[0], pos[2] + stepRY, pos[4]);
kenken0721 4:e66bd933bafa 308 allMotionWait();
kenken0721 4:e66bd933bafa 309 //0,3
kenken0721 4:e66bd933bafa 310 setSite(0, pos[0], pos[1] + stepRY, pos[3]-pos[6]);
kenken0721 4:e66bd933bafa 311 setSite(1, pos[0], pos[1], pos[3]);
kenken0721 4:e66bd933bafa 312 setSite(2, pos[0], pos[2], pos[4]);
kenken0721 4:e66bd933bafa 313 setSite(3, pos[0], pos[2] + stepLY, pos[4]-pos[7]);
kenken0721 4:e66bd933bafa 314 allMotionWait();
kenken0721 4:e66bd933bafa 315 setSite(0, pos[0], pos[1] + stepRY, pos[3]);
kenken0721 4:e66bd933bafa 316 setSite(3, pos[0], pos[2] + stepLY, pos[4]);
kenken0721 4:e66bd933bafa 317 allMotionWait();
kenken0721 4:e66bd933bafa 318 }
kenken0721 4:e66bd933bafa 319 }
kenken0721 4:e66bd933bafa 320 }
kenken0721 4:e66bd933bafa 321 bool forwardTrot2(float *pos, float *speed, float turn){
kenken0721 4:e66bd933bafa 322 float stepRY = pos[5];
kenken0721 4:e66bd933bafa 323 float stepLY = pos[5];
kenken0721 4:e66bd933bafa 324 bool alta = 0;
kenken0721 4:e66bd933bafa 325 moveSpeed = speed[0];
kenken0721 4:e66bd933bafa 326 if(turn > 0){
kenken0721 4:e66bd933bafa 327 stepRY += turn;
kenken0721 4:e66bd933bafa 328 stepLY -= turn;
kenken0721 4:e66bd933bafa 329 }else if(turn < 0){
kenken0721 4:e66bd933bafa 330 stepRY += turn;
kenken0721 4:e66bd933bafa 331 stepLY -= turn;
kenken0721 4:e66bd933bafa 332 }
kenken0721 4:e66bd933bafa 333 while(1){
kenken0721 4:e66bd933bafa 334 if(alta == 0){
kenken0721 4:e66bd933bafa 335 //0,3
kenken0721 4:e66bd933bafa 336 setSite(0, pos[0], pos[1] + stepRY, pos[3]-pos[6]);
kenken0721 4:e66bd933bafa 337 setSite(1, pos[0], pos[1], pos[3]);
kenken0721 4:e66bd933bafa 338 setSite(2, pos[0], pos[2], pos[4]);
kenken0721 4:e66bd933bafa 339 setSite(3, pos[0], pos[2] + stepLY, pos[4]-pos[7]);
kenken0721 4:e66bd933bafa 340 allMotionWait();
kenken0721 4:e66bd933bafa 341 setSite(0, pos[0], pos[1] + stepRY, pos[3]);
kenken0721 4:e66bd933bafa 342 setSite(3, pos[0], pos[2] + stepLY, pos[4]);
kenken0721 4:e66bd933bafa 343 allMotionWait();
kenken0721 4:e66bd933bafa 344 }else{
kenken0721 4:e66bd933bafa 345 //1,2
kenken0721 4:e66bd933bafa 346 setSite(0, pos[0], pos[1], pos[3]);
kenken0721 4:e66bd933bafa 347 setSite(1, pos[0], pos[1] + stepLY, pos[3]-pos[6]);
kenken0721 4:e66bd933bafa 348 setSite(2, pos[0], pos[2] + stepRY, pos[4]-pos[7]);
kenken0721 4:e66bd933bafa 349 setSite(3, pos[0], pos[2], pos[4]);
kenken0721 4:e66bd933bafa 350 allMotionWait();
kenken0721 4:e66bd933bafa 351 setSite(1, pos[0], pos[1] + stepLY, pos[3]);
kenken0721 4:e66bd933bafa 352 setSite(2, pos[0], pos[2] + stepRY, pos[4]);
kenken0721 4:e66bd933bafa 353 allMotionWait();
kenken0721 4:e66bd933bafa 354 }
kenken0721 4:e66bd933bafa 355 if(ropeFlag == true){
kenken0721 4:e66bd933bafa 356 return alta;
kenken0721 4:e66bd933bafa 357 }
kenken0721 4:e66bd933bafa 358 alta = !alta;
kenken0721 4:e66bd933bafa 359 }
kenken0721 4:e66bd933bafa 360 }
kenken0721 4:e66bd933bafa 361
kenken0721 4:e66bd933bafa 362 void clabWalk(unsigned int count, float *pos, float *speed, float turn){
kenken0721 2:6140746e19b1 363 moveSpeed = speed[0];
kenken0721 2:6140746e19b1 364 while(count-- > 0){
kenken0721 2:6140746e19b1 365 //0,3
kenken0721 2:6140746e19b1 366 setSite(0, pos[0]+turn, pos[1] + pos[5], pos[3]-pos[6]);
kenken0721 2:6140746e19b1 367 setSite(1, pos[0], pos[1], pos[3]);
kenken0721 2:6140746e19b1 368 setSite(2, pos[0], pos[2], pos[4]);
kenken0721 2:6140746e19b1 369 setSite(3, pos[0]-turn, pos[2] + pos[5], pos[4]-pos[7]);
kenken0721 2:6140746e19b1 370 allMotionWait();
kenken0721 2:6140746e19b1 371 setSite(0, pos[0]+turn, pos[1] + pos[5], pos[3]);
kenken0721 2:6140746e19b1 372 setSite(3, pos[0]-turn, pos[2] + pos[5], pos[4]);
kenken0721 2:6140746e19b1 373 allMotionWait();
kenken0721 2:6140746e19b1 374 //1,2
kenken0721 2:6140746e19b1 375 setSite(0, pos[0], pos[1], pos[3]);
kenken0721 2:6140746e19b1 376 setSite(1, pos[0]-turn, pos[1] + pos[5], pos[3]-pos[6]);
kenken0721 2:6140746e19b1 377 setSite(2, pos[0]+turn, pos[2] + pos[5], pos[4]-pos[7]);
kenken0721 2:6140746e19b1 378 setSite(3, pos[0], pos[2], pos[4]);
kenken0721 2:6140746e19b1 379 allMotionWait();
kenken0721 2:6140746e19b1 380 setSite(1, pos[0]-turn, pos[1] + pos[5], pos[3]);
kenken0721 2:6140746e19b1 381 setSite(2, pos[0]+turn, pos[2] + pos[5], pos[4]);
kenken0721 2:6140746e19b1 382 allMotionWait();
kenken0721 2:6140746e19b1 383 }
kenken0721 2:6140746e19b1 384 }
kenken0721 0:ffe56c419abd 385
kenken0721 2:6140746e19b1 386 void backTrot(unsigned int count,float *pos,float *speed, float turn){
kenken0721 1:5fb6895ad8e6 387 float stepRY = pos[5];
kenken0721 1:5fb6895ad8e6 388 float stepLY = pos[5];
kenken0721 1:5fb6895ad8e6 389 if(turn > 0){
kenken0721 1:5fb6895ad8e6 390 stepRY += turn;
kenken0721 1:5fb6895ad8e6 391 stepLY -= turn;
kenken0721 1:5fb6895ad8e6 392 }else if(turn < 0){
kenken0721 1:5fb6895ad8e6 393 stepRY += turn;
kenken0721 1:5fb6895ad8e6 394 stepLY -= turn;
kenken0721 0:ffe56c419abd 395 }
kenken0721 1:5fb6895ad8e6 396 moveSpeed = speed[0];
kenken0721 1:5fb6895ad8e6 397 while(count-- > 0){
kenken0721 1:5fb6895ad8e6 398 //0,3
kenken0721 1:5fb6895ad8e6 399 setSite(0, pos[0], pos[1] + stepRY, pos[3]-pos[6]);
kenken0721 1:5fb6895ad8e6 400 setSite(1, pos[0], pos[1], pos[3]);
kenken0721 2:6140746e19b1 401 setSite(2, pos[0], pos[2], pos[4]);
kenken0721 2:6140746e19b1 402 setSite(3, pos[0], pos[2] + stepLY, pos[4]-pos[7]);
kenken0721 1:5fb6895ad8e6 403 allMotionWait();
kenken0721 1:5fb6895ad8e6 404 setSite(0, pos[0], pos[1] + stepRY, pos[3]);
kenken0721 1:5fb6895ad8e6 405 setSite(3, pos[0], pos[2] + stepLY, pos[4]);
kenken0721 1:5fb6895ad8e6 406 allMotionWait();
kenken0721 1:5fb6895ad8e6 407 //1,2
kenken0721 1:5fb6895ad8e6 408 setSite(0, pos[0], pos[1], pos[3]);
kenken0721 1:5fb6895ad8e6 409 setSite(1, pos[0], pos[1] + stepLY, pos[3]-pos[6]);
kenken0721 1:5fb6895ad8e6 410 setSite(2, pos[0], pos[2] + stepRY, pos[4]-pos[7]);
kenken0721 2:6140746e19b1 411 setSite(3, pos[0], pos[2], pos[4]);
kenken0721 1:5fb6895ad8e6 412 allMotionWait();
kenken0721 1:5fb6895ad8e6 413 setSite(1, pos[0], pos[1] + stepLY, pos[3]);
kenken0721 1:5fb6895ad8e6 414 setSite(2, pos[0], pos[2] + stepRY, pos[4]);
kenken0721 1:5fb6895ad8e6 415 allMotionWait();
kenken0721 1:5fb6895ad8e6 416 }
kenken0721 0:ffe56c419abd 417 }
kenken0721 2:6140746e19b1 418 void stepOver(){
kenken0721 2:6140746e19b1 419 moveSpeed = 8;//-30
kenken0721 2:6140746e19b1 420 setSite(0, 200, 20, 250);
kenken0721 2:6140746e19b1 421 setSite(1, 200, 155, 250);
kenken0721 2:6140746e19b1 422 setSite(2, 200, -25, 250);
kenken0721 2:6140746e19b1 423 setSite(3, 200, -160, 250);
kenken0721 2:6140746e19b1 424 allMotionWait();
kenken0721 2:6140746e19b1 425
kenken0721 2:6140746e19b1 426 setSite(0, 200, -20, 30);
kenken0721 2:6140746e19b1 427 allMotionWait();
kenken0721 2:6140746e19b1 428 setSite(0, 200, 200, 30);
kenken0721 2:6140746e19b1 429 allMotionWait();
kenken0721 2:6140746e19b1 430 setSite(0, 200, 200, 140);
kenken0721 2:6140746e19b1 431 allMotionWait();
kenken0721 2:6140746e19b1 432
kenken0721 2:6140746e19b1 433 setSite(1, 200, 30, 30);
kenken0721 2:6140746e19b1 434 allMotionWait();
kenken0721 2:6140746e19b1 435 setSite(1, 200, 270, 30);
kenken0721 2:6140746e19b1 436 allMotionWait();
kenken0721 2:6140746e19b1 437 setSite(1, 200, 270, 140);
kenken0721 2:6140746e19b1 438 allMotionWait();
kenken0721 2:6140746e19b1 439
kenken0721 2:6140746e19b1 440 float stepPosTest[8] = {200,10,-150,110,250,70,60,80};
kenken0721 2:6140746e19b1 441 float stepSpeTest[2] = {7,5};
kenken0721 2:6140746e19b1 442 forwardTrot(2,stepPosTest,stepSpeTest,0);
kenken0721 2:6140746e19b1 443
kenken0721 2:6140746e19b1 444 moveSpeed = 8;
kenken0721 2:6140746e19b1 445 setSite(2, 200, -200, 30);
kenken0721 2:6140746e19b1 446 allMotionWait();
kenken0721 2:6140746e19b1 447 setSite(2, 200, 40, 30);
kenken0721 2:6140746e19b1 448 allMotionWait();
kenken0721 2:6140746e19b1 449 setSite(2, 200, 40, 140);
kenken0721 2:6140746e19b1 450 allMotionWait();
kenken0721 2:6140746e19b1 451
kenken0721 2:6140746e19b1 452 setSite(0, 200, 105, 30);
kenken0721 2:6140746e19b1 453 allMotionWait();
kenken0721 2:6140746e19b1 454 setSite(0, 200, 280, 30);
kenken0721 2:6140746e19b1 455 allMotionWait();
kenken0721 2:6140746e19b1 456 setSite(0, 200, 280, 240);
kenken0721 2:6140746e19b1 457 allMotionWait();
kenken0721 2:6140746e19b1 458
kenken0721 2:6140746e19b1 459 moveSpeed = 7;
kenken0721 2:6140746e19b1 460 setSite(0, 200, 180, 230);//80
kenken0721 2:6140746e19b1 461 setSite(1, 200, -20, 140);
kenken0721 2:6140746e19b1 462 setSite(2, 200, -70, 140);//-80
kenken0721 2:6140746e19b1 463 setSite(3, 200, -240, 240);
kenken0721 2:6140746e19b1 464 allMotionWait();
kenken0721 2:6140746e19b1 465
kenken0721 2:6140746e19b1 466 moveSpeed = 8;
kenken0721 2:6140746e19b1 467 setSite(3, 200, -250, 30);
kenken0721 2:6140746e19b1 468 allMotionWait();
kenken0721 2:6140746e19b1 469 setSite(3, 200, -150, 30);
kenken0721 2:6140746e19b1 470 allMotionWait();
kenken0721 2:6140746e19b1 471 setSite(3, 200, -150, 140);//-150
kenken0721 2:6140746e19b1 472 allMotionWait();
kenken0721 2:6140746e19b1 473
kenken0721 2:6140746e19b1 474 setSite(1, 200, -10, 30);
kenken0721 2:6140746e19b1 475 allMotionWait();
kenken0721 2:6140746e19b1 476 setSite(1, 200, 250, 30);
kenken0721 2:6140746e19b1 477 allMotionWait();
kenken0721 2:6140746e19b1 478 setSite(1, 200, 250, 240);//150
kenken0721 2:6140746e19b1 479 allMotionWait();
kenken0721 2:6140746e19b1 480
kenken0721 2:6140746e19b1 481 float stepPosTest2[8] = {200,30,-150,250,140,100,60,120};
kenken0721 2:6140746e19b1 482 float stepSpeTest2[2] = {7,6};
kenken0721 2:6140746e19b1 483 forwardTrot(1,stepPosTest2,stepSpeTest2,0);
kenken0721 2:6140746e19b1 484
kenken0721 2:6140746e19b1 485 float stepPosTest3[8] = {200,30,-120,200,180,100,60,140};
kenken0721 2:6140746e19b1 486 float stepSpeTest3[2] = {7,6};
kenken0721 2:6140746e19b1 487 forwardTrot(1,stepPosTest3,stepSpeTest3,0);
kenken0721 2:6140746e19b1 488
kenken0721 2:6140746e19b1 489
kenken0721 2:6140746e19b1 490 }
kenken0721 2:6140746e19b1 491
kenken0721 4:e66bd933bafa 492 bool ropeOver(){
kenken0721 4:e66bd933bafa 493 float levelPosTest[8] = {200,30,-160,160,170,145,60,70};
kenken0721 4:e66bd933bafa 494 float levelSpeTest[2] = {7.5,8};
kenken0721 4:e66bd933bafa 495
kenken0721 2:6140746e19b1 496 ropeFlag = false;
kenken0721 2:6140746e19b1 497 ropeSign = true;
kenken0721 4:e66bd933bafa 498 bool alta;
kenken0721 4:e66bd933bafa 499 alta = forwardTrot2(levelPosTest,levelSpeTest,0);
kenken0721 4:e66bd933bafa 500 ropeTouch.pulsewidth_us(2400);
kenken0721 2:6140746e19b1 501 ropeSign = false;
kenken0721 2:6140746e19b1 502 moveSpeed = 6;
kenken0721 4:e66bd933bafa 503 int f1;
kenken0721 4:e66bd933bafa 504 int f2;
kenken0721 4:e66bd933bafa 505 int b1;
kenken0721 4:e66bd933bafa 506 int b2;
kenken0721 4:e66bd933bafa 507 if(alta == true){
kenken0721 4:e66bd933bafa 508 f1 = 0;
kenken0721 4:e66bd933bafa 509 f2 = 1;
kenken0721 4:e66bd933bafa 510 b1 = 3;
kenken0721 4:e66bd933bafa 511 b2 = 2;
kenken0721 4:e66bd933bafa 512 }else{
kenken0721 4:e66bd933bafa 513 f1 = 1;
kenken0721 4:e66bd933bafa 514 f2 = 0;
kenken0721 4:e66bd933bafa 515 b1 = 2;
kenken0721 4:e66bd933bafa 516 b2 = 3;
kenken0721 4:e66bd933bafa 517 }
kenken0721 4:e66bd933bafa 518
kenken0721 4:e66bd933bafa 519 setSite(f1, 200, 60, 180);
kenken0721 4:e66bd933bafa 520 setSite(f2, 200, 160, 180);
kenken0721 4:e66bd933bafa 521
kenken0721 4:e66bd933bafa 522 setSite(b2, 200, -10, 180);
kenken0721 4:e66bd933bafa 523 setSite(b1, 200, -110, 180);
kenken0721 2:6140746e19b1 524 allMotionWait();
kenken0721 2:6140746e19b1 525
kenken0721 4:e66bd933bafa 526 setSite(f1, 200, 0, 30);
kenken0721 4:e66bd933bafa 527 setSite(f2, 200, 160, 180);
kenken0721 4:e66bd933bafa 528 setSite(b2, 200, -10, 180);
kenken0721 4:e66bd933bafa 529 setSite(b1, 200, -110, 30);
kenken0721 2:6140746e19b1 530 allMotionWait();
kenken0721 2:6140746e19b1 531
kenken0721 4:e66bd933bafa 532 setSite(f1, 250, 160, 30);
kenken0721 4:e66bd933bafa 533 setSite(f2, 200, 60, 180);
kenken0721 4:e66bd933bafa 534 setSite(b2, 200, -110, 180);
kenken0721 4:e66bd933bafa 535 setSite(b1, 200, -10, 180);
kenken0721 4:e66bd933bafa 536 allMotionWait();
kenken0721 4:e66bd933bafa 537
kenken0721 4:e66bd933bafa 538 setSite(f1, 250, 100, 180);
kenken0721 4:e66bd933bafa 539
kenken0721 4:e66bd933bafa 540 setSite(f1, 250, 100, 200);
kenken0721 4:e66bd933bafa 541 setSite(f2, 200, 0, 30);
kenken0721 4:e66bd933bafa 542 setSite(b2, 200, -110, 30);
kenken0721 4:e66bd933bafa 543 setSite(b1, 200, -10, 200);
kenken0721 2:6140746e19b1 544 allMotionWait();
kenken0721 2:6140746e19b1 545
kenken0721 4:e66bd933bafa 546 setSite(f1, 200, 60, 180);
kenken0721 4:e66bd933bafa 547 setSite(f2, 200, 220, 30);
kenken0721 4:e66bd933bafa 548 setSite(b2, 200, -10, 180);
kenken0721 4:e66bd933bafa 549 setSite(b1, 200, -110, 180);
kenken0721 4:e66bd933bafa 550 allMotionWait();
kenken0721 4:e66bd933bafa 551
kenken0721 4:e66bd933bafa 552 setSite(f2, 200, 300, 180);
kenken0721 2:6140746e19b1 553 allMotionWait();
kenken0721 4:e66bd933bafa 554
kenken0721 4:e66bd933bafa 555 forwardTrot(1,levelPosTest,levelSpeTest,0,alta);
kenken0721 4:e66bd933bafa 556 moveSpeed = 9.0;
kenken0721 4:e66bd933bafa 557 setSite(f1, 200, -20, 240);
kenken0721 4:e66bd933bafa 558 setSite(f2, 200, 80, 240);
kenken0721 4:e66bd933bafa 559 setSite(b2, 200, -190, 260);
kenken0721 4:e66bd933bafa 560 setSite(b1, 200, -190, 260);
kenken0721 2:6140746e19b1 561 allMotionWait();
kenken0721 2:6140746e19b1 562
kenken0721 4:e66bd933bafa 563 setSite(b1, 200, -300, 30);
kenken0721 2:6140746e19b1 564 allMotionWait();
kenken0721 4:e66bd933bafa 565 setSite(b1, 200, 0, 30);
kenken0721 2:6140746e19b1 566 allMotionWait();
kenken0721 4:e66bd933bafa 567 setSite(b1, 200, 0, 260);
kenken0721 2:6140746e19b1 568 allMotionWait();
kenken0721 2:6140746e19b1 569
kenken0721 4:e66bd933bafa 570 setSite(f1, 230, -40, 230);
kenken0721 4:e66bd933bafa 571 setSite(f2, 200, 60, 230);
kenken0721 4:e66bd933bafa 572 setSite(b2, 200, -180, 260);
kenken0721 4:e66bd933bafa 573 setSite(b1, 200, -100, 260);
kenken0721 2:6140746e19b1 574 allMotionWait();
kenken0721 2:6140746e19b1 575
kenken0721 4:e66bd933bafa 576 setSite(b2, 200, -300, 30);
kenken0721 2:6140746e19b1 577 allMotionWait();
kenken0721 4:e66bd933bafa 578 setSite(b2, 200, 0, 30);
kenken0721 2:6140746e19b1 579 allMotionWait();
kenken0721 4:e66bd933bafa 580 setSite(b2, 200, 0, 260);
kenken0721 2:6140746e19b1 581 allMotionWait();
kenken0721 2:6140746e19b1 582 ropeFlag = false;
kenken0721 4:e66bd933bafa 583 return alta;
kenken0721 2:6140746e19b1 584 }
kenken0721 1:5fb6895ad8e6 585 //初期姿勢
kenken0721 1:5fb6895ad8e6 586 void initPos(){
kenken0721 1:5fb6895ad8e6 587 moveSpeed = 1;
kenken0721 1:5fb6895ad8e6 588 float pos[3] = {150,0,100};
kenken0721 0:ffe56c419abd 589 for(int i=0;i<4;i++){
kenken0721 1:5fb6895ad8e6 590 for(int j=0;j<3;j++){
kenken0721 1:5fb6895ad8e6 591 currentSite[i][j] = pos[j];
kenken0721 1:5fb6895ad8e6 592 nextSite[i][j] = pos[j];
kenken0721 1:5fb6895ad8e6 593 }
kenken0721 0:ffe56c419abd 594 }
kenken0721 0:ffe56c419abd 595 }
kenken0721 2:6140746e19b1 596 //7680,10100
kenken0721 2:6140746e19b1 597 //5800,9600
kenken0721 3:0366c1adc510 598 void lackUp(float initPos1,float initPos2){
kenken0721 3:0366c1adc510 599 //1->10170,2->7500
kenken0721 2:6140746e19b1 600 update.detach();
kenken0721 3:0366c1adc510 601 float pos1 = initPos1;
kenken0721 3:0366c1adc510 602 float pos2 = initPos2;
kenken0721 3:0366c1adc510 603 float sum1 = float((10170.0 - float(initPos1))/1000.0);
kenken0721 3:0366c1adc510 604 float sum2 = float((7500.0 - float(initPos2))/1000.0);
kenken0721 2:6140746e19b1 605 for(int i=0;i<1000;i++){
kenken0721 2:6140746e19b1 606 pos1 = pos1 + sum1;
kenken0721 2:6140746e19b1 607 pos2 = pos2 + sum2;
kenken0721 2:6140746e19b1 608 servo[0].move(4,int(pos1));
kenken0721 2:6140746e19b1 609 servo[0].move(5,int(pos2));
kenken0721 2:6140746e19b1 610 wait(0.0008);
kenken0721 2:6140746e19b1 611 }
kenken0721 2:6140746e19b1 612 }
kenken0721 2:6140746e19b1 613
kenken0721 1:5fb6895ad8e6 614 //---------------------------------------------------------------
kenken0721 0:ffe56c419abd 615
kenken0721 0:ffe56c419abd 616 int main() {
kenken0721 3:0366c1adc510 617 //Led = 0;
kenken0721 2:6140746e19b1 618 ropeTouch.pulsewidth_us(2400);
kenken0721 0:ffe56c419abd 619 for(int i=0;i<4;i++){
kenken0721 1:5fb6895ad8e6 620 servo[i].init();
kenken0721 0:ffe56c419abd 621 }
kenken0721 3:0366c1adc510 622 //------赤、青モード決定-------------
kenken0721 3:0366c1adc510 623 color.mode(PullUp);
kenken0721 3:0366c1adc510 624 int colorCount = 0;
kenken0721 3:0366c1adc510 625 for(int i=0;i<100;i++){
kenken0721 3:0366c1adc510 626 if(color.read() == true){
kenken0721 3:0366c1adc510 627 colorCount++;
kenken0721 3:0366c1adc510 628 }else{
kenken0721 3:0366c1adc510 629 colorCount--;
kenken0721 3:0366c1adc510 630 }
kenken0721 3:0366c1adc510 631 if(colorCount >= 0)
kenken0721 3:0366c1adc510 632 initRed();
kenken0721 3:0366c1adc510 633 else
kenken0721 3:0366c1adc510 634 initBlue();
kenken0721 3:0366c1adc510 635 }
kenken0721 3:0366c1adc510 636 //--------------------------------
kenken0721 2:6140746e19b1 637 //------平地と坂のモード決定----------
kenken0721 2:6140746e19b1 638 sw.mode(PullUp);
kenken0721 2:6140746e19b1 639 int modeCount = 0;
kenken0721 2:6140746e19b1 640 while(1){
kenken0721 2:6140746e19b1 641 if(sw.read() == true){
kenken0721 2:6140746e19b1 642 modeCount++;
kenken0721 2:6140746e19b1 643 }else{
kenken0721 2:6140746e19b1 644 modeCount--;
kenken0721 2:6140746e19b1 645 }
kenken0721 2:6140746e19b1 646 if(modeCount >= 100){
kenken0721 2:6140746e19b1 647 modeFlag = true;
kenken0721 2:6140746e19b1 648 break;
kenken0721 2:6140746e19b1 649 }else if(modeCount <= -100){
kenken0721 2:6140746e19b1 650 modeFlag = false;
kenken0721 2:6140746e19b1 651 break;
kenken0721 2:6140746e19b1 652 }
kenken0721 2:6140746e19b1 653 }
kenken0721 2:6140746e19b1 654 //--------------------------------
kenken0721 1:5fb6895ad8e6 655
kenken0721 2:6140746e19b1 656 //---------平地モード---------------
kenken0721 2:6140746e19b1 657 if(modeFlag == true){
kenken0721 2:6140746e19b1 658 retry.mode(PullUp);
kenken0721 3:0366c1adc510 659 servo[0].move(4,initLack1);//持ち上げ機構初期位置へ
kenken0721 2:6140746e19b1 660 wait(0.1);
kenken0721 3:0366c1adc510 661 servo[0].move(5,initLack2);//持ち上げ機構初期位置へ
kenken0721 2:6140746e19b1 662 initPos();//スタート姿勢
kenken0721 2:6140746e19b1 663 update.attach(&siteUpdate,0.02);//20msごとに座標値更新
kenken0721 2:6140746e19b1 664 //wait(20.0);
kenken0721 2:6140746e19b1 665 setSite(0, 200, 30, 160);
kenken0721 4:e66bd933bafa 666 setSite(1, 200, 175, 160);
kenken0721 4:e66bd933bafa 667 setSite(2, 200, -15, 170);
kenken0721 2:6140746e19b1 668 setSite(3, 200, -160, 170);
kenken0721 1:5fb6895ad8e6 669 allMotionWait();
kenken0721 2:6140746e19b1 670 //while(1){};
kenken0721 3:0366c1adc510 671 wait(5.0);
kenken0721 2:6140746e19b1 672 //----------ゲルゲ受け取り待機-------------
kenken0721 2:6140746e19b1 673 bool flag = false;
kenken0721 2:6140746e19b1 674 int count = 0;
kenken0721 2:6140746e19b1 675 while(flag == false){
kenken0721 3:0366c1adc510 676 if(int(start.read()) == false){
kenken0721 2:6140746e19b1 677 count++;
kenken0721 2:6140746e19b1 678 }else{
kenken0721 2:6140746e19b1 679 count--;
kenken0721 2:6140746e19b1 680 }
kenken0721 2:6140746e19b1 681 if(count >= 5000){
kenken0721 2:6140746e19b1 682 flag = true;
kenken0721 2:6140746e19b1 683 }else if(count <= 0){
kenken0721 2:6140746e19b1 684 flag = false;
kenken0721 2:6140746e19b1 685 count = 0;
kenken0721 2:6140746e19b1 686 }
kenken0721 2:6140746e19b1 687 }
kenken0721 2:6140746e19b1 688 wait(0.2);
kenken0721 3:0366c1adc510 689 //Led = 1;
kenken0721 2:6140746e19b1 690 //-------スタート--------------------
kenken0721 2:6140746e19b1 691 float levelPosTest[8] = {200,30,-160,160,170,145,60,70};
kenken0721 3:0366c1adc510 692 float levelSpeTest[2] = {9.0,10};
kenken0721 2:6140746e19b1 693 if(retry.read() == false){
kenken0721 4:e66bd933bafa 694 //段差前
kenken0721 4:e66bd933bafa 695 forwardTrot(10,levelPosTest,levelSpeTest,flatCurve1);
kenken0721 4:e66bd933bafa 696 forwardTrot(2,levelPosTest,levelSpeTest,flatCurve2);
kenken0721 4:e66bd933bafa 697 //段差乗り越え
kenken0721 4:e66bd933bafa 698 stepOver();
kenken0721 4:e66bd933bafa 699 forwardTrot(3,levelPosTest,levelSpeTest,0);
kenken0721 4:e66bd933bafa 700 moveSpeed = 6;
kenken0721 4:e66bd933bafa 701 //リトライ待機
kenken0721 4:e66bd933bafa 702 setSite(0, 200, 30, 160);
kenken0721 4:e66bd933bafa 703 setSite(1, 200, 175, 160);
kenken0721 4:e66bd933bafa 704 setSite(2, 200, -15, 170);
kenken0721 4:e66bd933bafa 705 setSite(3, 200, -160, 170);
kenken0721 4:e66bd933bafa 706 allMotionWait();
kenken0721 4:e66bd933bafa 707 while(1){
kenken0721 4:e66bd933bafa 708 if(retry.read() == true){
kenken0721 4:e66bd933bafa 709 break;
kenken0721 4:e66bd933bafa 710 }
kenken0721 2:6140746e19b1 711 }
kenken0721 2:6140746e19b1 712 wait(0.5);
kenken0721 4:e66bd933bafa 713 //ロープ
kenken0721 4:e66bd933bafa 714 ropeTouch.pulsewidth_us(700);
kenken0721 4:e66bd933bafa 715 forwardTrot(2,levelPosTest,levelSpeTest,ropeClab2);
kenken0721 4:e66bd933bafa 716 Turn(2,levelPosTest,levelSpeTest,1);
kenken0721 2:6140746e19b1 717 ropeOver();
kenken0721 4:e66bd933bafa 718 ropeTouch.pulsewidth_us(700);
kenken0721 4:e66bd933bafa 719 float levelPosTest2[8] = {200,60,-140,160,170,0,60,70};
kenken0721 4:e66bd933bafa 720 float levelSpeTest2[2] = {8.0,10};
kenken0721 4:e66bd933bafa 721 clabWalk(clabCount,levelPosTest2,levelSpeTest2,ropeClab1);
kenken0721 4:e66bd933bafa 722 Turn(1,levelPosTest,levelSpeTest,ropeClab2);
kenken0721 2:6140746e19b1 723 ropeOver();
kenken0721 4:e66bd933bafa 724 forwardTrot(1,levelPosTest,levelSpeTest,0);
kenken0721 4:e66bd933bafa 725 //坂
kenken0721 4:e66bd933bafa 726 while(true){
kenken0721 4:e66bd933bafa 727 if(sw.read() == false){
kenken0721 4:e66bd933bafa 728 break;
kenken0721 4:e66bd933bafa 729 }
kenken0721 2:6140746e19b1 730 }
kenken0721 4:e66bd933bafa 731 wait(1.0);
kenken0721 4:e66bd933bafa 732 setSite(0, 200, 30, 150);
kenken0721 4:e66bd933bafa 733 setSite(1, 200, 170, 150);
kenken0721 4:e66bd933bafa 734 setSite(2, 200, -30, 180);//-70
kenken0721 4:e66bd933bafa 735 setSite(3, 200, -30, 180);//-70
kenken0721 4:e66bd933bafa 736 allMotionWait();
kenken0721 4:e66bd933bafa 737 update.detach();
kenken0721 4:e66bd933bafa 738 wait(0.1);
kenken0721 4:e66bd933bafa 739 servo[0].move(5,slopeLack2);//持ち上げ機構初期位置へ
kenken0721 4:e66bd933bafa 740 wait(0.1);
kenken0721 4:e66bd933bafa 741 servo[0].speed(4,40);
kenken0721 4:e66bd933bafa 742 wait(0.005);
kenken0721 4:e66bd933bafa 743 servo[0].move(4,slopeLack1);
kenken0721 4:e66bd933bafa 744 wait(0.1);
kenken0721 4:e66bd933bafa 745
kenken0721 4:e66bd933bafa 746
kenken0721 4:e66bd933bafa 747 }else{
kenken0721 4:e66bd933bafa 748 ropeTouch.pulsewidth_us(700);
kenken0721 4:e66bd933bafa 749 forwardTrot(2,levelPosTest,levelSpeTest,0);
kenken0721 4:e66bd933bafa 750 Turn(2,levelPosTest,levelSpeTest,ropeClab2);
kenken0721 2:6140746e19b1 751 ropeOver();
kenken0721 4:e66bd933bafa 752 ropeTouch.pulsewidth_us(700);
kenken0721 4:e66bd933bafa 753 float levelPosTest2[8] = {200,60,-140,160,170,0,60,70};
kenken0721 4:e66bd933bafa 754 float levelSpeTest2[2] = {8.0,10};
kenken0721 4:e66bd933bafa 755 clabWalk(clabCount,levelPosTest2,levelSpeTest2,ropeClab1);
kenken0721 4:e66bd933bafa 756 Turn(1,levelPosTest,levelSpeTest,ropeClab2);
kenken0721 2:6140746e19b1 757 ropeOver();
kenken0721 4:e66bd933bafa 758 forwardTrot(1,levelPosTest,levelSpeTest,0);
kenken0721 2:6140746e19b1 759 }
kenken0721 2:6140746e19b1 760
kenken0721 4:e66bd933bafa 761
kenken0721 2:6140746e19b1 762 //----------------------------------------------------------
kenken0721 2:6140746e19b1 763 wait(10);
kenken0721 2:6140746e19b1 764 }
kenken0721 2:6140746e19b1 765 //---------坂モード-----------------
kenken0721 2:6140746e19b1 766 else{
kenken0721 3:0366c1adc510 767 servo[0].move(4,initLack1);//持ち上げ機構初期位置へ
kenken0721 2:6140746e19b1 768 wait(0.1);
kenken0721 3:0366c1adc510 769 servo[0].move(5,slopeLack2);//持ち上げ機構初期位置へ
kenken0721 2:6140746e19b1 770
kenken0721 2:6140746e19b1 771 initPos();//スタート姿勢
kenken0721 2:6140746e19b1 772 update.attach(&siteUpdate,0.02);//20msごとに座標値更新
kenken0721 2:6140746e19b1 773
kenken0721 2:6140746e19b1 774 wait(1.0);
kenken0721 2:6140746e19b1 775 setSite(0, 200, 30, 150);
kenken0721 2:6140746e19b1 776 setSite(1, 200, 170, 150);
kenken0721 3:0366c1adc510 777 setSite(2, 200, -30, 180);//-70
kenken0721 3:0366c1adc510 778 setSite(3, 200, -30, 180);//-70
kenken0721 0:ffe56c419abd 779 allMotionWait();
kenken0721 3:0366c1adc510 780
kenken0721 2:6140746e19b1 781 update.detach();
kenken0721 2:6140746e19b1 782 //----------非接触合図待機-----------------
kenken0721 2:6140746e19b1 783 trig = 0;
kenken0721 2:6140746e19b1 784 echo.rise(&pulseRise);
kenken0721 2:6140746e19b1 785 echo.fall(&pulseFall);
kenken0721 2:6140746e19b1 786 bool flag = false;
kenken0721 2:6140746e19b1 787 int count = 0;
kenken0721 2:6140746e19b1 788 timer.reset();
kenken0721 2:6140746e19b1 789 while(flag == false){
kenken0721 2:6140746e19b1 790 trig = 0;
kenken0721 2:6140746e19b1 791 wait_us(1);
kenken0721 2:6140746e19b1 792 trig = 1;
kenken0721 2:6140746e19b1 793 wait_us(11);
kenken0721 2:6140746e19b1 794 trig = 0;
kenken0721 2:6140746e19b1 795 wait_us(1);
kenken0721 2:6140746e19b1 796 wait(0.01);
kenken0721 2:6140746e19b1 797 if(Distance <= 250)
kenken0721 2:6140746e19b1 798 count++;
kenken0721 2:6140746e19b1 799 if(count >= 30)
kenken0721 2:6140746e19b1 800 flag = true;
kenken0721 2:6140746e19b1 801 }
kenken0721 2:6140746e19b1 802 echo.rise(NULL);
kenken0721 2:6140746e19b1 803 echo.fall(NULL);
kenken0721 3:0366c1adc510 804 //Led = 1;
kenken0721 2:6140746e19b1 805 servo[0].speed(4,40);
kenken0721 2:6140746e19b1 806 wait(0.005);
kenken0721 3:0366c1adc510 807 servo[0].move(4,slopeLack1);
kenken0721 2:6140746e19b1 808 wait(0.1);
kenken0721 3:0366c1adc510 809 //while(1){}
kenken0721 2:6140746e19b1 810 update.attach(&siteUpdate,0.02);//20msごとに座標値更新
kenken0721 2:6140746e19b1 811 //-----スタート-------------------
kenken0721 2:6140746e19b1 812
kenken0721 2:6140746e19b1 813 moveSpeed = 8;
kenken0721 2:6140746e19b1 814 setSite(2, 200, 0, 50);
kenken0721 2:6140746e19b1 815 allMotionWait();
kenken0721 2:6140746e19b1 816 setSite(2, 200, -200, 50);
kenken0721 2:6140746e19b1 817 allMotionWait();
kenken0721 2:6140746e19b1 818 setSite(2, 200, -200, 160);
kenken0721 2:6140746e19b1 819 allMotionWait();
kenken0721 2:6140746e19b1 820 setSite(3, 200, 0, 50);
kenken0721 2:6140746e19b1 821 allMotionWait();
kenken0721 2:6140746e19b1 822 setSite(3, 200, -120, 50);
kenken0721 2:6140746e19b1 823 allMotionWait();
kenken0721 2:6140746e19b1 824 setSite(3, 200, -90, 160);
kenken0721 0:ffe56c419abd 825 allMotionWait();
kenken0721 0:ffe56c419abd 826
kenken0721 0:ffe56c419abd 827
kenken0721 4:e66bd933bafa 828 float slopePosTest[8] = {210,170,-20,140,130,-130,110,60};
kenken0721 3:0366c1adc510 829 float slopeSpeTest[8] = {7,7};
kenken0721 0:ffe56c419abd 830
kenken0721 2:6140746e19b1 831 float lackUpPos[8] = {170,150,-150,390,390,0,0,0};
kenken0721 2:6140746e19b1 832 float lackUpSpe[2] = {4.3,4};
kenken0721 4:e66bd933bafa 833 //backTrot(10,slopePosTest,slopeSpeTest,0);
kenken0721 4:e66bd933bafa 834 backTrot(4,slopePosTest,slopeSpeTest,15);
kenken0721 4:e66bd933bafa 835 backTrot(6,slopePosTest,slopeSpeTest,0);
kenken0721 0:ffe56c419abd 836
kenken0721 2:6140746e19b1 837 standUp(lackUpPos,lackUpSpe);
kenken0721 3:0366c1adc510 838 lackUp(slopeLack1,slopeLack2);
kenken0721 0:ffe56c419abd 839 }
kenken0721 0:ffe56c419abd 840 }