//
Dependencies: MPU6050 brushlessMotor mbed
Fork of gimbalController_brushless_IMU by
Diff: main.cpp
- Revision:
- 10:12f9371f3e0f
- Parent:
- 9:2779500685cb
diff -r 2779500685cb -r 12f9371f3e0f main.cpp --- a/main.cpp Sun May 15 16:15:44 2016 +0000 +++ b/main.cpp Mon May 16 09:50:44 2016 +0000 @@ -31,12 +31,14 @@ 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("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 } } @@ -50,22 +52,24 @@ rollMotorPID.PIDaanpassing(rollAngle); //pitch aanpassingen nodig? - if(pitchMotorPID.stop) // if the gimbal is within noise margin, dont move. + if(pitchMotorPID.stop){ // if the gimbal is within noise margin, dont move. pitchMotorTicker.detach(); - else + }else{ pitchMotorTicker.attach(&pitchMotorFunction, pitchMotorPID.delay); + } //roll motor aanpassingen nodig? - if(rollMotorPID.stop) // if the gimbal is within noise margin, dont move. + if(rollMotorPID.stop){ // if the gimbal is within noise margin, dont move. rollMotorTicker.detach(); - else - rollMotorTicker.attach(&rollMotorFunction, rollMotorPID.delay); + } else { + rollMotorTicker.attach(&rollMotorFunction, rollMotorPID.delay); + } } void pitchMotorFunction(){ - pitchMotor.brushlessControl(rollMotorPID.dir, 0); + pitchMotor.brushlessControl(pitchMotorPID.dir, 0); } void rollMotorFunction(){