ave

Dependencies:   QEI TextLCD mbed

Revision:
0:3fd90568b788
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 17 01:49:27 2013 +0000
@@ -0,0 +1,289 @@
+#include "mbed.h"
+#include "math.h"
+#include "stdlib.h"
+#include "main.h"
+#include "QEI.h"
+#include "TextLCD.h"
+
+int state[2] = {0}, rope_step = 0, waiting = 0, led_check = 0;
+double rev_r, rev_l, rev_p, pp = 0, rope_p = 0, power = 0, rope_s = 0;
+
+//超音波読み取り(割り込み)
+void ping()
+{
+    Ultrasonic();
+}
+
+//回転速度読み取り(割り込み)
+void speed()
+{
+/*    static double pulse1 = 0, pulse2 = 0;
+    double test;
+    
+    pulse2 = pulse1;
+    pulse1 = fabs((double)enc_p.getPulses()/(360*4));
+    
+    rev_p = ave(2, pulse1 - pulse2);
+*/    
+    rev_l = ave(0, fabs((double)enc_l.getPulses())/(360*4));
+    rev_r = ave(1, fabs((double)enc_r.getPulses())/(360*4));
+    rev_p = ave(2, fabs((double)enc_p.getPulses())/(360*4));
+    
+    //printf("%03.4f   %03.4f    %f\n", rev_l, rev_r, pp);
+/*    
+    lcd.locate(2, 0);
+    lcd.printf("%1.5f", rope_p);
+    //lcd.printf("%03.4f", rev_l);
+    lcd.locate(0, 1);
+    lcd.printf("%03.4f", rev_p);
+*/
+/*    lcd.cls();
+    lcd.locate(2, 0);
+    lcd.printf("%d", enc_p.getPulses()/2/320);
+    lcd.locate(0, 1);
+    lcd.printf("%d", enc_p.getPulses());
+*/
+    
+    enc_l.reset();
+    enc_r.reset();
+    enc_p.reset();
+}
+
+//p制御
+double pid(double target, double sensor, double kp)
+{
+    double diff, p;
+    
+    diff = target - sensor;
+    
+    p = kp * diff;
+    
+    return p;
+}
+
+void ppp()
+{
+/*    if(state[WHEEL]){
+        pp += pid((rev_l - 0.01), rev_r, 0.05);
+        pwm[1] = pp;
+    }
+*/    
+    if(rope_s){
+        rope_p += pid(ROPE_S, rev_p, 0.001);
+        if(rope_p > ROPE_MAX){
+            rope_p = ROPE_MAX;
+        } else if(rope_p < ROPE_V){
+            rope_p = ROPE_V;
+        }
+        pwm[2] = rope_p;
+    }
+}
+
+void rope_steps()
+{
+    if(rope_step == 0){
+        pwm[2] = ROPE_V;
+        rope_s = 1;
+        state[ROPE] = 1;
+        rope_step++;
+    } else if(rope_step == 1){
+        pwm[2] = ROPE_L;
+        rope_s = 0;
+        rope_steper.attach(&rope_steps,0.8);
+        rope_step++;
+    } else {
+        rope = 0;
+        legs = 0;
+        state[ROPE] = 0;
+        rope_step = 0;
+    }
+ /*   
+    lcd.locate(0, 0);
+    lcd.printf("%d", rope_step);
+    */
+}
+
+void rope_wait()
+{
+    waiting = 0;
+}
+
+void wheel_stop()
+{
+    wheel = 0;
+}
+/*
+void ledwait()
+{
+    led_wait = 0;
+}
+*/
+/*
+void warikomi()
+{
+    static int rev = 0;
+    
+    rgbled = 0x2A;
+    wait(0.1);
+    rgbled = 0x15;
+    
+    rev++;
+    if(rev > 1){
+        myled[1] = 0;
+        rope_steps();
+        waiter.attach(&rope_wait,1);
+        waiting = 1;
+        rev = 0;
+    }
+    
+    //wait(0.1);
+}
+*/
+int main() {
+
+    int rev = 0;
+    double s_flag[4] = {0};
+    
+    timer2.start();
+    pinger.attach(&ping,0.1);
+    pider.attach(&ppp,0.01);
+    speeder.attach(&speed,0.1);
+    //sw.mode(PullUp);
+    
+    //sw.fall(&warikomi);
+    
+    pwm[0] = WHEEL_V;
+    pwm[1] = WHEEL_VL;
+    pwm[3] = 0.1;
+    pp = WHEEL_V;
+    rope_p = ROPE_V;
+    //pwm[1] = 0.2;
+    
+    start_led();
+    
+/*   
+    for(;;){
+        pc.printf("%05d  %05d  %05d  %05d\n", ultrasonicVal[0], ultrasonicVal[1], ultrasonicVal[2], ultrasonicVal[3]);
+        wait(0.1);
+    }
+*/
+    
+    while(1) {
+        //data = ave(0, ultrasonicVal[0]);
+        
+        //pc.printf("%06d  %06d  %06d  %06d\n", ultrasonicVal[0], ultrasonicVal[1], ultrasonicVal[2], ultrasonicVal[3]);
+        
+        if(ultrasonicVal[0] < TYONPA_S){
+            s_flag[0]++;
+        } else {
+            s_flag[0] = 0;
+        }
+        
+        if(ultrasonicVal[1] < TYONPA_S){
+            s_flag[1]++;
+        } else {
+            s_flag[1] = 0;
+        }
+        
+        if(ultrasonicVal[2] < TYONPA_U){
+            s_flag[2]++;
+        } else {
+            s_flag[2] = 0;
+        }
+        
+        if(ultrasonicVal[3] < TYONPA_U){
+            s_flag[3]++;
+        } else {
+            s_flag[3] = 0;
+        }
+        
+        
+        if(s_flag[0] > FLAG_W){
+            if(state[WHEEL] == 0){
+                wheel_stoper.detach();
+            }
+            myled[3] = 1;
+            state[WHEEL] = 1;
+            pwm[0] = WHEEL_V;
+            pwm[1] = WHEEL_VL;
+            wheel = 0x05;
+            rgbled = 0;
+        } else if(s_flag[1] > FLAG_W){
+            if(state[WHEEL] == 0){
+                wheel_stoper.detach();
+            }
+            myled[2] = 1;
+            state[WHEEL] = 1;
+            pwm[0] = WHEEL_V;
+            pwm[1] = WHEEL_VL;
+            wheel = 0x0A;
+            rgbled = 0;
+        } else {
+            if(state[WHEEL]){
+                wheel_stoper.attach(&wheel_stop,0.3);
+                state[WHEEL] = 0;
+                pwm[0] = 0;
+                pwm[1] = 0;
+                myled[3] = 0;
+                myled[2] = 0;
+            }
+            //wheel = 0;
+            rgbled = 0x15;
+        }
+        
+        if(s_flag[2] > FLAG_R){
+            if(waiting == 0){
+                if(state[ROPE] == 0){
+                    myled[1] = 1;
+                    rope = 1;
+                    pwm[2] = ROPE_V0;
+                    rope_steper.attach(&rope_steps,1.5);
+                    waiter.attach(&rope_wait,1);
+                    legs = 0x02;
+                    //wait(1);
+                } else {
+                    myled[1] = 0;
+                    rope_steps();
+                    waiter.attach(&rope_wait,1);
+                }
+                waiting = 1;
+            }
+        }
+        
+        if(s_flag[3] > FLAG_R){
+            rgbled = 0x2A;
+            wait(0.1);
+            rgbled = 0x15;
+        }
+        
+        if(sw == 0){
+            rgbled = 0x2A;
+            wait(0.1);
+            if(led_check == 0){
+                rev++;
+                led_check = 1;
+                //led_wait = 1;
+                //waiter.attach(&ledwait,0.2);
+                if(rev > 1){
+                    myled[1] = 0;
+                    rope_steps();
+                    waiter.attach(&rope_wait,1);
+                    waiting = 1;
+                    rev = 0;
+                }
+            }
+        } else {
+            led_check = 0;
+        }
+/*        
+        if((s_flag[0] > FLAG_W) || (s_flag[1] > FLAG_W)){
+            rgbled = 0x09;
+        } else if((sw == 0) || (s_flag[3] > FLAG_R)){
+            rgbled = 0x24;
+        } else {
+            rgbled = 0x12;
+            //0000000.wait(0.1);
+        }
+*/        
+    }
+}