y w
/
moter_titech
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 7:1947b554f8e3
- Parent:
- 3:c92e21f305d8
- Child:
- 8:3eb58ae94a93
--- a/main.cpp Tue Jun 04 16:01:32 2013 +0100
+++ b/main.cpp Sat Dec 14 07:03:16 2013 +0000
@@ -1,21 +1,393 @@
#include "mbed.h"
#include "rtos.h"
+
+Serial pc(USBTX, USBRX);
+PwmOut Kata(p21);
+PwmOut Hiji(p22);
+PwmOut Te(p23);
+PwmOut Dou(p24);
+PwmOut Asi(p25);
+
+char c;
+
+int zen=0;
+int go=0;
+
+int dousa=0;
+int men=0;
+int bou=0;
+int tuk=0;
+int dou=0;
+int fei=1;
+
+float homeK = 0.028f; //home pos 0.045-0.105
+float homeH = 0.103f;
+float homeT = 0.045f;
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
+ float henni(float a, float b){
+ float temp = a;
+ temp = b - a;
+ return (temp);
+}
+
+ float maxof(float a, float b, float c){
+ float A = abs(a);
+ float B = abs(b);
+ float C = abs(c);
+ float max = A;
+ if (B >= max) max = B;
+ if (C >= max) max = C;
+ return (max);
+}
+
+ void HANTEI(void const *argument) {
+ while(true){
+ switch(c){
+
+ default:
+ c=pc.getc();
+ break;
+
+ case'z':
+ zen=1;
+ c=0;
+ pc.printf("zen\n");
+ break;
+
+ case'n':
+ zen=0;
+ go=0;
+ c=0;
+ pc.printf("stop\n");
+ break;
+
+ case'g':
+ go=1;
+ c=0;
+ pc.printf("go\n");
+ break;
+
+ case'm':
+ if(dousa==0){
+ men=1;
+ dousa=1;
+ pc.printf("men\n");
+ }
+ c=0;
+ break;
+
+ case'b':
+ if(dousa==0){
+ bou=1;
+ dousa=1;
+ pc.printf("bou\n");
+ }
+ c=0;
+ break;
+
+ case't':
+ if(dousa==0){
+ tuk=1;
+ dousa=1;
+ pc.printf("tuk\n");
+ }
+ c=0;
+ break;
+
+ case'd':
+ if(dousa==0){
+ dou=1;
+ dousa=1;
+ pc.printf("dou\n");
+ }
+ c=0;
+ break;
+
+ case'f':
+ if((bou==0)&&(fei==1)&&(dousa==1)){
+ fei=2;
+ pc.printf("fei\n");
+ }
+ c=0;
+ break;
+
+ /*pc.printf("men=%d",men);
+ pc.printf("bou=%d",bou);
+ pc.printf("tuk=%d",tuk);
+ pc.printf("dou=%d",dou);
+ pc.printf("fei=%d\n",fei);
+ wait(0.5);*/
+ }
+ }
+}
+
+ void ZENGO(void const *argument) {
+ Asi.period(0.00001);
+ while(true){
+ if(zen==1){
+ Asi.pulsewidth(0.00000f);
+ }
+ else if(go==1){
+ Asi.pulsewidth(0.00001f);
+ }
+ else{
+ Asi.pulsewidth(0.000005f);
+ }
+ }
+}
+
+ void MEN(void const *argument) {
+ int i1=0;
+ float a1=0.055f; //sireiti
+ float b1=0.101f;
+ float c1=0.045f;
+ float max1 = maxof(henni(homeK,a1),henni(homeH,b1),henni(homeT,c1));
+ float t1 = max1 / (0.03f / 0.17f * 0.01f); //nannbyou kakeruka 60(0.03duty)/0.17 servo /0.02s
+
+
+ int i2=0;
+ float a2=0.028f; //sireiti
+ float b2=0.111f;
+ float c2=0.045f;
+ float max2 = maxof(henni(a1,a2),henni(b1,b2),henni(c1,c2));
+ float t2 = max2 / (0.03f / 0.17f * 0.01f); //nannbyou kakeruka 60(0.03duty)/0.17 servo /0.02s
+
-void led2_thread(void const *args) {
- while (true) {
- led2 = !led2;
- Thread::wait(1000);
+ while(true){
+ if(men==1){
+
+ switch(fei){
+ case 1: //men motion
+ i1=i1+1;
+ Kata = homeK + (a1 - homeK) / t1 * i1;
+ Hiji = homeH + (b1 - homeH) / t1 * i1;
+ Te = homeT + (c1 - homeT) / t1 * i1;
+ wait(0.022);
+ if(i1>t1){
+ fei=3;
+ }
+ break;
+
+ case 2: //feint
+ i1=i1-1;
+ Kata = homeK + (a1 - homeK) / t1 * i1;
+ Hiji = homeH + (b1 - homeH) / t1 * i1;
+ Te = homeT + (c1 - homeT) / t1 * i1;
+ wait(0.022);
+ if(i1<0){
+ fei=1;
+ men=0;
+ dousa=0;
+ i1=1;
+ }
+ break;
+
+ case 3: //men iki
+ i2=i2+1;
+ Kata = a1 + (a2 - a1) / t2 * i2;
+ Hiji = b1 + (b2 - b1) / t2 * i2;
+ Te = c1 + (c2 - c1) / t2 * i2;
+ wait(0.022);
+ if(i2>t2){
+ fei=4;
+ }
+ break;
+
+ case 4: //men kaeri
+ i2=i2-1;
+ Kata = a1 + (a2 - a1) / t2 * i2;
+ Hiji = b1 + (b2 - b1) / t2 * i2;
+ Te = c1+ (c2 - c1) / t2 * i2;
+ wait(0.022);
+ if(i2<0){
+ i2=1;
+ fei=5;
+ }
+ break;
+
+ case 5: //home kaeri
+ i1=i1-1;
+ Kata = homeK + (a1 - homeK) / t1 * i1;
+ Hiji = homeH + (b1 - homeH) / t1 * i1;
+ Te = homeT + (c1 - homeT) / t1 * i1;
+ wait(0.022);
+ if(i1<0){
+ i1=1;
+ fei=1;
+ dousa=0;
+ men=0;
+ }
+ break;
+ }
+ }
+ }
+}
+
+void BOUGYO(void const *argument) {
+ int i=0;
+ float a=0.075f; //sireiti
+ float b=0.100f;
+ float c=0.035f;
+ float max = maxof(henni(homeK,a),henni(homeH,b),henni(homeT,c));
+ float t = max / (0.03f / 0.17f * 0.01f); //nannbyou kakeruka 60(0.03duty)/0.17 servo /0.02s
+
+ while(true){
+ if(bou==1){
+
+ switch(fei){
+ case 1: //men iki
+ i=i+1;
+ Kata = homeK + (a - homeK) / t * i;
+ Hiji = homeH + (b - homeH) / t * i;
+ Te = homeT + (c - homeT) / t * i;
+ wait(0.022);
+ if(i>t){
+ fei=3;
+ }
+ break;
+
+ case 3: //men kaeri
+ i=i-1;
+ Kata = homeK + (a - homeK) / t * i;
+ Hiji = homeH + (b - homeH) / t * i;
+ Te = homeT + (c - homeT) / t * i;
+ wait(0.022);
+ if(i<0){
+ fei=1;
+ bou=0;
+ dousa=0;
+ i=1;
+ }
+ break;
+ }
+ }
+ }
+}
+
+void TUKI(void const *argument) {
+ int i=0;
+ float a=0.056f; //sireiti
+ float b=0.084f;
+ float c=0.035f;
+ float max = maxof(henni(homeK,a),henni(homeH,b),henni(homeT,c));
+ float t = max / (0.03f / 0.17f * 0.01f); //nannbyou kakeruka 60(0.03duty)/0.17 servo /0.02s
+
+ while(true){
+ if(tuk==1){
+
+ switch(fei){
+ case 1: //men iki
+ i=i+1;
+ Kata = homeK + (a - homeK) / t * i;
+ Hiji = homeH + (b - homeH) / t * i;
+ Te = homeT + (c - homeT) / t * i;
+ wait(0.022);
+ if(i>t){
+ fei=3;
+ }
+ break;
+
+ case 2: //feint
+ i=i-1;
+ Kata = homeK + (a - homeK) / t * i;
+ Hiji = homeH + (b - homeH) / t * i;
+ Te = homeT + (c - homeT) / t * i;
+ wait(0.022);
+ if(i<0){
+ fei=1;
+ tuk=0;
+ dousa=0;
+ i=1;
+ }
+ break;
+
+ case 3: //men kaeri
+ i=i-1;
+ Kata = homeK + (a - homeK) / t * i;
+ Hiji = homeH + (b - homeH) / t * i;
+ Te = homeT + (c - homeT) / t * i;
+ wait(0.022);
+ if(i<0){
+ fei=1;
+ tuk=0;
+ dousa=0;
+ i=1;
+ }
+ break;
+ }
+ }
+ }
+}
+
+void DOU(void const *argument) {
+ int i=0;
+ float a=0.047f; //sireiti
+ float b=0.087f;
+ float c=0.045f;
+ float max = maxof(henni(homeK,a),henni(homeH,b),henni(homeT,c));
+ float t = max / (0.03f / 0.17f * 0.01f); //nannbyou kakeruka 60(0.03duty)/0.17 servo /0.02s
+
+ while(true){
+ if(dou==1){
+
+ switch(fei){
+ case 1: //men iki
+ i=i+1;
+ Kata = homeK + (a - homeK) / t * i;
+ Hiji = homeH + (b - homeH) / t * i;
+ Te = homeT + (c - homeT) / t * i;
+ wait(0.022);
+ if(i>t){
+ fei=3;
+ }
+ break;
+
+ case 2: //feint
+ i=i-1;
+ Kata = homeK + (a - homeK) / t * i;
+ Hiji = homeH + (b - homeH) / t * i;
+ Te = homeT + (c - homeT) / t * i;
+ wait(0.022);
+ if(i<0){
+ fei=1;
+ dou=0;
+ dousa=0;
+ i=1;
+ }
+ break;
+
+ case 3: //men kaeri
+ i=i-1;
+ Kata = homeK + (a - homeK) / t * i;
+ Hiji = homeH + (b - homeH) / t * i;
+ Te = homeT + (c - homeT) / t * i;
+ wait(0.022);
+ if(i<0){
+ fei=1;
+ dou=0;
+ dousa=0;
+ i=1;
+ }
+ break;
+ }
+ }
}
}
int main() {
- Thread thread(led2_thread);
+ Thread t1(HANTEI);
+ Thread t2(ZENGO);
+ Thread t3(MEN);
+ Thread t4(BOUGYO);
+ Thread t5(TUKI);
+ Thread t6(DOU);
- while (true) {
- led1 = !led1;
- Thread::wait(500);
+ Kata=homeK;
+ Hiji=homeH;
+ Te=homeT;
+
+ pc.printf("startfm\n");
+
+ while (1){
}
-}
+}
\ No newline at end of file