File content as of revision 0:3fd90568b788:

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