Karim Azzouz / Mbed 2 deprecated A-Quad

A quadcopter control Software (Still in development). achieved single axis stability!!!!! released for others benefit. if you'd like to help co-develop this code, then please let me know

Dependencies:   MovingAverageFilter MyI2C PID RC mbed-rtos mbed

Currently on hold, due to the fact that i don't own a RX/TX system

Files at this revision

API Documentation at this revision

Revision 0:54b67cd15a5b, committed 2012-12-23

Comitter:
KarimAzzouz
Date:
Sun Dec 23 23:50:21 2012 +0000
Child:
1:e08a4f517989
Commit message:
PPM interrupts clashing with i2c causing crashes!!

Changed in this revision

FastPWM.lib Show annotated file Show diff for this revision Revisions of this file
GlobalDefines.h Show annotated file Show diff for this revision Revisions of this file
MPU.h Show annotated file Show diff for this revision Revisions of this file
Motors.h Show annotated file Show diff for this revision Revisions of this file
PulseIn.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-rtos.lib Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastPWM.lib	Sun Dec 23 23:50:21 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Sissors/code/FastPWM/#3094d3806cfc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GlobalDefines.h	Sun Dec 23 23:50:21 2012 +0000
@@ -0,0 +1,23 @@
+#include "mbed.h"
+#include "PulseIn.h"
+#include "FastPWM.h"
+
+#define ToRad(x) (x*0.01745329252)  // *pi/180
+#define ToDeg(x) (x*57.2957795131)  // *180/pi
+
+//// Motor Pins /////
+FastPWM FrontMotor(p21); // Front Motor Pin
+FastPWM RearMotor(p22);  // Rear Motor Pin
+FastPWM LeftMotor(p23); // Left Motor Pin
+FastPWM RightMotor(p24);// Right Motor Pin
+/////////////////////
+
+DigitalOut one(LED1);
+PulseIn Throttle(p11);
+
+Timer Global;
+
+int16_t RX_Data[4];
+float pitch,roll,deltaTime;
+/////PID Variables/////
+float PID[3],error[3],I[3],D[3];
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU.h	Sun Dec 23 23:50:21 2012 +0000
@@ -0,0 +1,173 @@
+// Contains all Sensor related functions : initializing the sensor, calibrating sensors, getting data , accelerometer angles , and filtering gyro/acc data 
+
+#define MPU_ADDRESS 0x68
+#define GyroScale  16.4
+#define AccScale   4096
+
+I2C i2c(p9,p10);
+
+float filtered_AccX[4],filtered_AccY[4],filtered_AccZ[4],filtered_GyroX[4],filtered_GyroY[4],filtered_GyroZ[4]; 
+float Scaled_GyroX,Scaled_GyroY,Scaled_GyroZ;
+float Scaled_AccX,Scaled_AccY,Scaled_AccZ;
+float GyroOffset[3],AccOffset[3];
+float accangle[2];
+
+void write(char reg,char data){
+    
+    char tx[2]={reg,data};
+    
+    i2c.write((MPU_ADDRESS << 1)&0xFE, tx, 2);
+    
+ }
+
+int read (char reg){
+
+    char tx = reg;
+    char rx[2];
+    
+    i2c.write((MPU_ADDRESS << 1)&0xFE , &tx,1);
+    
+    i2c.read((MPU_ADDRESS << 1)|0x01, rx, 2);
+    
+    
+    int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
+ 
+    return output;
+}
+
+void MPU_Setup(){
+       
+       
+       write(0x6B,0x80); // Restart 
+       wait_ms(5);
+       write(0x6B,0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 1 (PLL with Z Gyro reference) 
+       write(0x1B,0x18); //GYRO_CONFIG   -- FS_SEL = 2 : Scale = +/-2000 deg/sec
+       write(0x1C,0x10); //ACCEL_CONFIG  -- AFS_SEL=2  : Scale = +/-8G
+       write(0x1A,0x03); //DLPF Settings -- 44Hz Accelerometer Bandwidth 42Hz Gyroscope Bandwidth 
+       wait_ms(60);
+}
+
+void ScaledGyro(){
+   
+    Scaled_GyroX = -read(0x45)/GyroScale;
+    Scaled_GyroY =  read(0x43)/GyroScale;
+    Scaled_GyroZ =  read(0x47)/GyroScale;
+}
+
+void ScaledAcc(){
+   
+   Scaled_AccX = (float)read(0x3B)/AccScale;
+   Scaled_AccY = (float)read(0x3D)/AccScale;
+   Scaled_AccZ = (float)read(0x3F)/AccScale;
+}
+
+ void CalibrateGyro(){
+ 
+     for(int k=0;k<200;k++){
+     ScaledGyro();
+     GyroOffset[0]+=Scaled_GyroX;
+     GyroOffset[1]+=Scaled_GyroY;
+     GyroOffset[2]+=Scaled_GyroZ;
+     wait_ms(1);
+     }
+     GyroOffset[0]/=200;
+     GyroOffset[1]/=200;
+     GyroOffset[2]/=200;
+ }
+ 
+  void CalibrateAcc(){
+ 
+     for(int k=0;k<200;k++){
+     ScaledAcc();
+     AccOffset[0]+=Scaled_AccX;
+     AccOffset[1]+=Scaled_AccY;
+     AccOffset[2]+=Scaled_AccZ;
+     wait_ms(1);
+     }
+     AccOffset[0]/=200;
+     AccOffset[1]/=200;
+     AccOffset[2]/=200;
+     AccOffset[2]-=1;
+ }
+ 
+ void GyroRate(){
+    
+    ScaledGyro();
+    Scaled_GyroX-=GyroOffset[0];
+    Scaled_GyroY-=GyroOffset[1];
+    Scaled_GyroZ-=GyroOffset[2];
+ }
+ 
+ void Acc(){
+ 
+    ScaledAcc();
+    Scaled_AccX-=AccOffset[0];
+    Scaled_AccY-=AccOffset[1];
+    Scaled_AccZ-=AccOffset[2];
+ }
+   void filterGyro(void){
+      
+       GyroRate();
+    
+    //////////////////////////////////////// Filter X Axis ////////////////////////////////////////////////////////////////
+    filtered_GyroX[0] = Scaled_GyroX;
+    Scaled_GyroX      = filtered_GyroX[0]*0.25 + filtered_GyroX[1]*0.25 + filtered_GyroX[2]*0.25 + filtered_GyroX[3]*0.25;
+    filtered_GyroX[3] = filtered_GyroX[2];
+    filtered_GyroX[2] = filtered_GyroX[1];
+    filtered_GyroX[1] = filtered_GyroX[0];
+    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    
+    /////////////////////////////////////// Filter Y Axis ////////////////////////////////////////////////////////////////
+    filtered_GyroY[0] = Scaled_GyroY;
+    Scaled_GyroY      = filtered_GyroY[0]*0.25 + filtered_GyroY[1]*0.25 + filtered_GyroY[2]*0.25 + filtered_GyroY[3]*0.25;
+    filtered_GyroY[3] = filtered_GyroY[2];
+    filtered_GyroY[2] = filtered_GyroY[1];
+    filtered_GyroY[1] = filtered_GyroY[0];
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    
+    /////////////////////////////////////// Filter Z Axis ////////////////////////////////////////////////////////////////
+    filtered_GyroZ[0] = Scaled_GyroZ;
+    Scaled_GyroZ      = filtered_GyroZ[0]*0.25 + filtered_GyroZ[1]*0.25 + filtered_GyroZ[2]*0.25 + filtered_GyroZ[3]*0.25;
+    filtered_GyroZ[3] = filtered_GyroZ[2];
+    filtered_GyroZ[2] = filtered_GyroZ[1];
+    filtered_GyroZ[1] = filtered_GyroZ[0];
+    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ }
+   
+   
+ void filterAcc(void){
+      
+       Acc();
+    
+    //////////////////////////////////////// Filter X Axis///////////////////////////////////////////////////////////
+    filtered_AccX[0] = Scaled_AccX;
+    Scaled_AccX      = filtered_AccX[0]*0.25 + filtered_AccX[1]*0.25 + filtered_AccX[2]*0.25 + filtered_AccX[3]*0.25;
+    filtered_AccX[3] = filtered_AccX[2];
+    filtered_AccX[2] = filtered_AccX[1];
+    filtered_AccX[1] = filtered_AccX[0];
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    
+    //////////////////////////////////////// Filter Y Axis //////////////////////////////////////////////////////////
+    filtered_AccY[0] = Scaled_AccY;
+    Scaled_AccY      = filtered_AccY[0]*0.25 + filtered_AccY[1]*0.25 + filtered_AccY[2]*0.25 + filtered_AccY[3]*0.25;
+    filtered_AccY[3] = filtered_AccY[2];
+    filtered_AccY[2] = filtered_AccY[1];
+    filtered_AccY[1] = filtered_AccY[0];
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    
+    //////////////////////////////////////// Filter Z Axis//////////////////////////////////////////////////////////
+    filtered_AccZ[0] = Scaled_AccZ;
+    Scaled_AccZ      = filtered_AccZ[0]*0.25 + filtered_AccZ[1]*0.25 + filtered_AccZ[2]*0.25 + filtered_AccZ[3]*0.25;
+    filtered_AccZ[3] = filtered_AccZ[2];
+    filtered_AccZ[2] = filtered_AccZ[1];
+    filtered_AccZ[1] = filtered_AccZ[0];
+    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ }
+ 
+  void AccAngle(){
+  
+   filterAcc();
+   
+   accangle[0] = ToDeg(atan2(Scaled_AccX,sqrt(Scaled_AccY*Scaled_AccY+Scaled_AccZ*Scaled_AccZ)));
+   accangle[1] = ToDeg(atan2(Scaled_AccY,sqrt(Scaled_AccX*Scaled_AccX+Scaled_AccZ*Scaled_AccZ)));
+ }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motors.h	Sun Dec 23 23:50:21 2012 +0000
@@ -0,0 +1,57 @@
+#define Kp 1
+#define Ki 1
+#define Kd 1
+
+#define PITCH 0
+#define ROLL 1
+#define YAW 2
+
+   int constrain(int a , int b, int c){
+   
+   if(a<b) return b;
+   else if(a>c)return c;
+   else return a;
+   }
+
+   void InitializeMotors(void){
+   
+    FrontMotor.period_ms(20);
+    RearMotor.period_ms(20);
+    LeftMotor.period_ms(20);
+    RightMotor.period_ms(20);
+   
+    FrontMotor.pulsewidth_us(1000);
+    RearMotor.pulsewidth_us(1000);
+    LeftMotor.pulsewidth_us(1000);
+    RightMotor.pulsewidth_us(1000);
+    
+    wait(3); // let the hardware settle
+ }
+ 
+  void updatePID(void){ 
+  
+    error[PITCH]= RX_Data[PITCH] - pitch;
+    error[ROLL]= RX_Data[ROLL] - roll ;
+    error[YAW]= RX_Data[YAW] - Scaled_GyroZ;
+    
+    I[PITCH]+= error[PITCH]*Ki;
+    I[ROLL]+= error[ROLL]*Ki;
+    I[YAW]+= error[YAW]*Ki;
+    
+    D[PITCH] = Scaled_GyroX;
+    D[ROLL] = Scaled_GyroY;
+    D[YAW] = Scaled_GyroZ;
+    
+    PID[PITCH] = error[PITCH]*Kp + I[PITCH] + D[PITCH]*Kd ; // Pitch correction
+    PID[ROLL]  = error[ROLL]*Kp + I[ROLL] + D[ROLL]*Kd ;    // Roll correction
+    PID[YAW]   = error[YAW] ;                               // Yaw correction 
+  
+   }
+   
+  void updateMotors(void){
+  
+    FrontMotor.pulsewidth_us(constrain(RX_Data[3]+PID[PITCH],1000,2000));
+    RearMotor.pulsewidth_us(constrain(RX_Data[3]-PID[PITCH],1000,2000));
+    LeftMotor.pulsewidth_us(constrain(RX_Data[3]+PID[ROLL],1000,2000));
+    RightMotor.pulsewidth_us(constrain(RX_Data[3]-PID[ROLL],1000,2000));
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PulseIn.lib	Sun Dec 23 23:50:21 2012 +0000
@@ -0,0 +1,1 @@
+PulseIn#eaf70ff4df07

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Dec 23 23:50:21 2012 +0000
@@ -0,0 +1,71 @@
+#include "rtos.h"
+#include "GlobalDefines.h"
+#include "MPU.h"
+#include "Motors.h"
+
+#define FlightControlUpdateRate 5
+#define RxUpdateRate 20
+#define Enable_Serial
+
+#ifdef Enable_Serial
+Serial pc(USBTX, USBRX);
+#endif
+
+ 
+
+void RX_thread(void const *args){
+     
+     while(true){
+     RX_Data[3] = Throttle.read();
+     #ifdef Enable_Serial
+     //pc.printf("%d\n",RX_Data[3]);
+     #endif
+     Thread::wait(RxUpdateRate);
+     }
+}
+
+int main(){
+
+    //i2c.frequency(400000);
+    
+    #ifdef Enable_Serial
+        pc.baud(57600);
+    #endif
+     
+     one=1;
+     MPU_Setup();
+     CalibrateGyro();
+     CalibrateAcc();
+     InitializeMotors();
+     one=0;
+ 
+ Thread thread(RX_thread);
+ 
+ Global.start();
+ 
+ while(true){
+    
+    // Get Sensor Data
+    filterGyro();
+    AccAngle();
+    
+    // Gotta Keep Time
+    deltaTime = Global.read();
+    Global.reset();
+    
+    //Pitch and Roll Calculation using complimentary filter 
+    //pitch= (0.98)*(pitch+Scaled_GyroX*deltaTime) + (0.02)*(accangle[0]);
+    pitch+= Scaled_GyroX*deltaTime;
+    
+    // update PID and Motor Commands
+    //updatePID();
+    updateMotors();
+    
+    #ifdef Enable_Serial
+    pc.printf("%f,%f\n",pitch,accangle[0]);
+    #endif
+    
+    Thread::wait(FlightControlUpdateRate);
+
+}
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Sun Dec 23 23:50:21 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#88a1a9c26ae3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Dec 23 23:50:21 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/63cdd78b2dc1
\ No newline at end of file