Diff: main.cpp

Revision:

7:ed304ba2712a

Parent:

6:28b7339892f9

Child:

8:59dd4860477f

--- a/main.cpp	Tue Sep 27 07:57:15 2016 +0000
+++ b/main.cpp	Tue Sep 27 12:50:45 2016 +0000
@@ -13,7 +13,8 @@
 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;
+float avg1,avg2,timeavg1,timeavg2,deltat,a,b;
+int16_t x=0,y=0,throttle = 0,rudder = 0,i=0,vmax,M_PI=3.14;
 int8_t button=0,hat = 0;
 long microsecondsToCentimeters(long microseconds);
 
@@ -21,15 +22,21 @@
 {
     flag = 0;
     tmr.reset();
-    sum1=0,sum2=0,count1=0,count2=0,timecount1=0,timecount2=0,timesum1=0,timesum2=0;
+    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()
 {
-    v=v*10;
+    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));  //θを求める
     printf("\n|v|: %lf theta: %lf\n",v,argv); //最終的にだすやつ
-    x = argv;
-    y = v;
+    x = (int)argv*M_PI/60;
+    y = (int)v/5*240-120;
     i=2;
     joystick.update(throttle, rudder, x, y, button, hat);
     wait(1);
@@ -40,7 +47,6 @@
 
 int main()
 {
-    float avg1,avg2,timeavg1,timeavg2,deltat,a,b;
     pc.baud(115200);
     while (1) {
         long duration1, duration2,USSDistance1, USSDistance2, cm1, cm2, time1,time2;
@@ -132,11 +138,7 @@
             deltat=timeavg2-timeavg1;   //Δtを求める
             printf("\ndeltat: %f\n",deltat);
 
-            a=pow((avg2-avg1)/deltat,2);
-            b=pow(0.1/deltat,2);
-            v=sqrt(a+b);    //|v|を求める
-
-            argv=atan(0.1/(avg2-avg1));  //θを求める
+           
         }
         wait_ms(10);
     }
@@ -151,4 +153,4 @@
 long microsecondsToCentimeters(long microseconds)
 {
     return microseconds / 29 / 2;
-}
\ No newline at end of file
+}