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?
User | Revision | Line number | New 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 | } |