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:
- 7:1947b554f8e3
- Parent:
- 3:c92e21f305d8
- Child:
- 8:3eb58ae94a93
diff -r 209f4db62daf -r 1947b554f8e3 main.cpp
--- 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