ケンタ ミヤザキ
nhk2019
Dependencies: mbed kondoSerialServo
Diff: main.cpp
- Revision:
- 1:5fb6895ad8e6
- Parent:
- 0:ffe56c419abd
- Child:
- 2:6140746e19b1
--- a/main.cpp Sun Mar 10 09:07:27 2019 +0000
+++ b/main.cpp Sun Mar 17 11:24:51 2019 +0000
@@ -1,72 +1,25 @@
#include "mbed.h"
-#include <math.h>
#include "SerialServo.h"
-Serial pc(USBTX,USBRX);//デバック用シリアル
+//Serial pc(USBTX,USBRX);//デバック用シリアル
Ticker update;//現在地座標の更新割り込み
-Ticker press;
-SerialServo serServo[2] = {SerialServo(PC_4,PA_10),
- SerialServo(PB_9,PB_8)};
-
-PwmOut pwmServo[4][2] = {
- {PwmOut(PA_13),
- PwmOut(PA_14)},
- {PwmOut(PA_15),
- PwmOut(PC_1)},
- {PwmOut(PC_2),
- PwmOut(PC_3)},
- {PwmOut(PC_13),
- PwmOut(PC_7)}
-};
-AnalogIn pressure[4] = {
- AnalogIn(PA_0),
- AnalogIn(PA_1),
- AnalogIn(PA_4),
- AnalogIn(PB_0)
-};
+SerialServo servo[4] = {SerialServo(PC_4,PC_5),//serial1
+ SerialServo(PA_2,PA_3),//serial5
+ //SerialServo(PC_10,PC_11),//serial5
+ SerialServo(PB_10,PB_11),//serial3
+ SerialServo(PC_12,PD_2)};//serial4
-DigitalOut leds[4] = {
- DigitalOut(PA_12),
- DigitalOut(PA_11),
- DigitalOut(PB_12),
- DigitalOut(PB_11)
-};
-
-const float DS3218MIN = 500;
-const float DS3218MAX = 2500;
-const float PDI2060MIN = 850;
-const float PDI2060MAX = 2350;
+const float PI = 3.1415926;
-const float bodyLength = 161.72;
-
-float PI = 3.1415926;
//---各足の長さ----
const float L1 = 76;
-const float L2 = 155.5;
-const float L3 = 290;
+const float L2 = 168;
+const float L3 = 252;
//------移動速度--------
-float moveSpeed = 3;
-float legSpeed = 7;
-float bodySpeed = 4;
-
-float groundZ = 300;
-float stepZ = 190;
-
-
-float defaultX = 210;
-float stepY = 100;
-float forwardDefaultY = 0;//50
-float backDefaultY = -180;//-150
-float downZ[4] = {300,300,300,300};
-float upZ = 70;
-float offsetZ= 20;
-
-float alpha;
-float beta;
-float gamma;
+float moveSpeed = 1;
//現在の座標
float currentSite[4][3];
@@ -81,24 +34,12 @@
//現在の周波数
int currentPulse[4][3];
-bool pressFlag[4] = {false,false,false,false};
-bool pressSign[4] = {false,false,false,false};
-bool onTheStep[4] = {false,false,false,false};
-
float fmap(float x, float in_min, float in_max, float out_min, float out_max){
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
//座標指定
-void setSite(int leg, float x, float y, float z, bool mode = false){
- onTheStep[leg] = mode;
- if(mode == false){
- pressFlag[leg] == false;
- downZ[leg] = groundZ;
- if(z != upZ){
- z = downZ[leg];
- }
- }
+void setSite(int leg, float x, float y, float z){
float length[3] = {0.0,0.0,0.0};
length[0] = x - currentSite[leg][0];
length[1] = y - currentSite[leg][1];
@@ -127,79 +68,52 @@
currentAngle[leg][2] = beta = (beta / PI * 180);
}
-//圧力センサー
-void pressUpdate(){
- for(int i=0;i<4;i++){
- //pc.printf("%f \t",downZ[i]);
- float val = pressure[i].read();
- if(val <= 0.55){
- pressSign[i] = true;
- leds[i] = 1;
- }else{
- pressSign[i] = false;
- leds[i] = 0;
+
+void toPulse(){
+ //右前足
+ currentPulse[0][0] = int(fmap(currentAngle[0][0],-135,135,3500,11500));
+ currentPulse[0][1] = int(fmap(currentAngle[0][1],-135,135,11500,3500)+100);
+ currentPulse[0][2] = int(fmap(currentAngle[0][2],0,270,3500,11500));
+ //左前足
+ currentPulse[1][0] = int(fmap(currentAngle[1][0],-135,135,11500,3500));
+ currentPulse[1][1] = int(fmap(currentAngle[1][1],-135,135,11500,3500)-200);
+ currentPulse[1][2] = int(fmap(currentAngle[1][2],0,270,3500,11500)-300);
+ //右後足
+ currentPulse[2][0] = int(fmap(currentAngle[2][0],-135,135,3500,11500));
+ currentPulse[2][1] = int(fmap(currentAngle[2][1],-135,135,11500,3500)-200);
+ currentPulse[2][2] = int(fmap(currentAngle[2][2],0,270,3500,11500));
+ //左後足
+ currentPulse[3][0] = int(fmap(currentAngle[3][0],-135,135,11500,3500));
+ currentPulse[3][1] = int(fmap(currentAngle[3][1],-135,135,11500,3500));
+ currentPulse[3][2] = int(fmap(currentAngle[3][2],0,270,3500,11500)+100);
+
+ for(int i=0;i<3;i++){
+ for(int j=0;j<4;j++){
+ servo[j].move(i+1,int(currentPulse[j][i]));
+ wait_us(100);
}
+ wait_us(400);
}
- //pc.printf("\n");
-}
-
-void toPulse(int leg){
- switch(leg){
- case 0://右前足
- currentPulse[leg][0] = int(fmap(currentAngle[leg][0],-90.0,90.0,DS3218MAX,DS3218MIN)+100);//+100
- currentPulse[leg][1] = int(fmap(currentAngle[leg][1],90,-90,PDI2060MIN,PDI2060MAX)+20);
- currentPulse[leg][2] = int(fmap(currentAngle[leg][2],0,270,3500,11500)-300);
- serServo[0].move(leg,int(currentPulse[leg][2]));
- break;
- case 1://左前足
- currentPulse[leg][0] = int(fmap(currentAngle[leg][0],-90.0,90.0,DS3218MIN,DS3218MAX));
- currentPulse[leg][1] = int(fmap(currentAngle[leg][1],90,-90,PDI2060MIN,PDI2060MAX));
- currentPulse[leg][2] = int(fmap(currentAngle[leg][2],0,270,3500,11500));
- serServo[1].move(leg,int(currentPulse[leg][2]));
- break;
- case 2://右後足
- currentPulse[leg][0] = int(fmap(currentAngle[leg][0],-90.0,90.0,DS3218MAX,DS3218MIN)-10);//-50
- currentPulse[leg][1] = int(fmap(currentAngle[leg][1],90,-90,PDI2060MIN,PDI2060MAX));
- currentPulse[leg][2] = int(fmap(currentAngle[leg][2],0,270,3500,11500));
- serServo[0].move(leg,int(currentPulse[leg][2])+250);
- break;
- case 3://左後足
- currentPulse[leg][0] = int(fmap(currentAngle[leg][0],-90.0,90.0,DS3218MIN,DS3218MAX)-50);//-40 max min
- currentPulse[leg][1] = int(fmap(currentAngle[leg][1],90,-90,PDI2060MIN,PDI2060MAX)-70);
- currentPulse[leg][2] = int(fmap(currentAngle[leg][2],0,270,3500,11500));
- serServo[1].move(leg,int(currentPulse[leg][2]));
- break;
- default:
- break;
- }
- pwmServo[leg][0].pulsewidth_us(currentPulse[leg][0]);
- pwmServo[leg][1].pulsewidth_us(currentPulse[leg][1]);
}
void siteUpdate(){
for(int i=0;i<4;i++){
for(int j=0;j<3;j++){
float siteLength = abs(currentSite[i][j] - nextSite[i][j]);
- if(j==2 && siteLength >= 110 && onTheStep[i] == true && pressSign[i] == true && pressFlag[i] == false){
- pressFlag[i] = true;
- //nextSite[i][0] = currentSite[i][0];
- downZ[i] = stepZ;
- nextSite[i][2] = stepZ;
- }
if( siteLength >= abs(tempSpeed[i][j]))
currentSite[i][j] += tempSpeed[i][j];
else
currentSite[i][j] = nextSite[i][j];
}
toAngle(i);
- toPulse(i);
}
+ toPulse();
}
void motionWait(int leg){
bool flag = false;
while(flag == false){
- wait_us(0.001);
+ wait_us(1);
if(currentSite[leg][0] == nextSite[leg][0]){
if(currentSite[leg][1] == nextSite[leg][1]){
if(currentSite[leg][2] == nextSite[leg][2]){
@@ -216,326 +130,244 @@
motionWait(i);
}
}
-
-
+//------------------歩行モーション------------------------------------
//静歩行
-void forwardWalk1(unsigned int count){
- defaultX = 210;
- stepY = 60;
- forwardDefaultY = 10;
- backDefaultY = -150;
- groundZ = 300;
- stepZ = 160;
- for(int i=0;i<4;i++){
- downZ[i] = groundZ;
- }
- upZ = 50;
- offsetZ= 10;
- legSpeed = 3;
- bodySpeed = 3;
+void forwardWalk1(unsigned int count,float *pos ,float *speed){
while(count-- > 0){
//2
- moveSpeed = legSpeed;
- setSite(2,defaultX,backDefaultY,upZ);
+ moveSpeed = speed[1];
+ setSite(2,pos[0],pos[2],pos[4]-pos[7]);
allMotionWait();
- setSite(2,defaultX+100,backDefaultY+stepY*2,upZ);
+ setSite(2,pos[0],pos[2]+pos[5]*2,pos[4]-pos[7]);
allMotionWait();
- setSite(2,defaultX,backDefaultY+stepY*2,downZ[2],1);
+ setSite(2,pos[0],pos[2]+pos[5]*2,pos[4]);
allMotionWait();
//0
- moveSpeed = legSpeed;
- setSite(0, defaultX+100, forwardDefaultY, upZ);
+ setSite(0, pos[0], pos[1], pos[3]-pos[6]);
allMotionWait();
- setSite(0, defaultX, forwardDefaultY + stepY*2, upZ);
+ setSite(0, pos[0], pos[1] + pos[5]*2, pos[3]-pos[6]);
allMotionWait();
- setSite(0, defaultX, forwardDefaultY + stepY*2, downZ[0],1);
+ setSite(0, pos[0], pos[1] + pos[5]*2, pos[3]);
allMotionWait();
-
- moveSpeed = bodySpeed;
- setSite(0,defaultX, forwardDefaultY + stepY, downZ[0],1);
- setSite(1,defaultX, forwardDefaultY, downZ[1],1);
- setSite(2,defaultX, backDefaultY + stepY, downZ[2],1);
- setSite(3,defaultX, backDefaultY, downZ[3],1);
+ //機体移動
+ moveSpeed = speed[0];
+ setSite(0,pos[0], pos[1] + pos[5], pos[3]);
+ setSite(1,pos[0], pos[1], pos[3]);
+ setSite(2,pos[0], pos[2] + pos[5], pos[4]);
+ setSite(3,pos[0], pos[2], pos[4]);
allMotionWait();
-
//3
- moveSpeed = legSpeed;
- setSite(3,defaultX,backDefaultY,upZ);
+ moveSpeed = speed[1];
+ setSite(3,pos[0],pos[2],pos[4]-pos[7]);
allMotionWait();
- setSite(3,defaultX+100,backDefaultY+stepY*2,upZ);
+ setSite(3,pos[0],pos[2]+pos[5]*2,pos[4]-pos[7]);
allMotionWait();
- setSite(3,defaultX,backDefaultY+stepY*2,downZ[3],1);
+ setSite(3,pos[0],pos[2]+pos[5]*2,pos[4]);
allMotionWait();
-
//1
- moveSpeed = legSpeed;
- setSite(1, defaultX+100, forwardDefaultY, upZ);
+ setSite(1, pos[0], pos[1], pos[3]-pos[6]);
allMotionWait();
- setSite(1, defaultX, forwardDefaultY + stepY*2, upZ);
+ setSite(1, pos[0], pos[1] + pos[5]*2, pos[3]-pos[6]);
allMotionWait();
- setSite(1, defaultX, forwardDefaultY + stepY*2, downZ[1],1);
+ setSite(1, pos[0], pos[1] + pos[5]*2, pos[3]);
allMotionWait();
-
-
-
- moveSpeed = bodySpeed;
- setSite(0,defaultX, forwardDefaultY , downZ[0],1);
- setSite(1,defaultX, forwardDefaultY + stepY, downZ[1],1);
- setSite(2,defaultX, backDefaultY, downZ[2],1);
- setSite(3,defaultX, backDefaultY + stepY, downZ[3],1);
+ //機体移動
+ moveSpeed = speed[0];
+ setSite(0,pos[0], pos[1] , pos[3]);
+ setSite(1,pos[0], pos[1] + pos[5], pos[3]);
+ setSite(2,pos[0], pos[2], pos[4]);
+ setSite(3,pos[0], pos[2] + pos[5], pos[4]);
allMotionWait();
}
}
-void backWalk1(unsigned int count){
- defaultX = 180;
- stepY = -40;
- forwardDefaultY = 120;//0,1
- backDefaultY = 0;//2,3
- for(int i=0;i<4;i++){
- downZ[i] = 200;
- }
- upZ = 110;
- legSpeed = 3;
- bodySpeed = 3;
- while(count-- > 0){
- //2
- moveSpeed = legSpeed;
- setSite(3,defaultX,backDefaultY,upZ);
- allMotionWait();
- setSite(3,defaultX,backDefaultY+stepY*2,upZ);
- allMotionWait();
- setSite(3,defaultX,backDefaultY+stepY*2,downZ[3]);
- allMotionWait();
- //0
- moveSpeed = legSpeed;
- setSite(1, defaultX, forwardDefaultY, upZ);
- allMotionWait();
- setSite(1, defaultX, forwardDefaultY + stepY*2, upZ);
- allMotionWait();
- setSite(1, defaultX, forwardDefaultY + stepY*2, downZ[1]);
- allMotionWait();
-
- moveSpeed = bodySpeed;
- setSite(0,defaultX, forwardDefaultY, downZ[0]);
- setSite(1,defaultX, forwardDefaultY + stepY, downZ[1]);
- setSite(2,defaultX, backDefaultY, downZ[2]);
- setSite(3,defaultX, backDefaultY + stepY, downZ[3]);
- allMotionWait();
-
- //2
- moveSpeed = legSpeed;
- setSite(2,defaultX,backDefaultY,upZ);
- allMotionWait();
- setSite(2,defaultX,backDefaultY+stepY*2,upZ);
- allMotionWait();
- setSite(2,defaultX,backDefaultY+stepY*2,downZ[2]);
- allMotionWait();
-
- //0
- moveSpeed = legSpeed;
- setSite(0, defaultX, forwardDefaultY, upZ);
- allMotionWait();
- setSite(0, defaultX, forwardDefaultY + stepY*2, upZ);
- allMotionWait();
- setSite(0, defaultX, forwardDefaultY + stepY*2, downZ[0]);
- allMotionWait();
-
- moveSpeed = bodySpeed;
- setSite(0,defaultX, forwardDefaultY + stepY , downZ[0]);
- setSite(1,defaultX, forwardDefaultY, downZ[2]);
- setSite(2,defaultX, backDefaultY + stepY, downZ[2]);
- setSite(3,defaultX, backDefaultY, downZ[3]);
- allMotionWait();
- }
+//座位
+void standDown(float *pos,float *speed){
+ moveSpeed = speed[0];
+ setSite(0, pos[0], pos[1], pos[3]);
+ setSite(1, pos[0], pos[1], pos[3]);
+ setSite(2, pos[0], pos[2], pos[4]);
+ setSite(3, pos[0], pos[2], pos[4]);
+ allMotionWait();
+}
+//直立姿勢
+void standUp(float *pos,float *speed){
+ moveSpeed = speed[0];
+ setSite(0, pos[0], pos[1], pos[3]);
+ setSite(1, pos[0], pos[1], pos[3]);
+ setSite(2, pos[0], pos[2], pos[4]);
+ setSite(3, pos[0], pos[2], pos[4]);
+ allMotionWait();
}
//トロット歩容
-void forwardWalk2(unsigned int count){
- defaultX = 210;
- stepY = 100;
- forwardDefaultY = 0;
- backDefaultY = -120;
- for(int i=0;i<4;i++){
- downZ[i] = 180;
+void forwardTrot(unsigned int count, float *pos, float *speed, float turn){
+ float stepRY = pos[5];
+ float stepLY = pos[5];
+ moveSpeed = speed[0];
+ if(turn > 0){
+ stepRY += turn;
+ stepLY -= turn;
+ }else if(turn < 0){
+ stepRY += turn;
+ stepLY -= turn;
}
- upZ = 100;
- offsetZ= 0;
- const float offsetX = 20;
-
while(count-- > 0){
- moveSpeed = 6;
-
- setSite(0, defaultX+offsetX, forwardDefaultY + stepY, upZ -offsetZ);
- setSite(1, defaultX+offsetX, forwardDefaultY, downZ[1] - offsetZ);
- setSite(2, defaultX, backDefaultY, downZ[2]);
- setSite(3, defaultX, backDefaultY + stepY, upZ);
+ //0,3
+ setSite(0, pos[0], pos[1] + stepRY, pos[3]-pos[6]);
+ setSite(1, pos[0], pos[1], pos[3]);
+ setSite(2, pos[0], pos[2], pos[4]);
+ setSite(3, pos[0], pos[2] + stepLY, pos[4]-pos[7]);
+ allMotionWait();
+ setSite(0, pos[0], pos[1] + stepRY, pos[3]);
+ setSite(3, pos[0], pos[2] + stepLY, pos[4]);
allMotionWait();
-
- setSite(0, defaultX+offsetX, forwardDefaultY + stepY, downZ[0] - offsetZ);
- setSite(1, defaultX+offsetX, forwardDefaultY, downZ[1] - offsetZ);
- setSite(2, defaultX, backDefaultY, downZ[2]);
- setSite(3, defaultX, backDefaultY + stepY, downZ[3]);
+ //1,2
+ setSite(0, pos[0], pos[1], pos[3]);
+ setSite(1, pos[0], pos[1] + stepLY, pos[3]-pos[6]);
+ setSite(2, pos[0], pos[2] + stepRY, pos[4]-pos[7]);
+ setSite(3, pos[0], pos[2], pos[4]);
allMotionWait();
-
- setSite(0, defaultX+offsetX, forwardDefaultY, downZ[0] - offsetZ);
- setSite(1, defaultX+offsetX, forwardDefaultY + stepY, upZ - offsetZ);
- setSite(2, defaultX, backDefaultY + stepY, upZ);
- setSite(3, defaultX, backDefaultY, downZ[3]);
- allMotionWait();
-
- setSite(0, defaultX+offsetX, forwardDefaultY, downZ[0] - offsetZ);
- setSite(1, defaultX+offsetX, forwardDefaultY + stepY, downZ[1] - offsetZ);
- setSite(2, defaultX, backDefaultY + stepY, downZ[2]);
- setSite(3, defaultX, backDefaultY, downZ[3]);
+ setSite(1, pos[0], pos[1] + stepLY, pos[3]);
+ setSite(2, pos[0], pos[2] + stepRY, pos[4]);
allMotionWait();
}
}
-void standDown(){
- defaultX = 210;
- forwardDefaultY = 120;
- backDefaultY = -120;
- for(int i=0;i<4;i++){
- downZ[i] = 180;
+void backTrot(unsigned int count, float turn,float *pos,float *speed){
+ float stepRY = pos[5];
+ float stepLY = pos[5];
+ if(turn > 0){
+ stepRY += turn;
+ stepLY -= turn;
+ }else if(turn < 0){
+ stepRY += turn;
+ stepLY -= turn;
}
-
- upZ = 100;
- offsetZ= 0;
-
- setSite(0, defaultX, forwardDefaultY, downZ[0]);
- setSite(1, defaultX, forwardDefaultY, downZ[1]);
- setSite(2, defaultX, backDefaultY, downZ[2]);
- setSite(3, defaultX, backDefaultY, downZ[3]);
- allMotionWait();
+ moveSpeed = speed[0];
+ while(count-- > 0){
+ //0,3
+ setSite(0, pos[0], pos[1] + stepRY, pos[3]-pos[6]);
+ setSite(1, pos[0], pos[1], pos[3]);
+ setSite(2, pos[0], pos[2], pos[3]);
+ setSite(3, pos[0], pos[2] + stepLY, pos[3]-pos[7]);
+ allMotionWait();
+ setSite(0, pos[0], pos[1] + stepRY, pos[3]);
+ setSite(3, pos[0], pos[2] + stepLY, pos[4]);
+ allMotionWait();
+ //1,2
+ setSite(0, pos[0], pos[1], pos[3]);
+ setSite(1, pos[0], pos[1] + stepLY, pos[3]-pos[6]);
+ setSite(2, pos[0], pos[2] + stepRY, pos[4]-pos[7]);
+ setSite(3, pos[0], pos[2], pos[3]);
+ allMotionWait();
+ setSite(1, pos[0], pos[1] + stepLY, pos[3]);
+ setSite(2, pos[0], pos[2] + stepRY, pos[4]);
+ allMotionWait();
+ }
}
-
-void standUp(){
- defaultX = 210;
- forwardDefaultY = 120;
- backDefaultY = -120;
+//初期姿勢
+void initPos(){
+ moveSpeed = 1;
+ float pos[3] = {150,0,100};
for(int i=0;i<4;i++){
- downZ[i] = 180;
+ for(int j=0;j<3;j++){
+ currentSite[i][j] = pos[j];
+ nextSite[i][j] = pos[j];
+ }
}
-
- upZ = 100;
- offsetZ= 0;
- const float standZ = 400;
- moveSpeed = 0.5;
- setSite(0, defaultX, forwardDefaultY, standZ);
- setSite(1, defaultX, forwardDefaultY, standZ);
- setSite(2, defaultX, backDefaultY, standZ);
- setSite(3, defaultX, backDefaultY, standZ);
- allMotionWait();
}
+//---------------------------------------------------------------
int main() {
- serServo[0].init();
- serServo[1].init();
for(int i=0;i<4;i++){
- leds[i] = 0;
- }
- for(int i=0;i<2;i++){
- currentSite[i][0] = defaultX;
- currentSite[i][1] = forwardDefaultY;
- currentSite[i][2] = downZ[i];
- nextSite[i][0] = defaultX;
- nextSite[i][1] = forwardDefaultY;
- nextSite[i][2] = downZ[i];
+ servo[i].init();
}
- for(int i=2;i<4;i++){
- currentSite[i][0] = defaultX;
- currentSite[i][1] = backDefaultY;
- currentSite[i][2] = downZ[i];
- nextSite[i][0] = defaultX;
- nextSite[i][1] = backDefaultY;
- nextSite[i][2] = downZ[i];
- }
+ wait(1.0);
+ initPos();
update.attach(&siteUpdate,0.02);//20msごとに座標値更新
- press.attach(&pressUpdate,0.1);
- wait(3.0);
+ wait(5.0);
+ float levelPosTest[8] = {170,60,-110,170,170,100,70,70};
+ float levelSpeTest[2] = {6,6};
+ float slopePosTest[8] = {200,40,-90,130,230,50,100,100};
+ float slopeSpeTest[2] = {4,4};
+ float slopePosTest2[8] = {180,60,-110,180,160,100,90,90};
+ float slopeSpeTest2[2] = {5,6};
+
while (true) {
- /*
- moveSpeed = 1;
- upZ = 190;
- setSite(3, defaultX, backDefaultY + 150, upZ);
+ forwardTrot(4,levelPosTest,levelSpeTest,0);
+ moveSpeed = 1.5;
+ setSite(0, 200, 100, 240);
+ setSite(1, 200, 100, 240);
+ setSite(2, 200, -20, 230);
+ setSite(3, 200, -20, 230);
+ allMotionWait();
+ setSite(0, 200, 100, 30);
+ allMotionWait();
+ setSite(0, 200, 200, 30);
+ allMotionWait();
+ setSite(0, 200, 200, 130);
+ allMotionWait();
+ setSite(1, 200, 100, 30);
allMotionWait();
- setSite(3, defaultX, backDefaultY , upZ);
+ setSite(1, 200, 200, 30);
+ allMotionWait();
+ setSite(1, 200, 200, 130);
allMotionWait();
- */
- /*
- setSite(0,defaultX+100,forwardDefaultY,50);
+ setSite(0, 200, 100, 130);
+ setSite(1, 200, 100, 130);
+ setSite(2, 200, -120, 230);
+ setSite(3, 200, -120, 230);
+ allMotionWait();
+ forwardTrot(4,slopePosTest,slopeSpeTest,0);
+ moveSpeed = 1.5;
+ setSite(0, 200, 70, 130);
+ setSite(1, 200, 70, 130);
+ setSite(2, 200, -150, 240);
+ setSite(3, 200, -150, 240);
allMotionWait();
- setSite(0,defaultX,forwardDefaultY+150,50);
+ setSite(2, 200, -120, 30);
+ allMotionWait();
+ setSite(2, 200, 0, 30);
+ allMotionWait();
+ setSite(2, 200, 0, 130);
allMotionWait();
- setSite(0,defaultX,forwardDefaultY+150,190);
- allMotionWait();
- wait(10.0);
- */
- /*
- pressFlag = true;
- moveSpeed = 1;
- setSite(0, defaultX, forwardDefaultY + 50, downZ);
- setSite(1, defaultX, forwardDefaultY, downZ);
- setSite(2, defaultX, backDefaultY + 50, downZ);
- setSite(3, defaultX, backDefaultY + 100, downZ);
- allMotionWait();
- pressFlag = false;
- setSite(1,defaultX+100, forwardDefaultY, 50);
+ setSite(0, 200, 20, 130);
+ setSite(1, 200, 20, 130);
+ setSite(2, 200, -100, 130);
+ setSite(3, 200, -200, 230);
allMotionWait();
- setSite(1,defaultX, forwardDefaultY+100, 50);
- allMotionWait();
- pressFlag = true;
- setSite(1,defaultX, forwardDefaultY+100, 300);
+
+ setSite(0, 200, 20, 30);
allMotionWait();
- */
- /*
- setSite(2,defaultX+100, backDefaultY+100, 50);
+ setSite(0, 200, 100, 30);
allMotionWait();
- setSite(2,defaultX, backDefaultY+100, 300);
- allMotionWait();
- */
- //setSite(2,defaultX+50, backDefaultY+100, upZ-30);
- /*
- setSite(3,defaultX, backDefaultY, downZ);
+ setSite(0, 200, 100, 200);
allMotionWait();
- setSite(3,defaultX+60, backDefaultY, upZ);
+ setSite(3, 200, -120, 30);
allMotionWait();
-
- setSite(3,defaultX+60, backDefaultY+100, upZ);
+ setSite(3, 200, 0, 30);
allMotionWait();
-
-
- setSite(3,defaultX, backDefaultY+100, downZ);
+ setSite(3, 200, 0, 130);
allMotionWait();
- */
- //setSite(3,defaultX, backDefaultY, downZ);
- //allMotionWait();
-
- //setSite(1,defaultX+60, forwardDefaultY, downZ-70);
-
- //setSite(2,defaultX, backDefaultY+100, downZ);
- //allMotionWait();
-
- //moveSpeed = 3;
- //setSite(3, 200, 0, 200);
- //allMotionWait();
+ setSite(1, 200, 20, 30);
+ allMotionWait();
+ setSite(1, 200, 200, 30);
+ allMotionWait();
+ setSite(1, 200, 200, 200);
+ allMotionWait();
/*
- pc.printf("%f \t",currentAngle[3][1]);
- pc.printf("%f \n",currentAngle[3][2]);
-
- setSite(3, 300, 0, 200);
+ setSite(0, 190, 150, 180);
+ setSite(1, 190, 150, 180);
+ setSite(2, 190, -150, 160);
+ setSite(3, 190, -150, 160);
allMotionWait();
- pc.printf("%f \t",currentAngle[3][1]);
- pc.printf("%f \n",currentAngle[3][2]);
*/
- forwardWalk1(100);
- //forwardWalk2(100);
- //backWalk1(100);
- //standDown();
- //standUp();
- //wait(30);
+ forwardTrot(10,slopePosTest2,slopeSpeTest2,0);
+
+ wait(10);
+ //backTrot(100,0);
}
}
\ No newline at end of file