Fixed PWM
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of Sequential_Timing by
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