Diff: main.cpp

Revision:

9:10fcf3d0ec2d

Parent:

8:71c39d2f80e2

Child:

10:716484b1ddb5

--- a/main.cpp	Fri Apr 01 21:41:41 2016 +0000
+++ b/main.cpp	Fri Apr 01 23:47:07 2016 +0000
@@ -55,10 +55,10 @@
 // ========
 // Velocity
 // ========
-float Kp = 3.0;                                     // Proportional factor
-float Ki = 1;                                       // Integral factor
-float Kd = 0.0001;                                       // Derivative factor
-float interval = 0.01;                              // Sampling interval
+float Kp = 6.0;                                     // Proportional factor
+float Ki = 0;                                       // Integral factor
+float Kd = 0;                                       // Derivative factor
+float interval = 0.1;                              // Sampling interval
 float ref_v = 1.0;
 PID motor_ctrl(Kp, Ki, Kd, interval);               // Motor controller
 
@@ -98,6 +98,16 @@
 // Functions
 // ================
 
+void tele_comm(void const *args) {
+    telemetry_serial.baud(115200);
+    telemetry_obj.transmit_header();
+    while (true) {
+        tele_time_ms = t.read_ms();
+        telemetry_obj.do_io();
+        Thread::wait(100);
+    }
+}
+
 // Communications
 //void communication(void const *args) {
 //    while (true) {
@@ -274,6 +284,7 @@
 // Main
 // ====
 int main() {
+    osThreadSetPriority(osThreadGetId(), osPriorityRealTime);
     t.start();
     
     // Initialize motor
@@ -284,10 +295,6 @@
     servo.calibrate(0.001, 45.0);
     servo = a / 180.0;
     
-    telemetry_serial.baud(115200);
-    telemetry_serial.printf("t_h\r\n");
-    telemetry_obj.transmit_header();
-    
     // Initialize & start camera
     clk = 0;
     read_camera();
@@ -301,17 +308,20 @@
     
     // Initialize bluetooth
 //    bt.baud(115200);
+
+//    osThreadSetPriority(osThreadGetId(), osPriorityAboveNormal);
     
     // Initialize communications thread
 //    Thread communication_thread(communication);
-    
+    Thread tele_comm_thread(tele_comm);
+//    tele_comm_thread.set_priority(osPriorityBelowNormal);
     
     // Start motor controller
     while (true) {
 //        telemetry_serial.printf("%d\r\n", t.read_ms());
 //        if (dec == 100) {
-            tele_time_ms = t.read_ms();
-            telemetry_obj.do_io();
+            //tele_time_ms = t.read_ms();
+//            telemetry_obj.do_io();
 //            dec = 0;
 //        }
 //        dec = dec + 1;
@@ -331,7 +341,7 @@
         motor_ctrl.setProcessValue(velocity);
         d = motor_ctrl.compute();
         motor = 1.0 - d;
-        wait(interval);
+        Thread::wait(interval*1000);
 //        pc.printf("Exposure: %d, Center: %d, Angle: %f\r\n", t_int, center, a);
     }
 }
\ No newline at end of file