Revision 0:3fd90568b788, committed 2013-10-17

Comitter:

com3

Date:

Thu Oct 17 01:49:27 2013 +0000

Commit message:

ave

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Thu Oct 17 01:49:27 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Thu Oct 17 01:49:27 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/simon/code/TextLCD/#44f34c09bd37

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/average/ave.cpp	Thu Oct 17 01:49:27 2013 +0000
@@ -0,0 +1,19 @@
+#include "mbed.h"
+
+#define KAZU 3
+
+double ave(int num, double value)
+{
+    static double sum[KAZU] = {0};
+    static double data[KAZU][5] = {{0}};
+    
+    sum[num] -= data[num][4];
+    sum[num] += value;
+    data[num][4] = data[num][3];
+    data[num][3] = data[num][2];
+    data[num][2] = data[num][1];
+    data[num][1] = data[num][0];
+    data[num][0] = value;
+    
+    return sum[num]/5;
+}
\ No newline at end of file

--- /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);
+        }
+*/        
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Thu Oct 17 01:49:27 2013 +0000
@@ -0,0 +1,53 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "TextLCD.h"
+
+#define WHEEL   0
+#define WHEEL_V 0.5     //left
+#define WHEEL_VL 0.2    //right
+#define FLAG_W  3
+
+#define ROPE    1
+#define ROPE_V0 0.18
+#define ROPE_MAX    0.08
+#define ROPE_V  0.05
+#define ROPE_S  0.05
+#define ROPE_L  0.001
+#define FLAG_R  5
+
+#define TYONPA_S    700
+#define TYONPA_U    600
+#define FLAG    5
+
+Timer timer2;
+
+extern double ultrasonicValue[4];
+extern uint16_t ultrasonicVal[4];
+extern void Ultrasonic(void);
+extern void start_led(void);
+extern double ave(int num, double value);
+
+
+BusOut wheel(p9, p10, p11, p12);
+BusOut rope(p13, p14);
+BusOut legs(p15, p16);
+BusOut rgbled(p17, p18, p19);
+PwmOut pwm[4] = {p21, p22, p23, p24};
+DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
+DigitalIn sw(p20);
+//InterruptIn sw(p24);
+QEI enc_l(p25, p26, NC, 360, QEI::X4_ENCODING);
+QEI enc_r(p27, p28, NC, 360, QEI::X4_ENCODING);
+QEI enc_p(p29, p30, NC, 360, QEI::X4_ENCODING);
+//TextLCD lcd(p30, p29, p28, p27, p26, p25);
+
+Ticker speeder;
+Ticker pider;
+Ticker pinger;
+
+Timeout rope_steper;
+Timeout rbleder;
+Timeout waiter;
+Timeout wheel_stoper;
+
+Serial pc(USBTX, USBRX);
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Oct 17 01:49:27 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/9c8f0e3462fb
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ping/ping.cpp	Thu Oct 17 01:49:27 2013 +0000
@@ -0,0 +1,49 @@
+#include "mbed.h"
+#include "ping.h"
+ 
+//DigitalOut myled = LED1; 
+ 
+extern Timer timer2;
+
+uint16_t ultrasonicVal[ALL_ULTRASONIC];
+double ultrasonicValue[ALL_ULTRASONIC] = {0};
+
+
+void Ultrasonic()
+{
+    for(int i = 0 ; i < ALL_ULTRASONIC; i++){
+       
+        uint8_t flag = 0;
+    
+        DigitalOut PingPinOut(ultrasonic_pin[i]);
+        PingPinOut = 1;
+        wait_us(10);
+        PingPinOut = 0;
+        DigitalIn PingPin(ultrasonic_pin[i]);
+        timer2.reset();
+        while(PingPin == 0){
+            if(timer2.read_us() > 1500){   //1.5ms以上応答なし
+                ultrasonicValue[i] =  PING_ERR;
+                flag = 1;
+                break;
+            }
+        } 
+  
+        timer2.reset();
+        while(PingPin == 1){
+            if((timer2.read_us() > 18500) || (flag == 1)){  //18.5ms以上のパルス
+                ultrasonicValue[i] =  PING_ERR;
+                flag = 1;
+                break;
+            }
+        }
+      
+        if(flag == 0){
+            ultrasonicValue[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //mm  MAX:3145
+            ultrasonicVal[i] = (int)(ultrasonicValue[i] * 10.0);
+        }else{
+            ultrasonicVal[i] = PING_ERR;
+        }
+
+    }
+} 
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ping/ping.h	Thu Oct 17 01:49:27 2013 +0000
@@ -0,0 +1,24 @@
+#define ALL_ULTRASONIC  4
+#define PING_ERR        0xFFFF
+
+PinName ultrasonic_pin[ALL_ULTRASONIC] = {
+    p5,
+    p6,
+    p7,
+    p8,
+};
+
+
+
+/*
+#define ALL_ULTRASONIC  4
+#define PING_ERR        0xFFFF
+
+
+PinName ultrasonic_pin[ALL_ULTRASONIC] = {
+    p17,
+    p18,
+    p19,
+    p20,
+};
+*/

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/start_led.cpp	Thu Oct 17 01:49:27 2013 +0000
@@ -0,0 +1,16 @@
+#include "mbed.h"
+
+BusOut myleds(LED1, LED2, LED3, LED4);
+
+void start_led()
+{
+    myleds = 0x08;
+    wait(1);
+    myleds = 0x04;
+    wait(1);
+    myleds = 0x02;
+    wait(1);
+    myleds = 0x01;
+    wait(1);
+    myleds = 0;
+}
\ No newline at end of file