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:
- 8:3eb58ae94a93
- Parent:
- 7:1947b554f8e3
- Child:
- 9:215d94939bc9
diff -r 1947b554f8e3 -r 3eb58ae94a93 main.cpp
--- a/main.cpp Sat Dec 14 07:03:16 2013 +0000
+++ b/main.cpp Thu Jan 23 05:39:20 2014 +0000
@@ -2,392 +2,140 @@
#include "rtos.h"
Serial pc(USBTX, USBRX);
-PwmOut Kata(p21);
-PwmOut Hiji(p22);
-PwmOut Te(p23);
-PwmOut Dou(p24);
-PwmOut Asi(p25);
+PwmOut Dou1(p21);
+PwmOut Asi1(p23);
+PwmOut Dou2(p22);
+PwmOut Asi2(p24);
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;
-
- float henni(float a, float b){
- float temp = a;
- temp = b - a;
- return (temp);
-}
+int zen1=0;
+int go1=0;
+int zen2=0;
+int go2=0;
+int dou1=0;
- 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) {
+void HANTEI(void const *argument) {
while(true){
- switch(c){
-
- default:
+ if(pc.readable()==1){
c=pc.getc();
- break;
-
- case'z':
- zen=1;
+ switch(c){
+ case'g':
+ zen1=1;
c=0;
- pc.printf("zen\n");
+ pc.printf("go\n");
break;
case'n':
- zen=0;
- go=0;
+ zen1=0;
+ go1=0;
c=0;
pc.printf("stop\n");
break;
- case'g':
- go=1;
+ case'z':
+ go1=1;
+ c=0;
+ pc.printf("zen\n");
+ break;
+
+ case'c':
+ zen2=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'p':
+ zen2=0;
+ go2=0;
+ c=0;
+ pc.printf("stop\n");
+ break;
+
+ case'x':
+ go2=1;
+ c=0;
+ pc.printf("zen\n");
+ 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);*/
- }
+ dou1=1;
+ c=0;
+ pc.printf("dou\n");
+ break;
+ }
}
}
+}
- void ZENGO(void const *argument) {
- Asi.period(0.00001);
+
+ void ZENGO1(void const *argument) {
+ Asi1.period(0.00001);
while(true){
- if(zen==1){
- Asi.pulsewidth(0.00000f);
+ if(go1==1){
+ Asi1.pulsewidth(0.00000f);
}
- else if(go==1){
- Asi.pulsewidth(0.00001f);
+ else if(zen1==1){
+ Asi1.pulsewidth(0.00001f);
}
else{
- Asi.pulsewidth(0.000005f);
+ Asi1.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 ZENGO2(void const *argument) {
+ Asi2.period(0.00001);
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;
+ if(zen2==1){
+ Asi2.pulsewidth(0.000006f);
}
- 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;
+ else if(go2==1){
+ Asi2.pulsewidth(0.000004f);
}
- 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;
+ else{
+ Asi2.pulsewidth(0.000005f);
}
- }
- }
-}
-
-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;
- }
- }
}
}
+ void DOU1(void const *argument) {
+ Dou1.period(0.00001);
+ int i=1;
+ int d=1;
+ while(true){
+ if(dou1==1){
+ switch(d){
+ case 1:
+ Dou1.pulsewidth(0.000004f);
+ i=i++;
+ wait(0.02);
+ if(i>50)d=2;
+ break;
+
+ case 2:
+ Dou1.pulsewidth(0.000006f);
+ i=i--;
+ wait(0.02);
+ if(i>2){
+ d=1;
+ dou1=0;
+ }
+ break;
+ }
+ }
+ }
+}
+
+
int main() {
Thread t1(HANTEI);
- Thread t2(ZENGO);
- Thread t3(MEN);
- Thread t4(BOUGYO);
- Thread t5(TUKI);
- Thread t6(DOU);
+ Thread t2(ZENGO1);
+ Thread t3(ZENGO2);
+ Thread t4(DOU1);
+
+ pc.printf("start\n");
- Kata=homeK;
- Hiji=homeH;
- Te=homeT;
-
- pc.printf("startfm\n");
-
- while (1){
- }
+while(1);
}
\ No newline at end of file