File content as of revision 5:62de7a36d3af:

#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;
int16_t x=0,y=0,throttle = 0,rudder = 0,i=0;
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;
}

void output()
{
    printf("\n|v|: %lf theta: %lf\n",v,argv); //最終的にだすやつ
    x = argv;
    y = v;
    i=2;
    
    wait(1);
    button = (i<<8) & 0x0F;

    joystick.update(throttle, rudder, x, y, button, hat);
}

int main()
{
    float avg1,avg2,timeavg1,timeavg2,deltat,a,b;
    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[ms]", 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[ms]\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[ms]",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[ms]\n",timeavg2);

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

            a=pow((avg2-avg1)/deltat,2);
            b=pow(10/deltat,2);
            v=sqrt(a+b);    //|v|を求める

            argv=atan(10/(avg2-avg1));  //θを求める
        }
        wait_ms(10);
    }

}

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

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