Montvydas Klumbys
Program used to control a quadcopter. It uses a PID library which can be found in: http://developer.mbed.org/cookbook/PID I also uses my own written library for easily controlling quadcopter motors, which can be found in: https://developer.mbed.org/users/moklumbys/code/Quadcopter/ One more library that I used is MPU6050 that was previously written by Erik Olieman but I was able to update it with new functions: https://developer.mbed.org/users/moklumbys/code/MPU6050/
Dependencies: MPU6050 PID Quadcopter Servo mbed
Diff: main.cpp
- Revision:
- 4:eb418af66d81
- Parent:
- 3:5f43c8374ff2
- Child:
- 5:8b3f82abe3a4
--- a/main.cpp Thu Feb 19 12:20:06 2015 +0000
+++ b/main.cpp Thu Feb 19 23:30:15 2015 +0000
@@ -1,71 +1,74 @@
-
-//firstly enable calibrating
#include "mbed.h"
#include "Quadcopter.h"
#include "PID.h"
#include "MPU6050.h"
#include "Timer.h"
-#define MAX_CALIBRATE 1.0
-#define MIN_CALIBRATE 0.35
+//defines the number of samples to be taken when calculating the offset for gyro and accelerometer
+#define OFFSET_SAMPLES 50
-#define OFFSET_SAMPLES 50
//define how the accelerometer is placed on surface
#define X_AXIS 1
#define Y_AXIS 2
#define Z_AXIS 0
+//ESC calibration values
#define MAX_CALIBRATE 1.0
#define MIN_CALIBRATE 0.35
+//Just to remember which motor corresponds to which number...
#define FL 0 // Front left
#define FR 1 // Front right
#define BL 2 // back left
#define BR 3 // back right
+//input and output values for pitch
#define PITCH_IN_MIN -90.0
#define PITCH_IN_MAX 90.0
#define PITCH_OUT_MIN -0.3
#define PITCH_OUT_MAX 0.3
+//input and output values for roll
#define ROLL_IN_MIN -90.0
#define ROLL_IN_MAX 90.0
#define ROLL_OUT_MIN -0.3
#define ROLL_OUT_MAX 0.3
+//PID intervals/constants
#define Kc 0.5
#define Ti 0.01
#define Td 0.00
#define RATE 0.01
+
//--------------------------------ALL THE FUNCTION HEADERS-----------------------
-float map(float x, float in_min, float in_max, float out_min, float out_max);
+float map(float x, float in_min, float in_max, float out_min, float out_max); //might be a useful function. One is used inside Quadcopter library though
//---------------------------------------END-------------------------------------
-Quadcopter quad (p21, p22, p23, p24);
-Serial pc(USBTX, USBRX); // tx, rx
-MPU6050 mpu(p9, p10); //Also disables sleep mode
-Timer timer;
+Quadcopter quad (p21, p22, p23, p24); //intance of the Quadcopter class
+Serial pc(USBTX, USBRX); // tx, rx
+MPU6050 mpu(p9, p10); //Also disables sleep mode
+Timer timer; //need a timer to tell how much time passed from the last calculation
-//Kc, Ti, Td, interval
+//put Kc, Ti, Td, interval for both pitch and roll PID models
PID pitchPID (Kc, Ti, Td, RATE);
PID rollPID (Kc, Ti, Td, RATE);
//***************************************STARTING MAIN FUNCTION*********************
int main() {
- pc.baud (115200);
+ pc.baud (115200); //fast transmition speed...
- float pitchDiff;
- float rollDiff;
+ float pitchDiff; //difference in pitch. Explained in PID library...
+ float rollDiff; //diference in roll
- float speed[4];
- float actSpeed[4];
+ float speed[4]; //speed for motors
+ float actSpeed[4]; //actual speed of for all motors
float accOffset[3]; //offset values
float gyroOffset[3];
float angle[3]; //total calculated angle
- float currTime;
- float prevTime;
+ float currTime; //current time variable will be given in the funtion
+ float prevTime; //previous time values will be given in the function
if (mpu.testConnection()) //just testing if things are working...
pc.printf("MPU connection succeeded\n\r");
@@ -74,7 +77,7 @@
mpu.setAlpha(0.97); //set Alpha coefficient for low/high pass filters
-// quad.calibrate(MIN_CALIBRATE, MAX_CALIBRATE); //calibrating motors
+ quad.calibrate(MIN_CALIBRATE, MAX_CALIBRATE); //calibrating motors
pitchPID.setInputLimits (PITCH_IN_MIN, PITCH_IN_MAX); //seting input and output limits for both pitch and roll
pitchPID.setOutputLimits (PITCH_OUT_MIN, PITCH_OUT_MAX);
@@ -86,64 +89,50 @@
rollPID.setMode(AUTO_MODE);
//need to vary this one to move quadcopter
- pitchPID.setSetPoint (0.0); //seting the middle point.. Or smth like that, need to look into it more
- rollPID.setSetPoint (0.0); //meaning that quadcopter is flying at a constant place.. no turning, blah blah blah
+ pitchPID.setSetPoint (0.0); //seting the middle point meaning that quadcopter is balancing in one place
+ rollPID.setSetPoint (0.0);
mpu.getOffset(accOffset, gyroOffset, OFFSET_SAMPLES); //take some samples at the beginning to get an offset
- wait(0.1); //wait to settle down
+ wait(0.1); //wait to settle down
- timer.start(); //will need timer to detect when was the last time the values were updated
- prevTime = timer.read();
+ timer.start(); //will need timer to detect when was the last time the values were updated
+ prevTime = timer.read(); //set previous timer value
- for (int i = 0; i < 4; i++){
+ for (int i = 0; i < 4; i++){ //initialise speed to be 0.0
speed[i] = 0.0;
}
+
+// for (int i = 0; i < 4; i++){ //start running motors ar 20% jus to make sure everything works fine
+// speed[i] = 0.2;
+// }
+// quad.run (speed);
//-------------------------------------------START INFINITE LOOP-------------------------------------------------
-pitchPID.setSetPoint (45.0); //head forward! by 45 degrees :D
while(1) {
-// for (float height = 0.0; height < 0.5; height+=0.05){
-// for (int i = 0; i < 4; i++){
-// speed[i] = height;
-// }
-// quad.run (speed);
-// wait(0.1);
-// }
-// for (uint16_t i = 0; i < 600; i++)
-// {
- currTime = timer.read(); //get present time
- mpu.computeAngle (angle, accOffset, gyroOffset, &currTime, &prevTime); // get angle using all these values
+ currTime = timer.read(); //get present time
+
+ mpu.computeAngle (angle, accOffset, gyroOffset, &currTime, &prevTime); // get angle using all these values
rollPID.setInterval(timer.read()-prevTime); //need to change the interval because don't know how much time passed
pitchPID.setInterval(timer.read()-prevTime);
prevTime = timer.read(); //get present time -> will be used later on as previous value
- rollPID.setProcessValue (angle[X_AXIS]); //take some values to do processing
+ rollPID.setProcessValue (angle[X_AXIS]); //take angle values, which correspond to pitch and roll and do processing
pitchPID.setProcessValue (angle[Y_AXIS]);
pitchDiff = pitchPID.compute(); //compute the difference
rollDiff = rollPID.compute();
- pc.printf ("pitchDiff=%0.4f, rollDiff=%0.4f\n", pitchDiff, rollDiff);
- quad.stabilise(speed, actSpeed, rollDiff, pitchDiff); //stabilise the speed by giving new actSpeed value
+
+ pc.printf ("pitchDiff=%0.4f, rollDiff=%0.4f\n", pitchDiff, rollDiff);
+ quad.stabilise(speed, actSpeed, rollDiff, pitchDiff); //stabilise the speed by giving out actual Speed value
//print some values to check how thing work out
// pc.printf("x=%0.3f y=%0.3f z=%0.3f\n", angle[X_AXIS], angle[Y_AXIS], angle[Z_AXIS]);
// pc.printf("Speed_FL=%0.4f, Speed_FR=%0.4f, Speed_BL= %0.4f, Speed_BR=%0.4f\n", speed[FL], speed[FR], speed[BL], speed[BR]);
// pc.printf("ActSpeed_FL=%0.4f, ActSpeed_FR=%0.4f, ActSpeed_BL=%0.4f, ActSpeed_BR=%0.4f\n", actSpeed[FL], actSpeed[FR], actSpeed[BL], actSpeed[BR]);
- // quad.run(actSpeed); //run the motors at the spesified speed actSpeed
-
+ quad.run(actSpeed); //run the motors at the spesified speed actSpeed
wait (0.01);
-// }
-
-// for (float height = 0.5; height > 0.0; height-=0.05){
-// for (int i = 0; i < 4; i++){
-// speed[i] = height;
-// }
-// quad.run (speed);
-// wait(0.1);
-// }
-// wait(1);
}
}
//************************************************END MAIN FUNCTION********************************************************