//
Dependencies: MPU6050 brushlessMotor mbed
Fork of gimbalController_brushless_IMU by
main.cpp
- Committer:
- alfaleader
- Date:
- 2016-05-16
- Revision:
- 10:12f9371f3e0f
- Parent:
- 9:2779500685cb
File content as of revision 10:12f9371f3e0f:
#include "mbed.h" #include "MPU6050.h" #include "PID.h" #include "brushlessMotor.h" Serial pc(USBTX, USBRX); // Debug serial connectie (voor de computer) MPU6050 mpu6050; // mpu6050 object voor onze gyroscoop PIDControll pitchMotorPID; //PID controle aanmaken voor de pitchmotor brushlessMotor pitchMotor(D5, D6, D7); //motoraansturing per motor Ticker pitchMotorTicker; //ticker (functie die x keer per seconde gaat uitgevoerd worden) void pitchMotorFunction(); //functie voor deze ticker (deze functie vullen we later in) PIDControll rollMotorPID; //idem voor roll motor brushlessMotor rollMotor(D8, D9, D10); Ticker rollMotorTicker; void rollMotorFunction(); Ticker updatePID; // een Ticker object voor onze filter --> een ticker is een functie die x keer per seconde uitgevoerd gaat worden void updatePIDfunctie(); //variabelen nodig voor ons programma float pitchAngle = 0; float rollAngle = 0; int main() { pc.baud(9600); // PC serial connectie maken voor debugging met baud rate 9600 mpu6050.whoAmI(); // Communicatie test? Zijn we wel verbonden met de MPU6050 of is er een probleem met de I2C connectie mpu6050.calibrate(accelBias,gyroBias); // Calibreren en upload deze waarden in het MPU6050 bias register, dit is onze 0 waarde waar de camera moet izjn pc.printf("Calibratie compleet \r\n"); mpu6050.init(); // Initializeer de gyroscoop pc.printf("MPU6050 is klaar \r\n\r\n"); updatePID.attach(&updatePIDfunctie, 0.005); // Elke 5ms onze complementary Filter aanroepen while(1) { pc.printf("Roll: %.1f, Pitch: %.1f\r\n",rollAngle, pitchAngle); //DEBUG pc.printf("pitch: dir: %d, delay: %f\r\n",pitchMotorPID.dir, pitchMotorPID.delay); //DEBUG pc.printf("roll: dir: %d, delay: %f\r\n",rollMotorPID.dir, rollMotorPID.delay); //DEBUG wait_ms(500); //500ms wachten } } //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 void updatePIDfunctie() { mpu6050.complementaryFilter(&pitchAngle, &rollAngle); //lees de pitchangle en de rollangle uit vanuit de gyroscoop //voer de PID regelaar uit pitchMotorPID.PIDaanpassing(pitchAngle); rollMotorPID.PIDaanpassing(rollAngle); //pitch aanpassingen nodig? if(pitchMotorPID.stop){ // if the gimbal is within noise margin, dont move. pitchMotorTicker.detach(); }else{ pitchMotorTicker.attach(&pitchMotorFunction, pitchMotorPID.delay); } //roll motor aanpassingen nodig? if(rollMotorPID.stop){ // if the gimbal is within noise margin, dont move. rollMotorTicker.detach(); } else { rollMotorTicker.attach(&rollMotorFunction, rollMotorPID.delay); } } void pitchMotorFunction(){ pitchMotor.brushlessControl(pitchMotorPID.dir, 0); } void rollMotorFunction(){ rollMotor.brushlessControl(rollMotorPID.dir, 0); }