use regulatoruse_regulator

1 files changed, 7 insertions, 4 deletions

diff --git a/client2.cpp b/client2.cpp
index 6f8e79d..e2f53b7 100644
--- a/client2.cpp
+++ b/client2.cpp
@@ -46,6 +46,7 @@
 #include <opencv2/opencv.hpp>
 #include "lib.h"
 #include "ringbuf.h"
+#include "regulator.h"
 
 #define PI 3.141593654
 
@@ -136,7 +137,7 @@ const char* drawFromCanvasFragmentSource =
 	"uniform float eye_pitch;\n"
 	"uniform float eye_roll;\n"
 	"const float aspect_ratio=1280./2/720.;\n"
-	"const float horiz_field_of_view=90/180.*3.141592654;\n"
+	"const float horiz_field_of_view=60/180.*3.141592654;\n"
 	"const float CAM_FX=1/2.0 / tan(horiz_field_of_view/2.0);\n"
 	"const mat3 eye_cal_inv = transpose(mat3(1/CAM_FX, 0, -1/2/CAM_FX,    0, 1/CAM_FX, -1/aspect_ratio/2/CAM_FX,     0,0,1));\n"
 	"const mat3 opencv_to_math = mat3(0,1,0,   0,0,1,   -1,0,0);\n"
@@ -568,7 +569,8 @@ void* video_fetcher_thread(void* ptr)
 	Ringbuffer delay_theta(DELAY_SIZE); // the sensor data
 	
 	int adjust_phi=0;//10;
-	
+	int time_now=0; // in msec
+	Regulator regulator(1./45.,0,0, 0);
 	
 	DroneConnection drone(SOCKETPATH);
 	navdata_t navdata;
@@ -700,8 +702,8 @@ void* video_fetcher_thread(void* ptr)
 		pthread_mutex_unlock(&my_mutex);
 
 		float yawdiff = fixup_angle((oculus_yaw-PI)*180./PI - yaw_cam);
-		float rotate = saturate(yawdiff/45., 1.);
-		drone.fly(0.,0.,0.,rotate);
+		regulator.put(yawdiff, time_now);
+		drone.fly(0.,0.,0.,saturate(regulator.get(),1.0));
 		
 
 
@@ -714,6 +716,7 @@ void* video_fetcher_thread(void* ptr)
 		oldgray = gray.clone();
 
 		usleep(40000);
+		time_now += 40;
 	}
 	return NULL;
 }