Diff: foot.cpp

Revision:

0:3dc012104243

Child:

4:9ba47e5db1e2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/foot.cpp	Tue Sep 24 13:35:31 2019 +0000
@@ -0,0 +1,142 @@
+#include "mbed.h"
+#include "foot.h"
+/*
+PwmOut RF(D3);
+PwmOut LF(D5);
+PwmOut RB(D6);
+PwmOut LB(D9);
+PwmOut pwms[4] = {RF,LF,RB,LB};
+*/
+//
+DigitalOut dig(PG_9);
+PwmOut RFA(PB_4);//3/1
+PwmOut RFB(PB_5);//3/2
+PwmOut LFA(PC_8);//3/3
+PwmOut LFB(PC_9);//3/4
+PwmOut RBA(PD_12);//4/1
+PwmOut RBB(PD_13);//4/2
+PwmOut LBA(PD_14);//4/3
+PwmOut LBB(PD_15);//4/4
+PwmOut pwms[8] = {RFA,RFB,LFA,LFB,RBA,RBB,LBA,LBB};
+PwmOut lockR(PB_10);
+PwmOut lockL(PB_11);
+PwmOut lockpwms[2] = {lockR,lockL};
+//new
+//PB_6 -> PD_12
+//PC_7 -> PB_5
+
+/*
+// PB_4 -> PA_6/////
+// PB_6 -> PD_12
+*/
+/*
+DigitalOut dig(PG_9);
+PwmOut RFA(PB_4);//3/1
+PwmOut RFB(PC_7);//3/2
+PwmOut LFA(PC_8);//3/3
+PwmOut LFB(PC_9);//3/4
+PwmOut RBA(PB_6);//4/1
+PwmOut RBB(PD_13);//4/2
+PwmOut LBA(PD_14);//4/3
+PwmOut LBB(PD_15);//4/4
+*/
+int Write[][4] {
+  {255, 255, 255, 255}, //0前
+  { -255, -255, -255, -255}, //1後ろ
+  { -255, 255, 255, -255}, //2右
+  {255, -255, -255, 255}, //3左
+  {0, 255, 255, 0}, //4右前
+  {255, 0, 0, 255}, //5左前
+  { -255, 0, 0, -255}, //6右後ろ
+  {0, -255, -255, 0}, //7左後ろ
+  { -255, 255, -255, 255}, //8右旋回
+  {255, -255, 255, -255},//9左旋回
+  {500,500,500,500}
+};
+float pwm_g[] = {1.1052f,1,1.0421f,1.0526f};
+
+void Move(int way,float pwm){
+    int num;
+    for(int i =0;i<4;i++){
+        num = Write[way][i];
+        if(num > 0)dig = 0;
+        else if(num < 0)dig = 1;
+        if(num != 0)pwms[i] = pwm;
+        else pwms[i] = 0;
+        }
+    }
+int count_Move = 0;
+int count_Move_lock = 0;
+void Move2(int way,float pwm){
+    int num;
+    if(count_Move == 0){
+    for(int i =0;i<8;i++){
+        pwms[i].period_ms(1);
+        }
+        count_Move = 1;
+    }
+    for(int i =0;i<4;i++){
+        num = Write[way][i];
+        if(num == 255){
+            pwms[i*2] = pwm*pwm_g[i];
+            pwms[i*2+1] = 0;
+        }else if(num == -255){
+            pwms[i*2] = 0;
+            pwms[i*2+1] = pwm*pwm_g[i];
+        }else if(num == 500){
+            pwms[i*2] = 1;
+            pwms[i*2+1] = 1;
+            }
+        }
+    }
+    
+void Move_lock(int way,float pwm){
+    int num;
+    if(count_Move_lock == 0){
+    for(int i =0;i<2;i++){
+        lockpwms[i].period_ms(1);
+        }
+        count_Move_lock = 1;
+    }
+    for(int i =0;i<1;i++){
+        num = Write[way][i];
+        if(num == 255){
+            pwms[i*2] = pwm;
+            pwms[i*2+1] = 0;
+        }else if(num == -255){
+            pwms[i*2] = 0;
+            pwms[i*2+1] = pwm;
+        }
+        }
+    }
+
+void trans(int a,int dis,float *x,float *y,int *sign){
+    if(a == 0){
+        *y=dis;
+        *x = 0;
+    }else if(a == 1){
+        *y = -dis;
+        *x = 0;
+    }else if(a == 2){
+        *y = 0;
+        *x = dis;
+    }else if(a == 3){
+        *y = 0;
+        *x = -dis;
+    }else if(a == 4){
+        *y = dis/1.414;
+        *x = dis/1.414;
+    }else if(a == 5){
+        *y = dis/1.414;
+        *x = -dis/1.414;
+    }else if(a == 6){
+        *y = -dis/1.414;
+        *x = -dis/1.414;
+    }else if(a == 7){
+        *y = -dis/1.414;
+        *x = dis/1.414;
+        }
+    *sign = 0;
+    if(*x < 0)*sign = 0b10;
+    if(*y < 0)*sign = *sign | 0b01;
+    }
\ No newline at end of file