Naoyuki SAITO
/
moter_titech_ns
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.
Fork of
moter_titech
by 
y w
Diff: main.cpp
- Revision:
- 8:3eb58ae94a93
- Parent:
- 7:1947b554f8e3
- Child:
- 9:56d571c1cb9f
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