Diff: BOUGYO.h

Revision:

0:4682992b55fa

diff -r 000000000000 -r 4682992b55fa BOUGYO.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BOUGYO.h	Thu Jan 30 05:24:10 2014 +0000
@@ -0,0 +1,91 @@
+void BOU(void const *argument) { // bougyo
+ int i=0;    
+    while(1) { 
+
+ float a=0.071f;    //sireiti
+ float b=0.109f;
+ float c=0.126f;
+ float max = maxof(henni(homeK,a),henni(homeH,b),henni(homeT,c));
+ float t = max / (0.015f / 0.17f * 0.03f); //nannbyou kakeruka 60(0.03duty)/0.17 servo /0.02s
+ if(bou==1){
+        switch(fei){
+        case 1:
+        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;
+        break;
+        
+        case 2: 
+        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;
+            i=1;
+            bou=0;
+            dousa=0;
+            pc.printf("end1\n");
+            }else;
+        break;
+        
+        case 3:
+        break;
+        }
+      }else;
+    }
+}
+
+
+
+//2go-ki bougyo
+void BOU2(void const *argument) {
+ int i=0;    
+    while(1) { 
+
+ float a=0.058f;    //sireiti
+ float b=0.118f;
+ float c=0.113f;
+ 
+ float max = maxof(henni(homeK2,a),henni(homeH2,b),henni(homeT2,c));
+ float t = max / (0.015f / 0.17f * 0.03f); //nannbyou kakeruka 60(0.03duty)/0.17 servo /0.02s
+ if(bou2==1){
+        switch(fei2){
+        case 1:
+        i=i+1;
+        Kata2 = homeK2 + (a - homeK2) / t * i;
+        Hiji2 = homeH2 + (b - homeH2) / t * i;
+        Te2 = homeT2 + (c - homeT2) / t * i;
+        wait(0.022);
+        if(i>t){
+        fei2=3;
+        }else;
+        break;
+        
+        case 2: 
+        i=i-1;
+        Kata2 = homeK2 + (a - homeK2) / t * i;
+        Hiji2 = homeH2 + (b - homeH2) / t * i;
+        Te2 = homeT2 + (c - homeT2) / t * i;
+        wait(0.022);
+        if(i<0){
+            fei2=1;
+            i=1;
+            bou2=0;
+            dousa2=0;
+            pc.printf("end2\n");
+            }else;
+        break;
+        
+        case 3:
+        break;
+        }
+      }else;
+    }
+}