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?
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 | 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 | } |