ケンタ ミヤザキ
nhk2019
Dependencies: mbed kondoSerialServo
Diff: main.cpp
- Revision:
- 0:ffe56c419abd
- Child:
- 1:5fb6895ad8e6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Mar 10 09:07:27 2019 +0000
@@ -0,0 +1,541 @@
+#include "mbed.h"
+#include <math.h>
+#include "SerialServo.h"
+
+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)
+};
+
+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 bodyLength = 161.72;
+
+float PI = 3.1415926;
+//---各足の長さ----
+const float L1 = 76;
+const float L2 = 155.5;
+const float L3 = 290;
+
+//------移動速度--------
+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 currentSite[4][3];
+//次の座標
+float nextSite[4][3];
+//値更新用スピード
+float tempSpeed[4][3];
+
+//現在の角度
+float currentAngle[4][3];
+
+//現在の周波数
+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];
+ }
+ }
+ float length[3] = {0.0,0.0,0.0};
+ length[0] = x - currentSite[leg][0];
+ length[1] = y - currentSite[leg][1];
+ length[2] = z - currentSite[leg][2];
+ float L = sqrt(pow(length[0],2)+pow(length[1],2)+pow(length[2],2));
+ tempSpeed[leg][0] = (length[0]/L)*moveSpeed;
+ tempSpeed[leg][1] = (length[1]/L)*moveSpeed;
+ tempSpeed[leg][2] = (length[2]/L)*moveSpeed;
+ nextSite[leg][0] = x;
+ nextSite[leg][1] = y;
+ nextSite[leg][2] = z;
+}
+
+void toAngle(int leg){
+ float x = currentSite[leg][0];
+ float y = currentSite[leg][1];
+ float z = currentSite[leg][2];
+ float L = sqrt((x*x)+(y*y));
+ float w = L - L1;
+ float v = sqrt((w*w)+(z*z));
+ float alpha = atan2(z,w) - acos(((v*v)+(L2*L2)-(L3*L3))/(2*L2*v));
+ float beta = PI - acos(((L2*L2) + (L3*L3) - (v*v))/(2*L2*L3));
+ float gamma = atan2(y, x);
+ currentAngle[leg][0] = gamma = (gamma / PI * 180);
+ currentAngle[leg][1] = alpha = (alpha / PI * 180);
+ 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;
+ }
+ }
+ //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);
+ }
+}
+
+void motionWait(int leg){
+ bool flag = false;
+ while(flag == false){
+ wait_us(0.001);
+ if(currentSite[leg][0] == nextSite[leg][0]){
+ if(currentSite[leg][1] == nextSite[leg][1]){
+ if(currentSite[leg][2] == nextSite[leg][2]){
+ flag = true;
+ }
+ }
+ }
+ }
+}
+
+//すべての足が動き終わるまで待機
+void allMotionWait(){
+ for(int i=0;i<4;i++){
+ 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;
+ while(count-- > 0){
+ //2
+ moveSpeed = legSpeed;
+ setSite(2,defaultX,backDefaultY,upZ);
+ allMotionWait();
+ setSite(2,defaultX+100,backDefaultY+stepY*2,upZ);
+ allMotionWait();
+ setSite(2,defaultX,backDefaultY+stepY*2,downZ[2],1);
+ allMotionWait();
+ //0
+ moveSpeed = legSpeed;
+ setSite(0, defaultX+100, forwardDefaultY, upZ);
+ allMotionWait();
+ setSite(0, defaultX, forwardDefaultY + stepY*2, upZ);
+ allMotionWait();
+ setSite(0, defaultX, forwardDefaultY + stepY*2, downZ[0],1);
+ 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);
+ allMotionWait();
+
+ //3
+ moveSpeed = legSpeed;
+ setSite(3,defaultX,backDefaultY,upZ);
+ allMotionWait();
+ setSite(3,defaultX+100,backDefaultY+stepY*2,upZ);
+ allMotionWait();
+ setSite(3,defaultX,backDefaultY+stepY*2,downZ[3],1);
+ allMotionWait();
+
+ //1
+ moveSpeed = legSpeed;
+ setSite(1, defaultX+100, forwardDefaultY, upZ);
+ allMotionWait();
+ setSite(1, defaultX, forwardDefaultY + stepY*2, upZ);
+ allMotionWait();
+ setSite(1, defaultX, forwardDefaultY + stepY*2, downZ[1],1);
+ 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);
+ 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 forwardWalk2(unsigned int count){
+ defaultX = 210;
+ stepY = 100;
+ forwardDefaultY = 0;
+ backDefaultY = -120;
+ for(int i=0;i<4;i++){
+ downZ[i] = 180;
+ }
+ 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);
+ 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]);
+ 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]);
+ allMotionWait();
+ }
+}
+
+void standDown(){
+ defaultX = 210;
+ forwardDefaultY = 120;
+ backDefaultY = -120;
+ for(int i=0;i<4;i++){
+ downZ[i] = 180;
+ }
+
+ 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();
+}
+
+void standUp(){
+ defaultX = 210;
+ forwardDefaultY = 120;
+ backDefaultY = -120;
+ for(int i=0;i<4;i++){
+ downZ[i] = 180;
+ }
+
+ 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];
+ }
+ 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];
+ }
+ update.attach(&siteUpdate,0.02);//20msごとに座標値更新
+ press.attach(&pressUpdate,0.1);
+ wait(3.0);
+ while (true) {
+ /*
+ moveSpeed = 1;
+ upZ = 190;
+ setSite(3, defaultX, backDefaultY + 150, upZ);
+ allMotionWait();
+ setSite(3, defaultX, backDefaultY , upZ);
+ allMotionWait();
+ */
+ /*
+ setSite(0,defaultX+100,forwardDefaultY,50);
+ allMotionWait();
+
+ setSite(0,defaultX,forwardDefaultY+150,50);
+ 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);
+ allMotionWait();
+ setSite(1,defaultX, forwardDefaultY+100, 50);
+ allMotionWait();
+ pressFlag = true;
+ setSite(1,defaultX, forwardDefaultY+100, 300);
+ allMotionWait();
+ */
+ /*
+ setSite(2,defaultX+100, backDefaultY+100, 50);
+ allMotionWait();
+ setSite(2,defaultX, backDefaultY+100, 300);
+ allMotionWait();
+ */
+ //setSite(2,defaultX+50, backDefaultY+100, upZ-30);
+ /*
+ setSite(3,defaultX, backDefaultY, downZ);
+ allMotionWait();
+
+ setSite(3,defaultX+60, backDefaultY, upZ);
+ allMotionWait();
+
+ setSite(3,defaultX+60, backDefaultY+100, upZ);
+ allMotionWait();
+
+
+ setSite(3,defaultX, backDefaultY+100, downZ);
+ 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();
+ /*
+ pc.printf("%f \t",currentAngle[3][1]);
+ pc.printf("%f \n",currentAngle[3][2]);
+
+ setSite(3, 300, 0, 200);
+ 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);
+ }
+}
\ No newline at end of file