This is a one axis gimbal control program that takes roll angle from an IMU and moves the gimbal brushless motor accordingly.
Dependencies: MPU6050 brushlessController_TB6612FNG ledControl2 mbed
Diff: main.cpp
- Revision:
- 3:065a064b3453
- Parent:
- 2:f9f4d36c2367
- Child:
- 4:a041b7b5720b
diff -r f9f4d36c2367 -r 065a064b3453 main.cpp --- a/main.cpp Fri Jul 17 16:51:54 2015 +0000 +++ b/main.cpp Tue Jul 21 06:35:34 2015 +0000 @@ -43,8 +43,8 @@ float Kp = 1; float Ki = 0; float Kd = 0; -float set_point = -30; // which angle camera should stay -float errorMargin = 5; // error margin in degress +float set_point = -90; // which angle camera should stay +float errorMargin = 2; // error margin in degress float proportional = 0; float last_proportional =0; float integral = 0; @@ -61,10 +61,16 @@ mpu6050.init(); // Initialize the sensor pc.printf("MPU6050 is initialized for operation.. \r\n\r\n"); - filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period) - gimbal.attach(&gimbalPID, 0.001); // Call the gimbalPID func. every 1 ms + filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period) while(1) { + if(abs(errorPID) < 10) + gimbal.attach(&gimbalPID, 0.005); // Fine tuning + else + gimbal.attach(&gimbalPID, 0.001); // Coarse tuning + + pc.printf("%.1f,%.1f\r\n",rollAngle,pitchAngle); + wait_ms(40); } } @@ -76,7 +82,7 @@ void gimbalPID() { - bool dir; // direction of movement + bool dir; // direction of movement proportional = set_point - rollAngle; integral += proportional; @@ -85,7 +91,7 @@ errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative); (errorPID > 0)?(dir = 1):(dir = 0); - - if(abs(errorPID) > errorMargin) // Within error margin motor wont move. This is for stabilizing the gimbal system + + if(abs(errorPID) > errorMargin) // Within error margin motor wont move. This is for stabilizing the gimbal system brushlessControl(dir, 0); // No need for a delay time because gimbalPID function is periodic }