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

nhk2019

Dependencies:   mbed kondoSerialServo

main.cpp@0:ffe56c419abd, 2019-03-10 (annotated)

Committer:
kenken0721
Date:
Sun Mar 10 09:07:27 2019 +0000
Revision:
0:ffe56c419abd
Child:
1:5fb6895ad8e6
nhk2019

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kenken0721 0:ffe56c419abd 1 #include "mbed.h"
kenken0721 0:ffe56c419abd 2 #include <math.h>
kenken0721 0:ffe56c419abd 3 #include "SerialServo.h"
kenken0721 0:ffe56c419abd 4
kenken0721 0:ffe56c419abd 5 Serial pc(USBTX,USBRX);//デバック用シリアル
kenken0721 0:ffe56c419abd 6 Ticker update;//現在地座標の更新割り込み
kenken0721 0:ffe56c419abd 7 Ticker press;
kenken0721 0:ffe56c419abd 8 SerialServo serServo[2] = {SerialServo(PC_4,PA_10),
kenken0721 0:ffe56c419abd 9 SerialServo(PB_9,PB_8)};
kenken0721 0:ffe56c419abd 10
kenken0721 0:ffe56c419abd 11 PwmOut pwmServo[4][2] = {
kenken0721 0:ffe56c419abd 12 {PwmOut(PA_13),
kenken0721 0:ffe56c419abd 13 PwmOut(PA_14)},
kenken0721 0:ffe56c419abd 14 {PwmOut(PA_15),
kenken0721 0:ffe56c419abd 15 PwmOut(PC_1)},
kenken0721 0:ffe56c419abd 16 {PwmOut(PC_2),
kenken0721 0:ffe56c419abd 17 PwmOut(PC_3)},
kenken0721 0:ffe56c419abd 18 {PwmOut(PC_13),
kenken0721 0:ffe56c419abd 19 PwmOut(PC_7)}
kenken0721 0:ffe56c419abd 20 };
kenken0721 0:ffe56c419abd 21
kenken0721 0:ffe56c419abd 22 AnalogIn pressure[4] = {
kenken0721 0:ffe56c419abd 23 AnalogIn(PA_0),
kenken0721 0:ffe56c419abd 24 AnalogIn(PA_1),
kenken0721 0:ffe56c419abd 25 AnalogIn(PA_4),
kenken0721 0:ffe56c419abd 26 AnalogIn(PB_0)
kenken0721 0:ffe56c419abd 27 };
kenken0721 0:ffe56c419abd 28
kenken0721 0:ffe56c419abd 29 DigitalOut leds[4] = {
kenken0721 0:ffe56c419abd 30 DigitalOut(PA_12),
kenken0721 0:ffe56c419abd 31 DigitalOut(PA_11),
kenken0721 0:ffe56c419abd 32 DigitalOut(PB_12),
kenken0721 0:ffe56c419abd 33 DigitalOut(PB_11)
kenken0721 0:ffe56c419abd 34 };
kenken0721 0:ffe56c419abd 35
kenken0721 0:ffe56c419abd 36 const float DS3218MIN = 500;
kenken0721 0:ffe56c419abd 37 const float DS3218MAX = 2500;
kenken0721 0:ffe56c419abd 38
kenken0721 0:ffe56c419abd 39 const float PDI2060MIN = 850;
kenken0721 0:ffe56c419abd 40 const float PDI2060MAX = 2350;
kenken0721 0:ffe56c419abd 41
kenken0721 0:ffe56c419abd 42 const float bodyLength = 161.72;
kenken0721 0:ffe56c419abd 43
kenken0721 0:ffe56c419abd 44 float PI = 3.1415926;
kenken0721 0:ffe56c419abd 45 //---各足の長さ----
kenken0721 0:ffe56c419abd 46 const float L1 = 76;
kenken0721 0:ffe56c419abd 47 const float L2 = 155.5;
kenken0721 0:ffe56c419abd 48 const float L3 = 290;
kenken0721 0:ffe56c419abd 49
kenken0721 0:ffe56c419abd 50 //------移動速度--------
kenken0721 0:ffe56c419abd 51 float moveSpeed = 3;
kenken0721 0:ffe56c419abd 52 float legSpeed = 7;
kenken0721 0:ffe56c419abd 53 float bodySpeed = 4;
kenken0721 0:ffe56c419abd 54
kenken0721 0:ffe56c419abd 55 float groundZ = 300;
kenken0721 0:ffe56c419abd 56 float stepZ = 190;
kenken0721 0:ffe56c419abd 57
kenken0721 0:ffe56c419abd 58
kenken0721 0:ffe56c419abd 59 float defaultX = 210;
kenken0721 0:ffe56c419abd 60 float stepY = 100;
kenken0721 0:ffe56c419abd 61 float forwardDefaultY = 0;//50
kenken0721 0:ffe56c419abd 62 float backDefaultY = -180;//-150
kenken0721 0:ffe56c419abd 63 float downZ[4] = {300,300,300,300};
kenken0721 0:ffe56c419abd 64 float upZ = 70;
kenken0721 0:ffe56c419abd 65 float offsetZ= 20;
kenken0721 0:ffe56c419abd 66
kenken0721 0:ffe56c419abd 67 float alpha;
kenken0721 0:ffe56c419abd 68 float beta;
kenken0721 0:ffe56c419abd 69 float gamma;
kenken0721 0:ffe56c419abd 70
kenken0721 0:ffe56c419abd 71 //現在の座標
kenken0721 0:ffe56c419abd 72 float currentSite[4][3];
kenken0721 0:ffe56c419abd 73 //次の座標
kenken0721 0:ffe56c419abd 74 float nextSite[4][3];
kenken0721 0:ffe56c419abd 75 //値更新用スピード
kenken0721 0:ffe56c419abd 76 float tempSpeed[4][3];
kenken0721 0:ffe56c419abd 77
kenken0721 0:ffe56c419abd 78 //現在の角度
kenken0721 0:ffe56c419abd 79 float currentAngle[4][3];
kenken0721 0:ffe56c419abd 80
kenken0721 0:ffe56c419abd 81 //現在の周波数
kenken0721 0:ffe56c419abd 82 int currentPulse[4][3];
kenken0721 0:ffe56c419abd 83
kenken0721 0:ffe56c419abd 84 bool pressFlag[4] = {false,false,false,false};
kenken0721 0:ffe56c419abd 85 bool pressSign[4] = {false,false,false,false};
kenken0721 0:ffe56c419abd 86 bool onTheStep[4] = {false,false,false,false};
kenken0721 0:ffe56c419abd 87
kenken0721 0:ffe56c419abd 88 float fmap(float x, float in_min, float in_max, float out_min, float out_max){
kenken0721 0:ffe56c419abd 89 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
kenken0721 0:ffe56c419abd 90 }
kenken0721 0:ffe56c419abd 91
kenken0721 0:ffe56c419abd 92 //座標指定
kenken0721 0:ffe56c419abd 93 void setSite(int leg, float x, float y, float z, bool mode = false){
kenken0721 0:ffe56c419abd 94 onTheStep[leg] = mode;
kenken0721 0:ffe56c419abd 95 if(mode == false){
kenken0721 0:ffe56c419abd 96 pressFlag[leg] == false;
kenken0721 0:ffe56c419abd 97 downZ[leg] = groundZ;
kenken0721 0:ffe56c419abd 98 if(z != upZ){
kenken0721 0:ffe56c419abd 99 z = downZ[leg];
kenken0721 0:ffe56c419abd 100 }
kenken0721 0:ffe56c419abd 101 }
kenken0721 0:ffe56c419abd 102 float length[3] = {0.0,0.0,0.0};
kenken0721 0:ffe56c419abd 103 length[0] = x - currentSite[leg][0];
kenken0721 0:ffe56c419abd 104 length[1] = y - currentSite[leg][1];
kenken0721 0:ffe56c419abd 105 length[2] = z - currentSite[leg][2];
kenken0721 0:ffe56c419abd 106 float L = sqrt(pow(length[0],2)+pow(length[1],2)+pow(length[2],2));
kenken0721 0:ffe56c419abd 107 tempSpeed[leg][0] = (length[0]/L)*moveSpeed;
kenken0721 0:ffe56c419abd 108 tempSpeed[leg][1] = (length[1]/L)*moveSpeed;
kenken0721 0:ffe56c419abd 109 tempSpeed[leg][2] = (length[2]/L)*moveSpeed;
kenken0721 0:ffe56c419abd 110 nextSite[leg][0] = x;
kenken0721 0:ffe56c419abd 111 nextSite[leg][1] = y;
kenken0721 0:ffe56c419abd 112 nextSite[leg][2] = z;
kenken0721 0:ffe56c419abd 113 }
kenken0721 0:ffe56c419abd 114
kenken0721 0:ffe56c419abd 115 void toAngle(int leg){
kenken0721 0:ffe56c419abd 116 float x = currentSite[leg][0];
kenken0721 0:ffe56c419abd 117 float y = currentSite[leg][1];
kenken0721 0:ffe56c419abd 118 float z = currentSite[leg][2];
kenken0721 0:ffe56c419abd 119 float L = sqrt((x*x)+(y*y));
kenken0721 0:ffe56c419abd 120 float w = L - L1;
kenken0721 0:ffe56c419abd 121 float v = sqrt((w*w)+(z*z));
kenken0721 0:ffe56c419abd 122 float alpha = atan2(z,w) - acos(((v*v)+(L2*L2)-(L3*L3))/(2*L2*v));
kenken0721 0:ffe56c419abd 123 float beta = PI - acos(((L2*L2) + (L3*L3) - (v*v))/(2*L2*L3));
kenken0721 0:ffe56c419abd 124 float gamma = atan2(y, x);
kenken0721 0:ffe56c419abd 125 currentAngle[leg][0] = gamma = (gamma / PI * 180);
kenken0721 0:ffe56c419abd 126 currentAngle[leg][1] = alpha = (alpha / PI * 180);
kenken0721 0:ffe56c419abd 127 currentAngle[leg][2] = beta = (beta / PI * 180);
kenken0721 0:ffe56c419abd 128 }
kenken0721 0:ffe56c419abd 129
kenken0721 0:ffe56c419abd 130 //圧力センサー
kenken0721 0:ffe56c419abd 131 void pressUpdate(){
kenken0721 0:ffe56c419abd 132 for(int i=0;i<4;i++){
kenken0721 0:ffe56c419abd 133 //pc.printf("%f \t",downZ[i]);
kenken0721 0:ffe56c419abd 134 float val = pressure[i].read();
kenken0721 0:ffe56c419abd 135 if(val <= 0.55){
kenken0721 0:ffe56c419abd 136 pressSign[i] = true;
kenken0721 0:ffe56c419abd 137 leds[i] = 1;
kenken0721 0:ffe56c419abd 138 }else{
kenken0721 0:ffe56c419abd 139 pressSign[i] = false;
kenken0721 0:ffe56c419abd 140 leds[i] = 0;
kenken0721 0:ffe56c419abd 141 }
kenken0721 0:ffe56c419abd 142 }
kenken0721 0:ffe56c419abd 143 //pc.printf("\n");
kenken0721 0:ffe56c419abd 144 }
kenken0721 0:ffe56c419abd 145
kenken0721 0:ffe56c419abd 146 void toPulse(int leg){
kenken0721 0:ffe56c419abd 147 switch(leg){
kenken0721 0:ffe56c419abd 148 case 0://右前足
kenken0721 0:ffe56c419abd 149 currentPulse[leg][0] = int(fmap(currentAngle[leg][0],-90.0,90.0,DS3218MAX,DS3218MIN)+100);//+100
kenken0721 0:ffe56c419abd 150 currentPulse[leg][1] = int(fmap(currentAngle[leg][1],90,-90,PDI2060MIN,PDI2060MAX)+20);
kenken0721 0:ffe56c419abd 151 currentPulse[leg][2] = int(fmap(currentAngle[leg][2],0,270,3500,11500)-300);
kenken0721 0:ffe56c419abd 152 serServo[0].move(leg,int(currentPulse[leg][2]));
kenken0721 0:ffe56c419abd 153 break;
kenken0721 0:ffe56c419abd 154 case 1://左前足
kenken0721 0:ffe56c419abd 155 currentPulse[leg][0] = int(fmap(currentAngle[leg][0],-90.0,90.0,DS3218MIN,DS3218MAX));
kenken0721 0:ffe56c419abd 156 currentPulse[leg][1] = int(fmap(currentAngle[leg][1],90,-90,PDI2060MIN,PDI2060MAX));
kenken0721 0:ffe56c419abd 157 currentPulse[leg][2] = int(fmap(currentAngle[leg][2],0,270,3500,11500));
kenken0721 0:ffe56c419abd 158 serServo[1].move(leg,int(currentPulse[leg][2]));
kenken0721 0:ffe56c419abd 159 break;
kenken0721 0:ffe56c419abd 160 case 2://右後足
kenken0721 0:ffe56c419abd 161 currentPulse[leg][0] = int(fmap(currentAngle[leg][0],-90.0,90.0,DS3218MAX,DS3218MIN)-10);//-50
kenken0721 0:ffe56c419abd 162 currentPulse[leg][1] = int(fmap(currentAngle[leg][1],90,-90,PDI2060MIN,PDI2060MAX));
kenken0721 0:ffe56c419abd 163 currentPulse[leg][2] = int(fmap(currentAngle[leg][2],0,270,3500,11500));
kenken0721 0:ffe56c419abd 164 serServo[0].move(leg,int(currentPulse[leg][2])+250);
kenken0721 0:ffe56c419abd 165 break;
kenken0721 0:ffe56c419abd 166 case 3://左後足
kenken0721 0:ffe56c419abd 167 currentPulse[leg][0] = int(fmap(currentAngle[leg][0],-90.0,90.0,DS3218MIN,DS3218MAX)-50);//-40 max min
kenken0721 0:ffe56c419abd 168 currentPulse[leg][1] = int(fmap(currentAngle[leg][1],90,-90,PDI2060MIN,PDI2060MAX)-70);
kenken0721 0:ffe56c419abd 169 currentPulse[leg][2] = int(fmap(currentAngle[leg][2],0,270,3500,11500));
kenken0721 0:ffe56c419abd 170 serServo[1].move(leg,int(currentPulse[leg][2]));
kenken0721 0:ffe56c419abd 171 break;
kenken0721 0:ffe56c419abd 172 default:
kenken0721 0:ffe56c419abd 173 break;
kenken0721 0:ffe56c419abd 174 }
kenken0721 0:ffe56c419abd 175 pwmServo[leg][0].pulsewidth_us(currentPulse[leg][0]);
kenken0721 0:ffe56c419abd 176 pwmServo[leg][1].pulsewidth_us(currentPulse[leg][1]);
kenken0721 0:ffe56c419abd 177 }
kenken0721 0:ffe56c419abd 178
kenken0721 0:ffe56c419abd 179 void siteUpdate(){
kenken0721 0:ffe56c419abd 180 for(int i=0;i<4;i++){
kenken0721 0:ffe56c419abd 181 for(int j=0;j<3;j++){
kenken0721 0:ffe56c419abd 182 float siteLength = abs(currentSite[i][j] - nextSite[i][j]);
kenken0721 0:ffe56c419abd 183 if(j==2 && siteLength >= 110 && onTheStep[i] == true && pressSign[i] == true && pressFlag[i] == false){
kenken0721 0:ffe56c419abd 184 pressFlag[i] = true;
kenken0721 0:ffe56c419abd 185 //nextSite[i][0] = currentSite[i][0];
kenken0721 0:ffe56c419abd 186 downZ[i] = stepZ;
kenken0721 0:ffe56c419abd 187 nextSite[i][2] = stepZ;
kenken0721 0:ffe56c419abd 188 }
kenken0721 0:ffe56c419abd 189 if( siteLength >= abs(tempSpeed[i][j]))
kenken0721 0:ffe56c419abd 190 currentSite[i][j] += tempSpeed[i][j];
kenken0721 0:ffe56c419abd 191 else
kenken0721 0:ffe56c419abd 192 currentSite[i][j] = nextSite[i][j];
kenken0721 0:ffe56c419abd 193 }
kenken0721 0:ffe56c419abd 194 toAngle(i);
kenken0721 0:ffe56c419abd 195 toPulse(i);
kenken0721 0:ffe56c419abd 196 }
kenken0721 0:ffe56c419abd 197 }
kenken0721 0:ffe56c419abd 198
kenken0721 0:ffe56c419abd 199 void motionWait(int leg){
kenken0721 0:ffe56c419abd 200 bool flag = false;
kenken0721 0:ffe56c419abd 201 while(flag == false){
kenken0721 0:ffe56c419abd 202 wait_us(0.001);
kenken0721 0:ffe56c419abd 203 if(currentSite[leg][0] == nextSite[leg][0]){
kenken0721 0:ffe56c419abd 204 if(currentSite[leg][1] == nextSite[leg][1]){
kenken0721 0:ffe56c419abd 205 if(currentSite[leg][2] == nextSite[leg][2]){
kenken0721 0:ffe56c419abd 206 flag = true;
kenken0721 0:ffe56c419abd 207 }
kenken0721 0:ffe56c419abd 208 }
kenken0721 0:ffe56c419abd 209 }
kenken0721 0:ffe56c419abd 210 }
kenken0721 0:ffe56c419abd 211 }
kenken0721 0:ffe56c419abd 212
kenken0721 0:ffe56c419abd 213 //すべての足が動き終わるまで待機
kenken0721 0:ffe56c419abd 214 void allMotionWait(){
kenken0721 0:ffe56c419abd 215 for(int i=0;i<4;i++){
kenken0721 0:ffe56c419abd 216 motionWait(i);
kenken0721 0:ffe56c419abd 217 }
kenken0721 0:ffe56c419abd 218 }
kenken0721 0:ffe56c419abd 219
kenken0721 0:ffe56c419abd 220
kenken0721 0:ffe56c419abd 221 //静歩行
kenken0721 0:ffe56c419abd 222 void forwardWalk1(unsigned int count){
kenken0721 0:ffe56c419abd 223 defaultX = 210;
kenken0721 0:ffe56c419abd 224 stepY = 60;
kenken0721 0:ffe56c419abd 225 forwardDefaultY = 10;
kenken0721 0:ffe56c419abd 226 backDefaultY = -150;
kenken0721 0:ffe56c419abd 227 groundZ = 300;
kenken0721 0:ffe56c419abd 228 stepZ = 160;
kenken0721 0:ffe56c419abd 229 for(int i=0;i<4;i++){
kenken0721 0:ffe56c419abd 230 downZ[i] = groundZ;
kenken0721 0:ffe56c419abd 231 }
kenken0721 0:ffe56c419abd 232 upZ = 50;
kenken0721 0:ffe56c419abd 233 offsetZ= 10;
kenken0721 0:ffe56c419abd 234 legSpeed = 3;
kenken0721 0:ffe56c419abd 235 bodySpeed = 3;
kenken0721 0:ffe56c419abd 236 while(count-- > 0){
kenken0721 0:ffe56c419abd 237 //2
kenken0721 0:ffe56c419abd 238 moveSpeed = legSpeed;
kenken0721 0:ffe56c419abd 239 setSite(2,defaultX,backDefaultY,upZ);
kenken0721 0:ffe56c419abd 240 allMotionWait();
kenken0721 0:ffe56c419abd 241 setSite(2,defaultX+100,backDefaultY+stepY*2,upZ);
kenken0721 0:ffe56c419abd 242 allMotionWait();
kenken0721 0:ffe56c419abd 243 setSite(2,defaultX,backDefaultY+stepY*2,downZ[2],1);
kenken0721 0:ffe56c419abd 244 allMotionWait();
kenken0721 0:ffe56c419abd 245 //0
kenken0721 0:ffe56c419abd 246 moveSpeed = legSpeed;
kenken0721 0:ffe56c419abd 247 setSite(0, defaultX+100, forwardDefaultY, upZ);
kenken0721 0:ffe56c419abd 248 allMotionWait();
kenken0721 0:ffe56c419abd 249 setSite(0, defaultX, forwardDefaultY + stepY*2, upZ);
kenken0721 0:ffe56c419abd 250 allMotionWait();
kenken0721 0:ffe56c419abd 251 setSite(0, defaultX, forwardDefaultY + stepY*2, downZ[0],1);
kenken0721 0:ffe56c419abd 252 allMotionWait();
kenken0721 0:ffe56c419abd 253
kenken0721 0:ffe56c419abd 254 moveSpeed = bodySpeed;
kenken0721 0:ffe56c419abd 255 setSite(0,defaultX, forwardDefaultY + stepY, downZ[0],1);
kenken0721 0:ffe56c419abd 256 setSite(1,defaultX, forwardDefaultY, downZ[1],1);
kenken0721 0:ffe56c419abd 257 setSite(2,defaultX, backDefaultY + stepY, downZ[2],1);
kenken0721 0:ffe56c419abd 258 setSite(3,defaultX, backDefaultY, downZ[3],1);
kenken0721 0:ffe56c419abd 259 allMotionWait();
kenken0721 0:ffe56c419abd 260
kenken0721 0:ffe56c419abd 261 //3
kenken0721 0:ffe56c419abd 262 moveSpeed = legSpeed;
kenken0721 0:ffe56c419abd 263 setSite(3,defaultX,backDefaultY,upZ);
kenken0721 0:ffe56c419abd 264 allMotionWait();
kenken0721 0:ffe56c419abd 265 setSite(3,defaultX+100,backDefaultY+stepY*2,upZ);
kenken0721 0:ffe56c419abd 266 allMotionWait();
kenken0721 0:ffe56c419abd 267 setSite(3,defaultX,backDefaultY+stepY*2,downZ[3],1);
kenken0721 0:ffe56c419abd 268 allMotionWait();
kenken0721 0:ffe56c419abd 269
kenken0721 0:ffe56c419abd 270 //1
kenken0721 0:ffe56c419abd 271 moveSpeed = legSpeed;
kenken0721 0:ffe56c419abd 272 setSite(1, defaultX+100, forwardDefaultY, upZ);
kenken0721 0:ffe56c419abd 273 allMotionWait();
kenken0721 0:ffe56c419abd 274 setSite(1, defaultX, forwardDefaultY + stepY*2, upZ);
kenken0721 0:ffe56c419abd 275 allMotionWait();
kenken0721 0:ffe56c419abd 276 setSite(1, defaultX, forwardDefaultY + stepY*2, downZ[1],1);
kenken0721 0:ffe56c419abd 277 allMotionWait();
kenken0721 0:ffe56c419abd 278
kenken0721 0:ffe56c419abd 279
kenken0721 0:ffe56c419abd 280
kenken0721 0:ffe56c419abd 281 moveSpeed = bodySpeed;
kenken0721 0:ffe56c419abd 282 setSite(0,defaultX, forwardDefaultY , downZ[0],1);
kenken0721 0:ffe56c419abd 283 setSite(1,defaultX, forwardDefaultY + stepY, downZ[1],1);
kenken0721 0:ffe56c419abd 284 setSite(2,defaultX, backDefaultY, downZ[2],1);
kenken0721 0:ffe56c419abd 285 setSite(3,defaultX, backDefaultY + stepY, downZ[3],1);
kenken0721 0:ffe56c419abd 286 allMotionWait();
kenken0721 0:ffe56c419abd 287 }
kenken0721 0:ffe56c419abd 288 }
kenken0721 0:ffe56c419abd 289 void backWalk1(unsigned int count){
kenken0721 0:ffe56c419abd 290 defaultX = 180;
kenken0721 0:ffe56c419abd 291 stepY = -40;
kenken0721 0:ffe56c419abd 292 forwardDefaultY = 120;//0,1
kenken0721 0:ffe56c419abd 293 backDefaultY = 0;//2,3
kenken0721 0:ffe56c419abd 294 for(int i=0;i<4;i++){
kenken0721 0:ffe56c419abd 295 downZ[i] = 200;
kenken0721 0:ffe56c419abd 296 }
kenken0721 0:ffe56c419abd 297 upZ = 110;
kenken0721 0:ffe56c419abd 298 legSpeed = 3;
kenken0721 0:ffe56c419abd 299 bodySpeed = 3;
kenken0721 0:ffe56c419abd 300 while(count-- > 0){
kenken0721 0:ffe56c419abd 301 //2
kenken0721 0:ffe56c419abd 302 moveSpeed = legSpeed;
kenken0721 0:ffe56c419abd 303 setSite(3,defaultX,backDefaultY,upZ);
kenken0721 0:ffe56c419abd 304 allMotionWait();
kenken0721 0:ffe56c419abd 305 setSite(3,defaultX,backDefaultY+stepY*2,upZ);
kenken0721 0:ffe56c419abd 306 allMotionWait();
kenken0721 0:ffe56c419abd 307 setSite(3,defaultX,backDefaultY+stepY*2,downZ[3]);
kenken0721 0:ffe56c419abd 308 allMotionWait();
kenken0721 0:ffe56c419abd 309 //0
kenken0721 0:ffe56c419abd 310 moveSpeed = legSpeed;
kenken0721 0:ffe56c419abd 311 setSite(1, defaultX, forwardDefaultY, upZ);
kenken0721 0:ffe56c419abd 312 allMotionWait();
kenken0721 0:ffe56c419abd 313 setSite(1, defaultX, forwardDefaultY + stepY*2, upZ);
kenken0721 0:ffe56c419abd 314 allMotionWait();
kenken0721 0:ffe56c419abd 315 setSite(1, defaultX, forwardDefaultY + stepY*2, downZ[1]);
kenken0721 0:ffe56c419abd 316 allMotionWait();
kenken0721 0:ffe56c419abd 317
kenken0721 0:ffe56c419abd 318 moveSpeed = bodySpeed;
kenken0721 0:ffe56c419abd 319 setSite(0,defaultX, forwardDefaultY, downZ[0]);
kenken0721 0:ffe56c419abd 320 setSite(1,defaultX, forwardDefaultY + stepY, downZ[1]);
kenken0721 0:ffe56c419abd 321 setSite(2,defaultX, backDefaultY, downZ[2]);
kenken0721 0:ffe56c419abd 322 setSite(3,defaultX, backDefaultY + stepY, downZ[3]);
kenken0721 0:ffe56c419abd 323 allMotionWait();
kenken0721 0:ffe56c419abd 324
kenken0721 0:ffe56c419abd 325 //2
kenken0721 0:ffe56c419abd 326 moveSpeed = legSpeed;
kenken0721 0:ffe56c419abd 327 setSite(2,defaultX,backDefaultY,upZ);
kenken0721 0:ffe56c419abd 328 allMotionWait();
kenken0721 0:ffe56c419abd 329 setSite(2,defaultX,backDefaultY+stepY*2,upZ);
kenken0721 0:ffe56c419abd 330 allMotionWait();
kenken0721 0:ffe56c419abd 331 setSite(2,defaultX,backDefaultY+stepY*2,downZ[2]);
kenken0721 0:ffe56c419abd 332 allMotionWait();
kenken0721 0:ffe56c419abd 333
kenken0721 0:ffe56c419abd 334 //0
kenken0721 0:ffe56c419abd 335 moveSpeed = legSpeed;
kenken0721 0:ffe56c419abd 336 setSite(0, defaultX, forwardDefaultY, upZ);
kenken0721 0:ffe56c419abd 337 allMotionWait();
kenken0721 0:ffe56c419abd 338 setSite(0, defaultX, forwardDefaultY + stepY*2, upZ);
kenken0721 0:ffe56c419abd 339 allMotionWait();
kenken0721 0:ffe56c419abd 340 setSite(0, defaultX, forwardDefaultY + stepY*2, downZ[0]);
kenken0721 0:ffe56c419abd 341 allMotionWait();
kenken0721 0:ffe56c419abd 342
kenken0721 0:ffe56c419abd 343 moveSpeed = bodySpeed;
kenken0721 0:ffe56c419abd 344 setSite(0,defaultX, forwardDefaultY + stepY , downZ[0]);
kenken0721 0:ffe56c419abd 345 setSite(1,defaultX, forwardDefaultY, downZ[2]);
kenken0721 0:ffe56c419abd 346 setSite(2,defaultX, backDefaultY + stepY, downZ[2]);
kenken0721 0:ffe56c419abd 347 setSite(3,defaultX, backDefaultY, downZ[3]);
kenken0721 0:ffe56c419abd 348 allMotionWait();
kenken0721 0:ffe56c419abd 349 }
kenken0721 0:ffe56c419abd 350 }
kenken0721 0:ffe56c419abd 351 //トロット歩容
kenken0721 0:ffe56c419abd 352 void forwardWalk2(unsigned int count){
kenken0721 0:ffe56c419abd 353 defaultX = 210;
kenken0721 0:ffe56c419abd 354 stepY = 100;
kenken0721 0:ffe56c419abd 355 forwardDefaultY = 0;
kenken0721 0:ffe56c419abd 356 backDefaultY = -120;
kenken0721 0:ffe56c419abd 357 for(int i=0;i<4;i++){
kenken0721 0:ffe56c419abd 358 downZ[i] = 180;
kenken0721 0:ffe56c419abd 359 }
kenken0721 0:ffe56c419abd 360 upZ = 100;
kenken0721 0:ffe56c419abd 361 offsetZ= 0;
kenken0721 0:ffe56c419abd 362 const float offsetX = 20;
kenken0721 0:ffe56c419abd 363
kenken0721 0:ffe56c419abd 364 while(count-- > 0){
kenken0721 0:ffe56c419abd 365 moveSpeed = 6;
kenken0721 0:ffe56c419abd 366
kenken0721 0:ffe56c419abd 367 setSite(0, defaultX+offsetX, forwardDefaultY + stepY, upZ -offsetZ);
kenken0721 0:ffe56c419abd 368 setSite(1, defaultX+offsetX, forwardDefaultY, downZ[1] - offsetZ);
kenken0721 0:ffe56c419abd 369 setSite(2, defaultX, backDefaultY, downZ[2]);
kenken0721 0:ffe56c419abd 370 setSite(3, defaultX, backDefaultY + stepY, upZ);
kenken0721 0:ffe56c419abd 371 allMotionWait();
kenken0721 0:ffe56c419abd 372
kenken0721 0:ffe56c419abd 373 setSite(0, defaultX+offsetX, forwardDefaultY + stepY, downZ[0] - offsetZ);
kenken0721 0:ffe56c419abd 374 setSite(1, defaultX+offsetX, forwardDefaultY, downZ[1] - offsetZ);
kenken0721 0:ffe56c419abd 375 setSite(2, defaultX, backDefaultY, downZ[2]);
kenken0721 0:ffe56c419abd 376 setSite(3, defaultX, backDefaultY + stepY, downZ[3]);
kenken0721 0:ffe56c419abd 377 allMotionWait();
kenken0721 0:ffe56c419abd 378
kenken0721 0:ffe56c419abd 379 setSite(0, defaultX+offsetX, forwardDefaultY, downZ[0] - offsetZ);
kenken0721 0:ffe56c419abd 380 setSite(1, defaultX+offsetX, forwardDefaultY + stepY, upZ - offsetZ);
kenken0721 0:ffe56c419abd 381 setSite(2, defaultX, backDefaultY + stepY, upZ);
kenken0721 0:ffe56c419abd 382 setSite(3, defaultX, backDefaultY, downZ[3]);
kenken0721 0:ffe56c419abd 383 allMotionWait();
kenken0721 0:ffe56c419abd 384
kenken0721 0:ffe56c419abd 385 setSite(0, defaultX+offsetX, forwardDefaultY, downZ[0] - offsetZ);
kenken0721 0:ffe56c419abd 386 setSite(1, defaultX+offsetX, forwardDefaultY + stepY, downZ[1] - offsetZ);
kenken0721 0:ffe56c419abd 387 setSite(2, defaultX, backDefaultY + stepY, downZ[2]);
kenken0721 0:ffe56c419abd 388 setSite(3, defaultX, backDefaultY, downZ[3]);
kenken0721 0:ffe56c419abd 389 allMotionWait();
kenken0721 0:ffe56c419abd 390 }
kenken0721 0:ffe56c419abd 391 }
kenken0721 0:ffe56c419abd 392
kenken0721 0:ffe56c419abd 393 void standDown(){
kenken0721 0:ffe56c419abd 394 defaultX = 210;
kenken0721 0:ffe56c419abd 395 forwardDefaultY = 120;
kenken0721 0:ffe56c419abd 396 backDefaultY = -120;
kenken0721 0:ffe56c419abd 397 for(int i=0;i<4;i++){
kenken0721 0:ffe56c419abd 398 downZ[i] = 180;
kenken0721 0:ffe56c419abd 399 }
kenken0721 0:ffe56c419abd 400
kenken0721 0:ffe56c419abd 401 upZ = 100;
kenken0721 0:ffe56c419abd 402 offsetZ= 0;
kenken0721 0:ffe56c419abd 403
kenken0721 0:ffe56c419abd 404 setSite(0, defaultX, forwardDefaultY, downZ[0]);
kenken0721 0:ffe56c419abd 405 setSite(1, defaultX, forwardDefaultY, downZ[1]);
kenken0721 0:ffe56c419abd 406 setSite(2, defaultX, backDefaultY, downZ[2]);
kenken0721 0:ffe56c419abd 407 setSite(3, defaultX, backDefaultY, downZ[3]);
kenken0721 0:ffe56c419abd 408 allMotionWait();
kenken0721 0:ffe56c419abd 409 }
kenken0721 0:ffe56c419abd 410
kenken0721 0:ffe56c419abd 411 void standUp(){
kenken0721 0:ffe56c419abd 412 defaultX = 210;
kenken0721 0:ffe56c419abd 413 forwardDefaultY = 120;
kenken0721 0:ffe56c419abd 414 backDefaultY = -120;
kenken0721 0:ffe56c419abd 415 for(int i=0;i<4;i++){
kenken0721 0:ffe56c419abd 416 downZ[i] = 180;
kenken0721 0:ffe56c419abd 417 }
kenken0721 0:ffe56c419abd 418
kenken0721 0:ffe56c419abd 419 upZ = 100;
kenken0721 0:ffe56c419abd 420 offsetZ= 0;
kenken0721 0:ffe56c419abd 421 const float standZ = 400;
kenken0721 0:ffe56c419abd 422 moveSpeed = 0.5;
kenken0721 0:ffe56c419abd 423 setSite(0, defaultX, forwardDefaultY, standZ);
kenken0721 0:ffe56c419abd 424 setSite(1, defaultX, forwardDefaultY, standZ);
kenken0721 0:ffe56c419abd 425 setSite(2, defaultX, backDefaultY, standZ);
kenken0721 0:ffe56c419abd 426 setSite(3, defaultX, backDefaultY, standZ);
kenken0721 0:ffe56c419abd 427 allMotionWait();
kenken0721 0:ffe56c419abd 428 }
kenken0721 0:ffe56c419abd 429
kenken0721 0:ffe56c419abd 430 int main() {
kenken0721 0:ffe56c419abd 431 serServo[0].init();
kenken0721 0:ffe56c419abd 432 serServo[1].init();
kenken0721 0:ffe56c419abd 433 for(int i=0;i<4;i++){
kenken0721 0:ffe56c419abd 434 leds[i] = 0;
kenken0721 0:ffe56c419abd 435 }
kenken0721 0:ffe56c419abd 436 for(int i=0;i<2;i++){
kenken0721 0:ffe56c419abd 437 currentSite[i][0] = defaultX;
kenken0721 0:ffe56c419abd 438 currentSite[i][1] = forwardDefaultY;
kenken0721 0:ffe56c419abd 439 currentSite[i][2] = downZ[i];
kenken0721 0:ffe56c419abd 440 nextSite[i][0] = defaultX;
kenken0721 0:ffe56c419abd 441 nextSite[i][1] = forwardDefaultY;
kenken0721 0:ffe56c419abd 442 nextSite[i][2] = downZ[i];
kenken0721 0:ffe56c419abd 443 }
kenken0721 0:ffe56c419abd 444 for(int i=2;i<4;i++){
kenken0721 0:ffe56c419abd 445 currentSite[i][0] = defaultX;
kenken0721 0:ffe56c419abd 446 currentSite[i][1] = backDefaultY;
kenken0721 0:ffe56c419abd 447 currentSite[i][2] = downZ[i];
kenken0721 0:ffe56c419abd 448 nextSite[i][0] = defaultX;
kenken0721 0:ffe56c419abd 449 nextSite[i][1] = backDefaultY;
kenken0721 0:ffe56c419abd 450 nextSite[i][2] = downZ[i];
kenken0721 0:ffe56c419abd 451 }
kenken0721 0:ffe56c419abd 452 update.attach(&siteUpdate,0.02);//20msごとに座標値更新
kenken0721 0:ffe56c419abd 453 press.attach(&pressUpdate,0.1);
kenken0721 0:ffe56c419abd 454 wait(3.0);
kenken0721 0:ffe56c419abd 455 while (true) {
kenken0721 0:ffe56c419abd 456 /*
kenken0721 0:ffe56c419abd 457 moveSpeed = 1;
kenken0721 0:ffe56c419abd 458 upZ = 190;
kenken0721 0:ffe56c419abd 459 setSite(3, defaultX, backDefaultY + 150, upZ);
kenken0721 0:ffe56c419abd 460 allMotionWait();
kenken0721 0:ffe56c419abd 461 setSite(3, defaultX, backDefaultY , upZ);
kenken0721 0:ffe56c419abd 462 allMotionWait();
kenken0721 0:ffe56c419abd 463 */
kenken0721 0:ffe56c419abd 464 /*
kenken0721 0:ffe56c419abd 465 setSite(0,defaultX+100,forwardDefaultY,50);
kenken0721 0:ffe56c419abd 466 allMotionWait();
kenken0721 0:ffe56c419abd 467
kenken0721 0:ffe56c419abd 468 setSite(0,defaultX,forwardDefaultY+150,50);
kenken0721 0:ffe56c419abd 469 allMotionWait();
kenken0721 0:ffe56c419abd 470
kenken0721 0:ffe56c419abd 471 setSite(0,defaultX,forwardDefaultY+150,190);
kenken0721 0:ffe56c419abd 472 allMotionWait();
kenken0721 0:ffe56c419abd 473 wait(10.0);
kenken0721 0:ffe56c419abd 474 */
kenken0721 0:ffe56c419abd 475 /*
kenken0721 0:ffe56c419abd 476 pressFlag = true;
kenken0721 0:ffe56c419abd 477 moveSpeed = 1;
kenken0721 0:ffe56c419abd 478 setSite(0, defaultX, forwardDefaultY + 50, downZ);
kenken0721 0:ffe56c419abd 479 setSite(1, defaultX, forwardDefaultY, downZ);
kenken0721 0:ffe56c419abd 480 setSite(2, defaultX, backDefaultY + 50, downZ);
kenken0721 0:ffe56c419abd 481 setSite(3, defaultX, backDefaultY + 100, downZ);
kenken0721 0:ffe56c419abd 482 allMotionWait();
kenken0721 0:ffe56c419abd 483 pressFlag = false;
kenken0721 0:ffe56c419abd 484 setSite(1,defaultX+100, forwardDefaultY, 50);
kenken0721 0:ffe56c419abd 485 allMotionWait();
kenken0721 0:ffe56c419abd 486 setSite(1,defaultX, forwardDefaultY+100, 50);
kenken0721 0:ffe56c419abd 487 allMotionWait();
kenken0721 0:ffe56c419abd 488 pressFlag = true;
kenken0721 0:ffe56c419abd 489 setSite(1,defaultX, forwardDefaultY+100, 300);
kenken0721 0:ffe56c419abd 490 allMotionWait();
kenken0721 0:ffe56c419abd 491 */
kenken0721 0:ffe56c419abd 492 /*
kenken0721 0:ffe56c419abd 493 setSite(2,defaultX+100, backDefaultY+100, 50);
kenken0721 0:ffe56c419abd 494 allMotionWait();
kenken0721 0:ffe56c419abd 495 setSite(2,defaultX, backDefaultY+100, 300);
kenken0721 0:ffe56c419abd 496 allMotionWait();
kenken0721 0:ffe56c419abd 497 */
kenken0721 0:ffe56c419abd 498 //setSite(2,defaultX+50, backDefaultY+100, upZ-30);
kenken0721 0:ffe56c419abd 499 /*
kenken0721 0:ffe56c419abd 500 setSite(3,defaultX, backDefaultY, downZ);
kenken0721 0:ffe56c419abd 501 allMotionWait();
kenken0721 0:ffe56c419abd 502
kenken0721 0:ffe56c419abd 503 setSite(3,defaultX+60, backDefaultY, upZ);
kenken0721 0:ffe56c419abd 504 allMotionWait();
kenken0721 0:ffe56c419abd 505
kenken0721 0:ffe56c419abd 506 setSite(3,defaultX+60, backDefaultY+100, upZ);
kenken0721 0:ffe56c419abd 507 allMotionWait();
kenken0721 0:ffe56c419abd 508
kenken0721 0:ffe56c419abd 509
kenken0721 0:ffe56c419abd 510 setSite(3,defaultX, backDefaultY+100, downZ);
kenken0721 0:ffe56c419abd 511 allMotionWait();
kenken0721 0:ffe56c419abd 512
kenken0721 0:ffe56c419abd 513 */
kenken0721 0:ffe56c419abd 514 //setSite(3,defaultX, backDefaultY, downZ);
kenken0721 0:ffe56c419abd 515 //allMotionWait();
kenken0721 0:ffe56c419abd 516
kenken0721 0:ffe56c419abd 517 //setSite(1,defaultX+60, forwardDefaultY, downZ-70);
kenken0721 0:ffe56c419abd 518
kenken0721 0:ffe56c419abd 519 //setSite(2,defaultX, backDefaultY+100, downZ);
kenken0721 0:ffe56c419abd 520 //allMotionWait();
kenken0721 0:ffe56c419abd 521
kenken0721 0:ffe56c419abd 522 //moveSpeed = 3;
kenken0721 0:ffe56c419abd 523 //setSite(3, 200, 0, 200);
kenken0721 0:ffe56c419abd 524 //allMotionWait();
kenken0721 0:ffe56c419abd 525 /*
kenken0721 0:ffe56c419abd 526 pc.printf("%f \t",currentAngle[3][1]);
kenken0721 0:ffe56c419abd 527 pc.printf("%f \n",currentAngle[3][2]);
kenken0721 0:ffe56c419abd 528
kenken0721 0:ffe56c419abd 529 setSite(3, 300, 0, 200);
kenken0721 0:ffe56c419abd 530 allMotionWait();
kenken0721 0:ffe56c419abd 531 pc.printf("%f \t",currentAngle[3][1]);
kenken0721 0:ffe56c419abd 532 pc.printf("%f \n",currentAngle[3][2]);
kenken0721 0:ffe56c419abd 533 */
kenken0721 0:ffe56c419abd 534 forwardWalk1(100);
kenken0721 0:ffe56c419abd 535 //forwardWalk2(100);
kenken0721 0:ffe56c419abd 536 //backWalk1(100);
kenken0721 0:ffe56c419abd 537 //standDown();
kenken0721 0:ffe56c419abd 538 //standUp();
kenken0721 0:ffe56c419abd 539 //wait(30);
kenken0721 0:ffe56c419abd 540 }
kenken0721 0:ffe56c419abd 541 }