//
Dependencies: MPU6050 brushlessMotor mbed
Fork of gimbalController_brushless_IMU by
main.cpp@8:c573f315319a, 2016-05-12 (annotated)
- Committer:
- alfaleader
- Date:
- Thu May 12 20:10:25 2016 +0000
- Revision:
- 8:c573f315319a
- Parent:
- 7:b65164847018
- Child:
- 9:2779500685cb
//
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
BaserK | 0:40b56bdec1d2 | 1 | #include "mbed.h" |
BaserK | 0:40b56bdec1d2 | 2 | #include "MPU6050.h" |
alfaleader | 8:c573f315319a | 3 | #include "brushlessMotor.h" |
BaserK | 0:40b56bdec1d2 | 4 | |
alfaleader | 8:c573f315319a | 5 | Serial pc(USBTX, USBRX); // Debug serial connectie (voor de computer) |
alfaleader | 8:c573f315319a | 6 | MPU6050 mpu6050; // mpu6050 object voor onze gyroscoop |
alfaleader | 8:c573f315319a | 7 | Ticker filter; // een Ticker object voor onze filter --> een ticker is een functie die x keer per seconde uitgevoerd gaat worden |
alfaleader | 8:c573f315319a | 8 | Ticker gimbal; // ticker voor de gimball |
alfaleader | 8:c573f315319a | 9 | Ticker speed; // ticker voor de snelheids controle |
BaserK | 0:40b56bdec1d2 | 10 | |
alfaleader | 8:c573f315319a | 11 | //Functie prototypes, zodat het programma weet dat we deze functies later gaan "invullen" |
BaserK | 0:40b56bdec1d2 | 12 | void compFilter(); |
BaserK | 2:f9f4d36c2367 | 13 | void gimbalPID(); |
BaserK | 5:477eaa33eff5 | 14 | void speedControl(); |
BaserK | 0:40b56bdec1d2 | 15 | |
alfaleader | 8:c573f315319a | 16 | //variabelen nodig voor ons programma |
BaserK | 0:40b56bdec1d2 | 17 | float pitchAngle = 0; |
BaserK | 0:40b56bdec1d2 | 18 | float rollAngle = 0; |
alfaleader | 8:c573f315319a | 19 | bool dir; // links of rechts |
alfaleader | 8:c573f315319a | 20 | bool stop; // motor moet stoppen? |
alfaleader | 8:c573f315319a | 21 | float delay; // delay tussen onze stappen |
alfaleader | 8:c573f315319a | 22 | |
alfaleader | 8:c573f315319a | 23 | //PID |
BaserK | 5:477eaa33eff5 | 24 | float Kp = 10; |
BaserK | 6:af164f09f963 | 25 | float Ki = 0.0001; |
BaserK | 6:af164f09f963 | 26 | float Kd = 5; |
alfaleader | 8:c573f315319a | 27 | float set_point = 0; // camera hoek |
BaserK | 2:f9f4d36c2367 | 28 | float proportional = 0; |
BaserK | 2:f9f4d36c2367 | 29 | float last_proportional =0; |
BaserK | 2:f9f4d36c2367 | 30 | float integral = 0; |
BaserK | 2:f9f4d36c2367 | 31 | float derivative = 0; |
alfaleader | 8:c573f315319a | 32 | float errorPID = 0; |
BaserK | 0:40b56bdec1d2 | 33 | |
BaserK | 1:2ae94169eee6 | 34 | int main() |
BaserK | 0:40b56bdec1d2 | 35 | { |
alfaleader | 8:c573f315319a | 36 | pc.baud(9600); // PC serial connectie maken voor debugging met baud rate 9600 |
alfaleader | 8:c573f315319a | 37 | mpu6050.whoAmI(); // Communicatie test? Zijn we wel verbonden met de MPU6050 of is er een probleem met de I2C connectie |
alfaleader | 8:c573f315319a | 38 | mpu6050.calibrate(accelBias,gyroBias); // Calibreren en upload deze waarden in het MPU6050 bias register, dit is onze 0 waarde waar de camera moet izjn |
alfaleader | 8:c573f315319a | 39 | pc.printf("Calibratie compleet \r\n"); |
alfaleader | 8:c573f315319a | 40 | mpu6050.init(); // Initializeer de gyroscoop |
alfaleader | 8:c573f315319a | 41 | pc.printf("MPU6050 is klaar \r\n\r\n"); |
BaserK | 5:477eaa33eff5 | 42 | |
alfaleader | 8:c573f315319a | 43 | filter.attach(&compFilter, 0.005); // Elke 5ms onze complementary Filter aanroepen |
alfaleader | 8:c573f315319a | 44 | gimbal.attach(&gimbalPID, 0.005); // Gimbal aansturing ook elke 5ms aanroepen |
alfaleader | 8:c573f315319a | 45 | |
BaserK | 1:2ae94169eee6 | 46 | while(1) |
BaserK | 6:af164f09f963 | 47 | { |
alfaleader | 8:c573f315319a | 48 | pc.printf("Roll: %.1f, Pitch: %.1f\r\n",rollAngle,pitchAngle); //DEBUG |
alfaleader | 8:c573f315319a | 49 | wait_ms(500); //500ms wachten |
BaserK | 1:2ae94169eee6 | 50 | } |
BaserK | 0:40b56bdec1d2 | 51 | } |
BaserK | 0:40b56bdec1d2 | 52 | |
alfaleader | 8:c573f315319a | 53 | //elke 5ms de filter functie van de mpu6050 library aanroepen, berekent de pitch en de roll en voert deze in bij de variabelen pitchAngle en rollAngle |
BaserK | 2:f9f4d36c2367 | 54 | void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);} |
BaserK | 2:f9f4d36c2367 | 55 | |
alfaleader | 8:c573f315319a | 56 | //elke 5ms updaten we ook de gimball |
BaserK | 2:f9f4d36c2367 | 57 | void gimbalPID() |
BaserK | 2:f9f4d36c2367 | 58 | { |
BaserK | 2:f9f4d36c2367 | 59 | proportional = set_point - rollAngle; |
BaserK | 2:f9f4d36c2367 | 60 | integral += proportional; |
BaserK | 2:f9f4d36c2367 | 61 | derivative = proportional - last_proportional; |
BaserK | 2:f9f4d36c2367 | 62 | last_proportional = proportional; |
BaserK | 2:f9f4d36c2367 | 63 | |
BaserK | 2:f9f4d36c2367 | 64 | errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative); |
BaserK | 2:f9f4d36c2367 | 65 | (errorPID > 0)?(dir = 1):(dir = 0); |
BaserK | 5:477eaa33eff5 | 66 | |
alfaleader | 8:c573f315319a | 67 | // errorPID is restricted between -400 and 400 |
BaserK | 5:477eaa33eff5 | 68 | if(errorPID > 400) |
BaserK | 5:477eaa33eff5 | 69 | errorPID = 400; |
BaserK | 5:477eaa33eff5 | 70 | else if(errorPID < -400) |
BaserK | 5:477eaa33eff5 | 71 | errorPID = -400; |
BaserK | 3:065a064b3453 | 72 | |
BaserK | 5:477eaa33eff5 | 73 | stop = 0; |
alfaleader | 8:c573f315319a | 74 | delay = 0.1/abs(errorPID); // speed should be proportional to error, therefore time delay |
alfaleader | 8:c573f315319a | 75 | //between steps should be inverse proportional to error. |
BaserK | 5:477eaa33eff5 | 76 | |
BaserK | 5:477eaa33eff5 | 77 | if (abs(errorPID)< Kp/2) stop = 1; // 0.5 deg noise margin |
BaserK | 5:477eaa33eff5 | 78 | |
BaserK | 5:477eaa33eff5 | 79 | if(stop) // if the gimbal is within noise margin, dont move. |
BaserK | 5:477eaa33eff5 | 80 | speed.detach(); |
BaserK | 5:477eaa33eff5 | 81 | else |
alfaleader | 8:c573f315319a | 82 | speed.attach(&speedControl, delay); |
BaserK | 2:f9f4d36c2367 | 83 | } |
BaserK | 5:477eaa33eff5 | 84 | |
BaserK | 5:477eaa33eff5 | 85 | void speedControl() |
BaserK | 5:477eaa33eff5 | 86 | { |
BaserK | 5:477eaa33eff5 | 87 | brushlessControl(dir, 0); |
BaserK | 5:477eaa33eff5 | 88 | } |