main.cpp

00001 #include "mbed.h"
00002 #include "stdio.h"
00003 #include "math.h"
00004 #include "USBJoystick.h"
00005 
00006 Serial pc(USBTX,USBRX);
00007 USBJoystick joystick;
00008 
00009 DigitalInOut pingPin1(p18);
00010 DigitalInOut pingPin2(p19);
00011 
00012 Timer tmr1, tmr2,tmr;
00013 Timeout timeouter1,timeouter2;
00014 int flag=0;
00015 float sum1=0,sum2=0,count1=0,count2=0,timecount1=0,timecount2=0,timesum1=0,timesum2=0,v,argv;
00016 float avg1,avg2,timeavg1,timeavg2,deltat,a,b;
00017 int16_t x=0,y=0,throttle = 0,rudder = 0,i=0,vmax;
00018 int8_t button=0,hat = 0;
00019 long microsecondsToCentimeters(long microseconds);
00020 
00021 void reset()
00022 {
00023     flag = 0;
00024     tmr.reset();
00025     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;
00026 }
00027 
00028 void output()
00029 {
00030     timeouter1.detach();
00031     a=pow((avg2-avg1)/deltat*10,2);
00032     b=pow(0.1/deltat*1000,2);
00033     v=sqrt(a+b);    //|v|を求める
00034     if(deltat==0) {
00035         vmax=5;
00036     }
00037     argv=atan(10/(avg2-avg1));  //θを求める
00038 
00039     x = argv/3.141516*60.0;
00040     //y = v/5.0*240.0-120.0;
00041     y = v/5.0*120.0;
00042     button=2;
00043     printf("\n|v|: %lf theta: %lf\n",v,argv); //最終的にだすやつ
00044     printf("\n x : %d y   : %d\n",x,y); //最終的にだすやつ
00045     joystick.move(x, y);
00046     wait_ms(10);
00047     joystick.button(2);
00048     wait(1);
00049     button = 0;
00050     //joystick.move(0, 0);
00051     wait_ms(10);
00052     joystick.button(0);
00053     reset();
00054 }
00055 
00056 int main()
00057 {
00058     pc.baud(115200);
00059     while (1) {
00060         long duration1, duration2,USSDistance1, USSDistance2, cm1, cm2, time1,time2;
00061 
00062         pingPin1.output();
00063         pingPin1 = 0;
00064         wait_us(2);
00065         pingPin1 = 1;
00066         wait_us(5);
00067         pingPin1 = 0;
00068 
00069         pingPin1.input(); // pulseIn
00070         tmr1.reset();
00071 
00072         while (!pingPin1); // wait for high
00073         tmr1.start();
00074         while (pingPin1); // wait for low
00075         tmr1.stop();
00076         duration1 = tmr1.read_us();
00077         USSDistance1 = duration1;// * 0.0170;
00078 
00079         cm1 = microsecondsToCentimeters(USSDistance1);
00080 
00081         if(15<cm1&&cm1<40) {
00082             if(flag==0) {
00083                 flag=1;
00084                 tmr.reset();
00085                 tmr.start();
00086                 timeouter2.attach(&output, 1);
00087                 timeouter1.attach(&reset, 5);
00088             }
00089             if(flag==1) {
00090                 timeouter1.detach();
00091                 timeouter2.detach();
00092                 timeouter2.attach(&output, 1);
00093                 timeouter1.attach(&reset, 5);
00094             }
00095             time1=tmr.read_ms();
00096             printf("1: %d  %d[s]", cm1,time1);
00097 
00098             count1++;
00099             timecount1++;
00100 
00101             sum1=sum1+cm1;   //cm1の値を合計する
00102             avg1=sum1/count1;    //cm1の平均
00103             printf("   avg1: %f",avg1);
00104 
00105             timesum1=timesum1+time1;    //time1を合計する
00106             timeavg1=timesum1/timecount1;   //time1の平均
00107             printf("   timeavg1: %f[s]\n",timeavg1);
00108         }
00109 
00110         wait_ms(10);
00111 
00112         pingPin2.output();
00113         pingPin2 = 0;
00114         wait_us(2);
00115         pingPin2 = 1;
00116         wait_us(5);
00117         pingPin2 = 0;
00118 
00119         pingPin2.input(); // pulseIn
00120         tmr2.reset();
00121 
00122         while (!pingPin2); // wait for high
00123         tmr2.start();
00124         while (pingPin2); // wait for low
00125         tmr2.stop();
00126 
00127         duration2 = tmr2.read_us();
00128         USSDistance2 = duration2;// * 0.0170;
00129 
00130         cm2 = microsecondsToCentimeters(USSDistance2);
00131         if(15<cm2&&cm2<40) {
00132             time2=tmr.read_ms();
00133             printf("                                              2: %d %d[s]",cm2,time2);
00134 
00135             count2++;
00136             timecount2++;
00137 
00138             sum2=sum2+cm2;   //cm2の値を合計する
00139             avg2=sum2/count2;    //cm2の平均
00140             printf("   avg2: %f",avg2);
00141 
00142             timesum2=timesum2+time2;    //time2を合計する
00143             timeavg2=timesum2/timecount2;   //time2の平均
00144             printf("   timeavg2:%f[s]\n",timeavg2);
00145 
00146             deltat=timeavg2-timeavg1;   //Δtを求める
00147             printf("\ndeltat: %f\n",deltat);
00148 
00149 
00150         }
00151         wait_ms(10);
00152     }
00153 
00154 }
00155 
00156 long microsecondsToInches(long microseconds)
00157 {
00158     return microseconds / 74 / 2;
00159 }
00160 
00161 long microsecondsToCentimeters(long microseconds)
00162 {
00163     return microseconds / 29 / 2;
00164 }