ケンタ ミヤザキ
nhk2019
Dependencies: mbed kondoSerialServo
main.cpp@1:5fb6895ad8e6, 2019-03-17 (annotated)
- Committer:
- kenken0721
- Date:
- Sun Mar 17 11:24:51 2019 +0000
- Revision:
- 1:5fb6895ad8e6
- Parent:
- 0:ffe56c419abd
- Child:
- 2:6140746e19b1
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 | 1:5fb6895ad8e6 | 14 | const float PI = 3.1415926; |
| kenken0721 | 0:ffe56c419abd | 15 | |
| kenken0721 | 0:ffe56c419abd | 16 | //---各足の長さ---- |
| kenken0721 | 0:ffe56c419abd | 17 | const float L1 = 76; |
| kenken0721 | 1:5fb6895ad8e6 | 18 | const float L2 = 168; |
| kenken0721 | 1:5fb6895ad8e6 | 19 | const float L3 = 252; |
| kenken0721 | 0:ffe56c419abd | 20 | |
| kenken0721 | 0:ffe56c419abd | 21 | //------移動速度-------- |
| kenken0721 | 1:5fb6895ad8e6 | 22 | float moveSpeed = 1; |
| kenken0721 | 0:ffe56c419abd | 23 | |
| kenken0721 | 0:ffe56c419abd | 24 | //現在の座標 |
| kenken0721 | 0:ffe56c419abd | 25 | float currentSite[4][3]; |
| kenken0721 | 0:ffe56c419abd | 26 | //次の座標 |
| kenken0721 | 0:ffe56c419abd | 27 | float nextSite[4][3]; |
| kenken0721 | 0:ffe56c419abd | 28 | //値更新用スピード |
| kenken0721 | 0:ffe56c419abd | 29 | float tempSpeed[4][3]; |
| kenken0721 | 0:ffe56c419abd | 30 | |
| kenken0721 | 0:ffe56c419abd | 31 | //現在の角度 |
| kenken0721 | 0:ffe56c419abd | 32 | float currentAngle[4][3]; |
| kenken0721 | 0:ffe56c419abd | 33 | |
| kenken0721 | 0:ffe56c419abd | 34 | //現在の周波数 |
| kenken0721 | 0:ffe56c419abd | 35 | int currentPulse[4][3]; |
| kenken0721 | 0:ffe56c419abd | 36 | |
| kenken0721 | 0:ffe56c419abd | 37 | float fmap(float x, float in_min, float in_max, float out_min, float out_max){ |
| kenken0721 | 0:ffe56c419abd | 38 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
| kenken0721 | 0:ffe56c419abd | 39 | } |
| kenken0721 | 0:ffe56c419abd | 40 | |
| kenken0721 | 0:ffe56c419abd | 41 | //座標指定 |
| kenken0721 | 1:5fb6895ad8e6 | 42 | void setSite(int leg, float x, float y, float z){ |
| kenken0721 | 0:ffe56c419abd | 43 | float length[3] = {0.0,0.0,0.0}; |
| kenken0721 | 0:ffe56c419abd | 44 | length[0] = x - currentSite[leg][0]; |
| kenken0721 | 0:ffe56c419abd | 45 | length[1] = y - currentSite[leg][1]; |
| kenken0721 | 0:ffe56c419abd | 46 | length[2] = z - currentSite[leg][2]; |
| kenken0721 | 0:ffe56c419abd | 47 | float L = sqrt(pow(length[0],2)+pow(length[1],2)+pow(length[2],2)); |
| kenken0721 | 0:ffe56c419abd | 48 | tempSpeed[leg][0] = (length[0]/L)*moveSpeed; |
| kenken0721 | 0:ffe56c419abd | 49 | tempSpeed[leg][1] = (length[1]/L)*moveSpeed; |
| kenken0721 | 0:ffe56c419abd | 50 | tempSpeed[leg][2] = (length[2]/L)*moveSpeed; |
| kenken0721 | 0:ffe56c419abd | 51 | nextSite[leg][0] = x; |
| kenken0721 | 0:ffe56c419abd | 52 | nextSite[leg][1] = y; |
| kenken0721 | 0:ffe56c419abd | 53 | nextSite[leg][2] = z; |
| kenken0721 | 0:ffe56c419abd | 54 | } |
| kenken0721 | 0:ffe56c419abd | 55 | |
| kenken0721 | 0:ffe56c419abd | 56 | void toAngle(int leg){ |
| kenken0721 | 0:ffe56c419abd | 57 | float x = currentSite[leg][0]; |
| kenken0721 | 0:ffe56c419abd | 58 | float y = currentSite[leg][1]; |
| kenken0721 | 0:ffe56c419abd | 59 | float z = currentSite[leg][2]; |
| kenken0721 | 0:ffe56c419abd | 60 | float L = sqrt((x*x)+(y*y)); |
| kenken0721 | 0:ffe56c419abd | 61 | float w = L - L1; |
| kenken0721 | 0:ffe56c419abd | 62 | float v = sqrt((w*w)+(z*z)); |
| kenken0721 | 0:ffe56c419abd | 63 | float alpha = atan2(z,w) - acos(((v*v)+(L2*L2)-(L3*L3))/(2*L2*v)); |
| kenken0721 | 0:ffe56c419abd | 64 | float beta = PI - acos(((L2*L2) + (L3*L3) - (v*v))/(2*L2*L3)); |
| kenken0721 | 0:ffe56c419abd | 65 | float gamma = atan2(y, x); |
| kenken0721 | 0:ffe56c419abd | 66 | currentAngle[leg][0] = gamma = (gamma / PI * 180); |
| kenken0721 | 0:ffe56c419abd | 67 | currentAngle[leg][1] = alpha = (alpha / PI * 180); |
| kenken0721 | 0:ffe56c419abd | 68 | currentAngle[leg][2] = beta = (beta / PI * 180); |
| kenken0721 | 0:ffe56c419abd | 69 | } |
| kenken0721 | 0:ffe56c419abd | 70 | |
| kenken0721 | 1:5fb6895ad8e6 | 71 | |
| kenken0721 | 1:5fb6895ad8e6 | 72 | void toPulse(){ |
| kenken0721 | 1:5fb6895ad8e6 | 73 | //右前足 |
| kenken0721 | 1:5fb6895ad8e6 | 74 | currentPulse[0][0] = int(fmap(currentAngle[0][0],-135,135,3500,11500)); |
| kenken0721 | 1:5fb6895ad8e6 | 75 | currentPulse[0][1] = int(fmap(currentAngle[0][1],-135,135,11500,3500)+100); |
| kenken0721 | 1:5fb6895ad8e6 | 76 | currentPulse[0][2] = int(fmap(currentAngle[0][2],0,270,3500,11500)); |
| kenken0721 | 1:5fb6895ad8e6 | 77 | //左前足 |
| kenken0721 | 1:5fb6895ad8e6 | 78 | currentPulse[1][0] = int(fmap(currentAngle[1][0],-135,135,11500,3500)); |
| kenken0721 | 1:5fb6895ad8e6 | 79 | currentPulse[1][1] = int(fmap(currentAngle[1][1],-135,135,11500,3500)-200); |
| kenken0721 | 1:5fb6895ad8e6 | 80 | currentPulse[1][2] = int(fmap(currentAngle[1][2],0,270,3500,11500)-300); |
| kenken0721 | 1:5fb6895ad8e6 | 81 | //右後足 |
| kenken0721 | 1:5fb6895ad8e6 | 82 | currentPulse[2][0] = int(fmap(currentAngle[2][0],-135,135,3500,11500)); |
| kenken0721 | 1:5fb6895ad8e6 | 83 | currentPulse[2][1] = int(fmap(currentAngle[2][1],-135,135,11500,3500)-200); |
| kenken0721 | 1:5fb6895ad8e6 | 84 | currentPulse[2][2] = int(fmap(currentAngle[2][2],0,270,3500,11500)); |
| kenken0721 | 1:5fb6895ad8e6 | 85 | //左後足 |
| kenken0721 | 1:5fb6895ad8e6 | 86 | currentPulse[3][0] = int(fmap(currentAngle[3][0],-135,135,11500,3500)); |
| kenken0721 | 1:5fb6895ad8e6 | 87 | currentPulse[3][1] = int(fmap(currentAngle[3][1],-135,135,11500,3500)); |
| kenken0721 | 1:5fb6895ad8e6 | 88 | currentPulse[3][2] = int(fmap(currentAngle[3][2],0,270,3500,11500)+100); |
| kenken0721 | 1:5fb6895ad8e6 | 89 | |
| kenken0721 | 1:5fb6895ad8e6 | 90 | for(int i=0;i<3;i++){ |
| kenken0721 | 1:5fb6895ad8e6 | 91 | for(int j=0;j<4;j++){ |
| kenken0721 | 1:5fb6895ad8e6 | 92 | servo[j].move(i+1,int(currentPulse[j][i])); |
| kenken0721 | 1:5fb6895ad8e6 | 93 | wait_us(100); |
| kenken0721 | 0:ffe56c419abd | 94 | } |
| kenken0721 | 1:5fb6895ad8e6 | 95 | wait_us(400); |
| kenken0721 | 0:ffe56c419abd | 96 | } |
| kenken0721 | 0:ffe56c419abd | 97 | } |
| kenken0721 | 0:ffe56c419abd | 98 | |
| kenken0721 | 0:ffe56c419abd | 99 | void siteUpdate(){ |
| kenken0721 | 0:ffe56c419abd | 100 | for(int i=0;i<4;i++){ |
| kenken0721 | 0:ffe56c419abd | 101 | for(int j=0;j<3;j++){ |
| kenken0721 | 0:ffe56c419abd | 102 | float siteLength = abs(currentSite[i][j] - nextSite[i][j]); |
| kenken0721 | 0:ffe56c419abd | 103 | if( siteLength >= abs(tempSpeed[i][j])) |
| kenken0721 | 0:ffe56c419abd | 104 | currentSite[i][j] += tempSpeed[i][j]; |
| kenken0721 | 0:ffe56c419abd | 105 | else |
| kenken0721 | 0:ffe56c419abd | 106 | currentSite[i][j] = nextSite[i][j]; |
| kenken0721 | 0:ffe56c419abd | 107 | } |
| kenken0721 | 0:ffe56c419abd | 108 | toAngle(i); |
| kenken0721 | 0:ffe56c419abd | 109 | } |
| kenken0721 | 1:5fb6895ad8e6 | 110 | toPulse(); |
| kenken0721 | 0:ffe56c419abd | 111 | } |
| kenken0721 | 0:ffe56c419abd | 112 | |
| kenken0721 | 0:ffe56c419abd | 113 | void motionWait(int leg){ |
| kenken0721 | 0:ffe56c419abd | 114 | bool flag = false; |
| kenken0721 | 0:ffe56c419abd | 115 | while(flag == false){ |
| kenken0721 | 1:5fb6895ad8e6 | 116 | wait_us(1); |
| kenken0721 | 0:ffe56c419abd | 117 | if(currentSite[leg][0] == nextSite[leg][0]){ |
| kenken0721 | 0:ffe56c419abd | 118 | if(currentSite[leg][1] == nextSite[leg][1]){ |
| kenken0721 | 0:ffe56c419abd | 119 | if(currentSite[leg][2] == nextSite[leg][2]){ |
| kenken0721 | 0:ffe56c419abd | 120 | flag = true; |
| kenken0721 | 0:ffe56c419abd | 121 | } |
| kenken0721 | 0:ffe56c419abd | 122 | } |
| kenken0721 | 0:ffe56c419abd | 123 | } |
| kenken0721 | 0:ffe56c419abd | 124 | } |
| kenken0721 | 0:ffe56c419abd | 125 | } |
| kenken0721 | 0:ffe56c419abd | 126 | |
| kenken0721 | 0:ffe56c419abd | 127 | //すべての足が動き終わるまで待機 |
| kenken0721 | 0:ffe56c419abd | 128 | void allMotionWait(){ |
| kenken0721 | 0:ffe56c419abd | 129 | for(int i=0;i<4;i++){ |
| kenken0721 | 0:ffe56c419abd | 130 | motionWait(i); |
| kenken0721 | 0:ffe56c419abd | 131 | } |
| kenken0721 | 0:ffe56c419abd | 132 | } |
| kenken0721 | 1:5fb6895ad8e6 | 133 | //------------------歩行モーション------------------------------------ |
| kenken0721 | 0:ffe56c419abd | 134 | //静歩行 |
| kenken0721 | 1:5fb6895ad8e6 | 135 | void forwardWalk1(unsigned int count,float *pos ,float *speed){ |
| kenken0721 | 0:ffe56c419abd | 136 | while(count-- > 0){ |
| kenken0721 | 0:ffe56c419abd | 137 | //2 |
| kenken0721 | 1:5fb6895ad8e6 | 138 | moveSpeed = speed[1]; |
| kenken0721 | 1:5fb6895ad8e6 | 139 | setSite(2,pos[0],pos[2],pos[4]-pos[7]); |
| kenken0721 | 0:ffe56c419abd | 140 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 141 | setSite(2,pos[0],pos[2]+pos[5]*2,pos[4]-pos[7]); |
| kenken0721 | 0:ffe56c419abd | 142 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 143 | setSite(2,pos[0],pos[2]+pos[5]*2,pos[4]); |
| kenken0721 | 0:ffe56c419abd | 144 | allMotionWait(); |
| kenken0721 | 0:ffe56c419abd | 145 | //0 |
| kenken0721 | 1:5fb6895ad8e6 | 146 | setSite(0, pos[0], pos[1], pos[3]-pos[6]); |
| kenken0721 | 0:ffe56c419abd | 147 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 148 | setSite(0, pos[0], pos[1] + pos[5]*2, pos[3]-pos[6]); |
| kenken0721 | 0:ffe56c419abd | 149 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 150 | setSite(0, pos[0], pos[1] + pos[5]*2, pos[3]); |
| kenken0721 | 0:ffe56c419abd | 151 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 152 | //機体移動 |
| kenken0721 | 1:5fb6895ad8e6 | 153 | moveSpeed = speed[0]; |
| kenken0721 | 1:5fb6895ad8e6 | 154 | setSite(0,pos[0], pos[1] + pos[5], pos[3]); |
| kenken0721 | 1:5fb6895ad8e6 | 155 | setSite(1,pos[0], pos[1], pos[3]); |
| kenken0721 | 1:5fb6895ad8e6 | 156 | setSite(2,pos[0], pos[2] + pos[5], pos[4]); |
| kenken0721 | 1:5fb6895ad8e6 | 157 | setSite(3,pos[0], pos[2], pos[4]); |
| kenken0721 | 0:ffe56c419abd | 158 | allMotionWait(); |
| kenken0721 | 0:ffe56c419abd | 159 | //3 |
| kenken0721 | 1:5fb6895ad8e6 | 160 | moveSpeed = speed[1]; |
| kenken0721 | 1:5fb6895ad8e6 | 161 | setSite(3,pos[0],pos[2],pos[4]-pos[7]); |
| kenken0721 | 0:ffe56c419abd | 162 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 163 | setSite(3,pos[0],pos[2]+pos[5]*2,pos[4]-pos[7]); |
| kenken0721 | 0:ffe56c419abd | 164 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 165 | setSite(3,pos[0],pos[2]+pos[5]*2,pos[4]); |
| kenken0721 | 0:ffe56c419abd | 166 | allMotionWait(); |
| kenken0721 | 0:ffe56c419abd | 167 | //1 |
| kenken0721 | 1:5fb6895ad8e6 | 168 | setSite(1, pos[0], pos[1], pos[3]-pos[6]); |
| kenken0721 | 0:ffe56c419abd | 169 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 170 | setSite(1, pos[0], pos[1] + pos[5]*2, pos[3]-pos[6]); |
| kenken0721 | 0:ffe56c419abd | 171 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 172 | setSite(1, pos[0], pos[1] + pos[5]*2, pos[3]); |
| kenken0721 | 0:ffe56c419abd | 173 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 174 | //機体移動 |
| kenken0721 | 1:5fb6895ad8e6 | 175 | moveSpeed = speed[0]; |
| kenken0721 | 1:5fb6895ad8e6 | 176 | setSite(0,pos[0], pos[1] , pos[3]); |
| kenken0721 | 1:5fb6895ad8e6 | 177 | setSite(1,pos[0], pos[1] + pos[5], pos[3]); |
| kenken0721 | 1:5fb6895ad8e6 | 178 | setSite(2,pos[0], pos[2], pos[4]); |
| kenken0721 | 1:5fb6895ad8e6 | 179 | setSite(3,pos[0], pos[2] + pos[5], pos[4]); |
| kenken0721 | 0:ffe56c419abd | 180 | allMotionWait(); |
| kenken0721 | 0:ffe56c419abd | 181 | } |
| kenken0721 | 0:ffe56c419abd | 182 | } |
| kenken0721 | 1:5fb6895ad8e6 | 183 | //座位 |
| kenken0721 | 1:5fb6895ad8e6 | 184 | void standDown(float *pos,float *speed){ |
| kenken0721 | 1:5fb6895ad8e6 | 185 | moveSpeed = speed[0]; |
| kenken0721 | 1:5fb6895ad8e6 | 186 | setSite(0, pos[0], pos[1], pos[3]); |
| kenken0721 | 1:5fb6895ad8e6 | 187 | setSite(1, pos[0], pos[1], pos[3]); |
| kenken0721 | 1:5fb6895ad8e6 | 188 | setSite(2, pos[0], pos[2], pos[4]); |
| kenken0721 | 1:5fb6895ad8e6 | 189 | setSite(3, pos[0], pos[2], pos[4]); |
| kenken0721 | 1:5fb6895ad8e6 | 190 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 191 | } |
| kenken0721 | 1:5fb6895ad8e6 | 192 | //直立姿勢 |
| kenken0721 | 1:5fb6895ad8e6 | 193 | void standUp(float *pos,float *speed){ |
| kenken0721 | 1:5fb6895ad8e6 | 194 | moveSpeed = speed[0]; |
| kenken0721 | 1:5fb6895ad8e6 | 195 | setSite(0, pos[0], pos[1], pos[3]); |
| kenken0721 | 1:5fb6895ad8e6 | 196 | setSite(1, pos[0], pos[1], pos[3]); |
| kenken0721 | 1:5fb6895ad8e6 | 197 | setSite(2, pos[0], pos[2], pos[4]); |
| kenken0721 | 1:5fb6895ad8e6 | 198 | setSite(3, pos[0], pos[2], pos[4]); |
| kenken0721 | 1:5fb6895ad8e6 | 199 | allMotionWait(); |
| kenken0721 | 0:ffe56c419abd | 200 | } |
| kenken0721 | 0:ffe56c419abd | 201 | //トロット歩容 |
| kenken0721 | 1:5fb6895ad8e6 | 202 | void forwardTrot(unsigned int count, float *pos, float *speed, float turn){ |
| kenken0721 | 1:5fb6895ad8e6 | 203 | float stepRY = pos[5]; |
| kenken0721 | 1:5fb6895ad8e6 | 204 | float stepLY = pos[5]; |
| kenken0721 | 1:5fb6895ad8e6 | 205 | moveSpeed = speed[0]; |
| kenken0721 | 1:5fb6895ad8e6 | 206 | if(turn > 0){ |
| kenken0721 | 1:5fb6895ad8e6 | 207 | stepRY += turn; |
| kenken0721 | 1:5fb6895ad8e6 | 208 | stepLY -= turn; |
| kenken0721 | 1:5fb6895ad8e6 | 209 | }else if(turn < 0){ |
| kenken0721 | 1:5fb6895ad8e6 | 210 | stepRY += turn; |
| kenken0721 | 1:5fb6895ad8e6 | 211 | stepLY -= turn; |
| kenken0721 | 0:ffe56c419abd | 212 | } |
| kenken0721 | 0:ffe56c419abd | 213 | while(count-- > 0){ |
| kenken0721 | 1:5fb6895ad8e6 | 214 | //0,3 |
| kenken0721 | 1:5fb6895ad8e6 | 215 | setSite(0, pos[0], pos[1] + stepRY, pos[3]-pos[6]); |
| kenken0721 | 1:5fb6895ad8e6 | 216 | setSite(1, pos[0], pos[1], pos[3]); |
| kenken0721 | 1:5fb6895ad8e6 | 217 | setSite(2, pos[0], pos[2], pos[4]); |
| kenken0721 | 1:5fb6895ad8e6 | 218 | setSite(3, pos[0], pos[2] + stepLY, pos[4]-pos[7]); |
| kenken0721 | 1:5fb6895ad8e6 | 219 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 220 | setSite(0, pos[0], pos[1] + stepRY, pos[3]); |
| kenken0721 | 1:5fb6895ad8e6 | 221 | setSite(3, pos[0], pos[2] + stepLY, pos[4]); |
| kenken0721 | 0:ffe56c419abd | 222 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 223 | //1,2 |
| kenken0721 | 1:5fb6895ad8e6 | 224 | setSite(0, pos[0], pos[1], pos[3]); |
| kenken0721 | 1:5fb6895ad8e6 | 225 | setSite(1, pos[0], pos[1] + stepLY, pos[3]-pos[6]); |
| kenken0721 | 1:5fb6895ad8e6 | 226 | setSite(2, pos[0], pos[2] + stepRY, pos[4]-pos[7]); |
| kenken0721 | 1:5fb6895ad8e6 | 227 | setSite(3, pos[0], pos[2], pos[4]); |
| kenken0721 | 0:ffe56c419abd | 228 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 229 | setSite(1, pos[0], pos[1] + stepLY, pos[3]); |
| kenken0721 | 1:5fb6895ad8e6 | 230 | setSite(2, pos[0], pos[2] + stepRY, pos[4]); |
| kenken0721 | 0:ffe56c419abd | 231 | allMotionWait(); |
| kenken0721 | 0:ffe56c419abd | 232 | } |
| kenken0721 | 0:ffe56c419abd | 233 | } |
| kenken0721 | 0:ffe56c419abd | 234 | |
| kenken0721 | 1:5fb6895ad8e6 | 235 | void backTrot(unsigned int count, float turn,float *pos,float *speed){ |
| kenken0721 | 1:5fb6895ad8e6 | 236 | float stepRY = pos[5]; |
| kenken0721 | 1:5fb6895ad8e6 | 237 | float stepLY = pos[5]; |
| kenken0721 | 1:5fb6895ad8e6 | 238 | if(turn > 0){ |
| kenken0721 | 1:5fb6895ad8e6 | 239 | stepRY += turn; |
| kenken0721 | 1:5fb6895ad8e6 | 240 | stepLY -= turn; |
| kenken0721 | 1:5fb6895ad8e6 | 241 | }else if(turn < 0){ |
| kenken0721 | 1:5fb6895ad8e6 | 242 | stepRY += turn; |
| kenken0721 | 1:5fb6895ad8e6 | 243 | stepLY -= turn; |
| kenken0721 | 0:ffe56c419abd | 244 | } |
| kenken0721 | 1:5fb6895ad8e6 | 245 | moveSpeed = speed[0]; |
| kenken0721 | 1:5fb6895ad8e6 | 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[3]); |
| kenken0721 | 1:5fb6895ad8e6 | 251 | setSite(3, pos[0], pos[2] + stepLY, pos[3]-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 | 1:5fb6895ad8e6 | 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[3]); |
| kenken0721 | 1:5fb6895ad8e6 | 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 | 1:5fb6895ad8e6 | 264 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 265 | } |
| kenken0721 | 0:ffe56c419abd | 266 | } |
| kenken0721 | 1:5fb6895ad8e6 | 267 | //初期姿勢 |
| kenken0721 | 1:5fb6895ad8e6 | 268 | void initPos(){ |
| kenken0721 | 1:5fb6895ad8e6 | 269 | moveSpeed = 1; |
| kenken0721 | 1:5fb6895ad8e6 | 270 | float pos[3] = {150,0,100}; |
| kenken0721 | 0:ffe56c419abd | 271 | for(int i=0;i<4;i++){ |
| kenken0721 | 1:5fb6895ad8e6 | 272 | for(int j=0;j<3;j++){ |
| kenken0721 | 1:5fb6895ad8e6 | 273 | currentSite[i][j] = pos[j]; |
| kenken0721 | 1:5fb6895ad8e6 | 274 | nextSite[i][j] = pos[j]; |
| kenken0721 | 1:5fb6895ad8e6 | 275 | } |
| kenken0721 | 0:ffe56c419abd | 276 | } |
| kenken0721 | 0:ffe56c419abd | 277 | } |
| kenken0721 | 1:5fb6895ad8e6 | 278 | //--------------------------------------------------------------- |
| kenken0721 | 0:ffe56c419abd | 279 | |
| kenken0721 | 0:ffe56c419abd | 280 | int main() { |
| kenken0721 | 0:ffe56c419abd | 281 | for(int i=0;i<4;i++){ |
| kenken0721 | 1:5fb6895ad8e6 | 282 | servo[i].init(); |
| kenken0721 | 0:ffe56c419abd | 283 | } |
| kenken0721 | 1:5fb6895ad8e6 | 284 | wait(1.0); |
| kenken0721 | 1:5fb6895ad8e6 | 285 | initPos(); |
| kenken0721 | 0:ffe56c419abd | 286 | update.attach(&siteUpdate,0.02);//20msごとに座標値更新 |
| kenken0721 | 1:5fb6895ad8e6 | 287 | wait(5.0); |
| kenken0721 | 1:5fb6895ad8e6 | 288 | float levelPosTest[8] = {170,60,-110,170,170,100,70,70}; |
| kenken0721 | 1:5fb6895ad8e6 | 289 | float levelSpeTest[2] = {6,6}; |
| kenken0721 | 1:5fb6895ad8e6 | 290 | float slopePosTest[8] = {200,40,-90,130,230,50,100,100}; |
| kenken0721 | 1:5fb6895ad8e6 | 291 | float slopeSpeTest[2] = {4,4}; |
| kenken0721 | 1:5fb6895ad8e6 | 292 | float slopePosTest2[8] = {180,60,-110,180,160,100,90,90}; |
| kenken0721 | 1:5fb6895ad8e6 | 293 | float slopeSpeTest2[2] = {5,6}; |
| kenken0721 | 1:5fb6895ad8e6 | 294 | |
| kenken0721 | 0:ffe56c419abd | 295 | while (true) { |
| kenken0721 | 1:5fb6895ad8e6 | 296 | forwardTrot(4,levelPosTest,levelSpeTest,0); |
| kenken0721 | 1:5fb6895ad8e6 | 297 | moveSpeed = 1.5; |
| kenken0721 | 1:5fb6895ad8e6 | 298 | setSite(0, 200, 100, 240); |
| kenken0721 | 1:5fb6895ad8e6 | 299 | setSite(1, 200, 100, 240); |
| kenken0721 | 1:5fb6895ad8e6 | 300 | setSite(2, 200, -20, 230); |
| kenken0721 | 1:5fb6895ad8e6 | 301 | setSite(3, 200, -20, 230); |
| kenken0721 | 1:5fb6895ad8e6 | 302 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 303 | setSite(0, 200, 100, 30); |
| kenken0721 | 1:5fb6895ad8e6 | 304 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 305 | setSite(0, 200, 200, 30); |
| kenken0721 | 1:5fb6895ad8e6 | 306 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 307 | setSite(0, 200, 200, 130); |
| kenken0721 | 1:5fb6895ad8e6 | 308 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 309 | setSite(1, 200, 100, 30); |
| kenken0721 | 0:ffe56c419abd | 310 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 311 | setSite(1, 200, 200, 30); |
| kenken0721 | 1:5fb6895ad8e6 | 312 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 313 | setSite(1, 200, 200, 130); |
| kenken0721 | 0:ffe56c419abd | 314 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 315 | setSite(0, 200, 100, 130); |
| kenken0721 | 1:5fb6895ad8e6 | 316 | setSite(1, 200, 100, 130); |
| kenken0721 | 1:5fb6895ad8e6 | 317 | setSite(2, 200, -120, 230); |
| kenken0721 | 1:5fb6895ad8e6 | 318 | setSite(3, 200, -120, 230); |
| kenken0721 | 1:5fb6895ad8e6 | 319 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 320 | forwardTrot(4,slopePosTest,slopeSpeTest,0); |
| kenken0721 | 1:5fb6895ad8e6 | 321 | moveSpeed = 1.5; |
| kenken0721 | 1:5fb6895ad8e6 | 322 | setSite(0, 200, 70, 130); |
| kenken0721 | 1:5fb6895ad8e6 | 323 | setSite(1, 200, 70, 130); |
| kenken0721 | 1:5fb6895ad8e6 | 324 | setSite(2, 200, -150, 240); |
| kenken0721 | 1:5fb6895ad8e6 | 325 | setSite(3, 200, -150, 240); |
| kenken0721 | 0:ffe56c419abd | 326 | allMotionWait(); |
| kenken0721 | 0:ffe56c419abd | 327 | |
| kenken0721 | 1:5fb6895ad8e6 | 328 | setSite(2, 200, -120, 30); |
| kenken0721 | 1:5fb6895ad8e6 | 329 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 330 | setSite(2, 200, 0, 30); |
| kenken0721 | 1:5fb6895ad8e6 | 331 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 332 | setSite(2, 200, 0, 130); |
| kenken0721 | 0:ffe56c419abd | 333 | allMotionWait(); |
| kenken0721 | 0:ffe56c419abd | 334 | |
| kenken0721 | 1:5fb6895ad8e6 | 335 | setSite(0, 200, 20, 130); |
| kenken0721 | 1:5fb6895ad8e6 | 336 | setSite(1, 200, 20, 130); |
| kenken0721 | 1:5fb6895ad8e6 | 337 | setSite(2, 200, -100, 130); |
| kenken0721 | 1:5fb6895ad8e6 | 338 | setSite(3, 200, -200, 230); |
| kenken0721 | 0:ffe56c419abd | 339 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 340 | |
| kenken0721 | 1:5fb6895ad8e6 | 341 | setSite(0, 200, 20, 30); |
| kenken0721 | 0:ffe56c419abd | 342 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 343 | setSite(0, 200, 100, 30); |
| kenken0721 | 0:ffe56c419abd | 344 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 345 | setSite(0, 200, 100, 200); |
| kenken0721 | 0:ffe56c419abd | 346 | allMotionWait(); |
| kenken0721 | 0:ffe56c419abd | 347 | |
| kenken0721 | 1:5fb6895ad8e6 | 348 | setSite(3, 200, -120, 30); |
| kenken0721 | 0:ffe56c419abd | 349 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 350 | setSite(3, 200, 0, 30); |
| kenken0721 | 0:ffe56c419abd | 351 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 352 | setSite(3, 200, 0, 130); |
| kenken0721 | 0:ffe56c419abd | 353 | allMotionWait(); |
| kenken0721 | 0:ffe56c419abd | 354 | |
| kenken0721 | 1:5fb6895ad8e6 | 355 | setSite(1, 200, 20, 30); |
| kenken0721 | 1:5fb6895ad8e6 | 356 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 357 | setSite(1, 200, 200, 30); |
| kenken0721 | 1:5fb6895ad8e6 | 358 | allMotionWait(); |
| kenken0721 | 1:5fb6895ad8e6 | 359 | setSite(1, 200, 200, 200); |
| kenken0721 | 1:5fb6895ad8e6 | 360 | allMotionWait(); |
| kenken0721 | 0:ffe56c419abd | 361 | /* |
| kenken0721 | 1:5fb6895ad8e6 | 362 | setSite(0, 190, 150, 180); |
| kenken0721 | 1:5fb6895ad8e6 | 363 | setSite(1, 190, 150, 180); |
| kenken0721 | 1:5fb6895ad8e6 | 364 | setSite(2, 190, -150, 160); |
| kenken0721 | 1:5fb6895ad8e6 | 365 | setSite(3, 190, -150, 160); |
| kenken0721 | 0:ffe56c419abd | 366 | allMotionWait(); |
| kenken0721 | 0:ffe56c419abd | 367 | */ |
| kenken0721 | 1:5fb6895ad8e6 | 368 | forwardTrot(10,slopePosTest2,slopeSpeTest2,0); |
| kenken0721 | 1:5fb6895ad8e6 | 369 | |
| kenken0721 | 1:5fb6895ad8e6 | 370 | wait(10); |
| kenken0721 | 1:5fb6895ad8e6 | 371 | //backTrot(100,0); |
| kenken0721 | 0:ffe56c419abd | 372 | } |
| kenken0721 | 0:ffe56c419abd | 373 | } |