File content as of revision 13:a06ce818a615:

#include "mbed.h"
#include "stdio.h"
#include "math.h"
#include "USBJoystick.h"

Serial pc(USBTX,USBRX);
USBJoystick joystick;

DigitalInOut pingPin1(p18);
DigitalInOut pingPin2(p19);

Timer tmr1, tmr2,tmr;
Timeout timeouter1,timeouter2;
int flag=0;
float sum1=0,sum2=0,count1=0,count2=0,timecount1=0,timecount2=0,timesum1=0,timesum2=0,v,argv;
float avg1,avg2,timeavg1,timeavg2,deltat,a,b;
int16_t x=0,y=0,throttle = 0,rudder = 0,i=0,vmax;
int8_t button=0,hat = 0;
long microsecondsToCentimeters(long microseconds);

void reset()
{
    flag = 0;
    tmr.reset();
    sum1=0,sum2=0,count1=0,count2=0,timecount1=0,timecount2=0,timesum1=0,timesum2=0,avg1=0,avg2=0,timeavg1=0,timeavg2=0,deltat=0,a=0,b=0;
}

void output()
{
    timeouter1.detach();
    a=pow((avg2-avg1)/deltat*10,2);
    b=pow(0.1/deltat*1000,2);
    v=sqrt(a+b);    //|v|を求める
    if(deltat==0) {
        vmax=5;
    }
    argv=atan(10/(avg2-avg1));  //θを求める

    x = argv/3.141516*60.0;
    //y = v/5.0*240.0-120.0;
    y = v/5.0*120.0;
    button=2;
    printf("\n|v|: %lf theta: %lf\n",v,argv); //最終的にだすやつ
    printf("\n x : %d y   : %d\n",x,y); //最終的にだすやつ
    joystick.move(x, y);
    wait_ms(10);
    joystick.button(2);
    wait(1);
    button = 0;
    //joystick.move(0, 0);
    wait_ms(10);
    joystick.button(0);
    reset();
}

int main()
{
    pc.baud(115200);
    while (1) {
        long duration1, duration2,USSDistance1, USSDistance2, cm1, cm2, time1,time2;

        pingPin1.output();
        pingPin1 = 0;
        wait_us(2);
        pingPin1 = 1;
        wait_us(5);
        pingPin1 = 0;

        pingPin1.input(); // pulseIn
        tmr1.reset();

        while (!pingPin1); // wait for high
        tmr1.start();
        while (pingPin1); // wait for low
        tmr1.stop();
        duration1 = tmr1.read_us();
        USSDistance1 = duration1;// * 0.0170;

        cm1 = microsecondsToCentimeters(USSDistance1);

        if(15<cm1&&cm1<40) {
            if(flag==0) {
                flag=1;
                tmr.reset();
                tmr.start();
                timeouter2.attach(&output, 1);
                timeouter1.attach(&reset, 5);
            }
            if(flag==1) {
                timeouter1.detach();
                timeouter2.detach();
                timeouter2.attach(&output, 1);
                timeouter1.attach(&reset, 5);
            }
            time1=tmr.read_ms();
            printf("1: %d  %d[s]", cm1,time1);

            count1++;
            timecount1++;

            sum1=sum1+cm1;   //cm1の値を合計する
            avg1=sum1/count1;    //cm1の平均
            printf("   avg1: %f",avg1);

            timesum1=timesum1+time1;    //time1を合計する
            timeavg1=timesum1/timecount1;   //time1の平均
            printf("   timeavg1: %f[s]\n",timeavg1);
        }

        wait_ms(10);

        pingPin2.output();
        pingPin2 = 0;
        wait_us(2);
        pingPin2 = 1;
        wait_us(5);
        pingPin2 = 0;

        pingPin2.input(); // pulseIn
        tmr2.reset();

        while (!pingPin2); // wait for high
        tmr2.start();
        while (pingPin2); // wait for low
        tmr2.stop();

        duration2 = tmr2.read_us();
        USSDistance2 = duration2;// * 0.0170;

        cm2 = microsecondsToCentimeters(USSDistance2);
        if(15<cm2&&cm2<40) {
            time2=tmr.read_ms();
            printf("                                              2: %d %d[s]",cm2,time2);

            count2++;
            timecount2++;

            sum2=sum2+cm2;   //cm2の値を合計する
            avg2=sum2/count2;    //cm2の平均
            printf("   avg2: %f",avg2);

            timesum2=timesum2+time2;    //time2を合計する
            timeavg2=timesum2/timecount2;   //time2の平均
            printf("   timeavg2:%f[s]\n",timeavg2);

            deltat=timeavg2-timeavg1;   //Δtを求める
            printf("\ndeltat: %f\n",deltat);


        }
        wait_ms(10);
    }

}

long microsecondsToInches(long microseconds)
{
    return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds)
{
    return microseconds / 29 / 2;
}