Revision 4:a51fa881cc4c, committed 2018-02-12

Comitter:

roger_wee

Date:

Mon Feb 12 07:27:47 2018 +0000

Parent:

3:394c971eab83

Commit message:

Script for a Drone PID tuning plaform

Changed in this revision

--- a/HMC5883L.lib	Sun Jun 04 06:58:45 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/users/roger_wee/code/HMC5883L/#f5f6aaf24be0

--- a/MARGfilter.lib	Sun Jun 04 06:58:45 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://mbed.org/users/elromulous/code/MARGfilter/#6259fdd04e83

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/HMC5883L.lib	Mon Feb 12 07:27:47 2018 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/roger_wee/code/HMC5883L/#f5f6aaf24be0

--- a/Sensors/IMU.h	Sun Jun 04 06:58:45 2017 +0000
+++ b/Sensors/IMU.h	Mon Feb 12 07:27:47 2018 +0000
@@ -77,9 +77,9 @@
         mpu6050.getAres();
  
         // Now we'll calculate the accleration value into actual g's
-        ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
-        ay = (float)accelCount[1]*aRes - accelBias[1];
-        az = (float)accelCount[2]*aRes - accelBias[2];
+        ax = (float)accelCount[0] *aRes - accelBias[0];  // get actual g value, this depends on scale being set
+        ay = (float)accelCount[1] *aRes - accelBias[1];
+        az = (float)accelCount[2] *aRes - accelBias[2];
  
         mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
         mpu6050.getGres();
@@ -100,7 +100,7 @@
         
     Now = t.read_us();
     deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
-    sampleFreq = 1/deltat;
+    //sampleFreq = 1/deltat;
     lastUpdate = Now;
  
     sum += deltat;
@@ -111,11 +111,13 @@
         zeta = 0.015; // increasey bias drift gain after stabilized
     }
     
-    //Convert gyro rate as rad/s
+    // Convert gyro rate as rad/s
     gx *= PI/180.0f;
     gy *= PI/180.0f;
     gz *= PI/180.0f;
     
+    // Calculate position if time
+    mpu6050.getDisplacement(ax,ay);
 
     // Pass gyro rate as rad/s
     mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx, gy, gz, magdata[0], magdata[1], magdata[2]);
@@ -124,20 +126,6 @@
     delt_t = t.read_ms() - count;
     if (delt_t > 0) { // update LCD once per half-second independent of read rate
  
-     //   pc.printf("ax = %f", 1000*ax);
-     //   pc.printf(" ay = %f", 1000*ay);
-     //   pc.printf(" az = %f  mg\n\r", 1000*az);
- 
-     //   pc.printf("gx = %f", gx);
-     //   pc.printf(" gy = %f", gy);
-     //   pc.printf(" gz = %f  deg/s\n\r", gz);
- 
-     //   pc.printf(" temperature = %f  C\n\r", temperature);
- 
-     //   pc.printf("q0 = %f\n\r", q[0]);
-     //   pc.printf("q1 = %f\n\r", q[1]);
-     //   pc.printf("q2 = %f\n\r", q[2]);
-     //   pc.printf("q3 = %f\n\r", q[3]);
         
         // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
         // In this coordinate system, the positive z-axis is down toward Earth.

--- a/Sensors/MPU6050.h	Sun Jun 04 06:58:45 2017 +0000
+++ b/Sensors/MPU6050.h	Mon Feb 12 07:27:47 2018 +0000
@@ -167,6 +167,12 @@
 float temperature;
 float SelfTest[6];
 
+signed int accelerationx[2], accelerationy[2];
+signed long velocityx[2], velocityy[2];
+signed long positionX[2];
+signed long positionY[2];
+signed long positionZ[2]; 
+unsigned char countx, county;
 
 float heading = 0;
 float magdata[3];
@@ -174,11 +180,11 @@
 int delt_t = 0; // used to control display output rate
 int count = 0;  // used to control display output rate
  
-// parameters for 6 DoF sensor fusion calculations
+// parameters for 9 DoF sensor fusion calculations
 float PI = 3.14159265358979323846f;
-float GyroMeasError = PI * (40.0f / 180.0f);     // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
+float GyroMeasError = PI * (90.0f / 180.0f);     // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
 float beta = sqrt(3.0f / 4.0f) * GyroMeasError;  // compute beta
-float GyroMeasDrift = PI * (1.0f / 180.0f);      // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
+float GyroMeasDrift = PI * (3.0f / 180.0f);      // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
 float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;  // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
 float yaw, pitch, roll;
 float deltat = 0.0f;                              // integration interval for both filter schemes
@@ -797,7 +803,89 @@
             q[3] = q4 * norm;
 
         }
-    
+        
+        void getDisplacement(float ax, float ay)
+        {
+            unsigned char count2 ;
+            count2=0;
+
+            do{
+                accelerationx[1] = accelerationx[1] + ax; //filtering routine for noise attenuation
+                accelerationy[1] = accelerationy[1] + ay; //64 samples are averaged. The resulting 
+
+                //average represents the acceleration of
+                //an instant
+                count2++;
+            }while (count2!=0x40); // 64 sums of the acceleration sample
+
+            accelerationx[1]= accelerationx[1]>>6; // division by 64
+            accelerationy[1]= accelerationy[1]>>6;
+
+            //accelerationx[1] = accelerationx[1] - (int)sstatex; //eliminating zero reference
+            //offset of the acceleration data
+            //accelerationy[1] = accelerationy[1] - (int)sstatey; // to obtain positive and negative
+            //acceleration
+
+
+            if ((accelerationx[1] <=3)&&(accelerationx[1] >= -3)) //Discrimination window applied
+            {accelerationx[1] = 0;} // to the X axis acceleration variable
+
+            if ((accelerationy[1] <=3)&&(accelerationy[1] >= -3))
+            {accelerationy[1] = 0;}
+
+            //first X integration:
+            velocityx[1]= velocityx[0]+ accelerationx[0]+ ((accelerationx[1] - accelerationx[0])>>1);
+            //second X integration:
+            positionX[1]= positionX[0] + velocityx[0] + ((velocityx[1] - velocityx[0])>>1);
+            //first Y integration:
+            velocityy[1] = velocityy[0] + accelerationy[0] + ((accelerationy[1] -accelerationy[0])>>1);
+            //second Y integration:
+            positionY[1] = positionY[0] + velocityy[0] + ((velocityy[1] - velocityy[0])>>1);
+
+            accelerationx[0] = accelerationx[1]; //The current acceleration value must be sent
+            //to the previous acceleration
+            accelerationy[0] = accelerationy[1]; //variable in order to introduce the new
+            //acceleration value.
+
+            velocityx[0] = velocityx[1]; //Same done for the velocity variable
+            velocityy[0] = velocityy[1];
+
+            positionX[1] = positionX[1]<<18; //The idea behind this shifting (multiplication)
+            //is a sensibility adjustment.
+            positionY[1] = positionY[1]<<18; //Some applications require adjustments to a
+            //particular situation i.e. mouse application
+
+            positionX[1] = positionX[1]>>18; //once the variables are sent them must return to
+            positionY[1] = positionY[1]>>18; //their original state
+ 
+            movement_end_check();
+
+            positionX[0] = positionX[1]; //actual position data must be sent to the
+            positionY[0] = positionY[1]; //previous position
+        }
+        
+        void movement_end_check(void)
+        {
+            if (accelerationx[1]==0) //we count the number of acceleration samples that equals cero
+            { countx++;}
+            else { countx =0;}
+
+            if (countx>=25) //if this number exceeds 25, we can assume that velocity is cero
+            {
+                velocityx[1]=0;
+                velocityx[0]=0;
+            }
+
+            if (accelerationy[1]==0) //we do the same for the Y axis
+            { county++;}
+            else { county =0;}
+
+            if (county>=25)
+            {
+                velocityy[1]=0;
+                velocityy[0]=0;
+            }
+        }
  
 };
 #endif
\ No newline at end of file

--- a/Sensors/MS5837.cpp	Sun Jun 04 06:58:45 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,100 +0,0 @@
-#include <stdlib.h>
-#include "MS5837.h"
- //Pressure Sensor
- 
-/*
- * Sensor operating function according data sheet
- */
- 
-void MS5837::MS5837Init(void)
-{
-    MS5837Reset();
-    MS5837ReadProm();
-    return;
-}
- 
-/* Send soft reset to the sensor */
-void MS5837::MS5837Reset(void)
-{
-    /* transmit out 1 byte reset command */
-    ms5837_tx_data[0] = ms5837_reset;
-    if ( i2c.write( device_address,  ms5837_tx_data, 1 ) );
-    //printf("send soft reset");
-    wait_ms(20);
-}
- 
-/* read the sensor calibration data from rom */
-void MS5837::MS5837ReadProm(void)
-{
-    uint8_t i,j;
-    for (i=0; i<8; i++) {
-        j = i;
-        ms5837_tx_data[0] = ms5837_PROMread + (j<<1);
-        if ( i2c.write( device_address,  ms5837_tx_data, 1 ) );
-        if ( i2c.read( device_address,  ms5837_rx_data, 2 ) );
-        C[i]   = ms5837_rx_data[1] + (ms5837_rx_data[0]<<8);
-    }
-}
- 
-/* Start the sensor pressure conversion */
-void MS5837::MS5837ConvertD1(void)
-{
-    ms5837_tx_data[0] = ms5837_convD1;
-    if ( i2c.write( device_address,  ms5837_tx_data, 1 ) );
-}
- 
-/* Start the sensor temperature conversion */
-void MS5837:: MS5837ConvertD2(void)
-{
-    ms5837_tx_data[0] = ms5837_convD2;
-    if ( i2c.write( device_address,  ms5837_tx_data, 1 ) );
-}
- 
-/* Read the previous started conversion results */
-int32_t MS5837::MS5837ReadADC(void)
-{
-    int32_t adc;
-    wait_ms(150);
-    ms5837_tx_data[0] = ms5837_ADCread;
-    if ( i2c.write( device_address,  ms5837_tx_data, 1 ) );
-    if ( i2c.read( device_address,  ms5837_rx_data, 3 ) );
-    adc = ms5837_rx_data[2] + (ms5837_rx_data[1]<<8) + (ms5837_rx_data[0]<<16);
-    return (adc);
-}
- 
-/* return the results */
-float MS5837::MS5837_Pressure (void)
-{
-    return P_MS5837;
-}
-float MS5837::MS5837_Temperature (void)
-{
-    return T_MS5837;
-}
- 
-/* Sensor reading and calculation procedure */
-void MS5837::Barometer_MS5837(void)
-{
-    int32_t dT, temp;
-    int64_t OFF, SENS, press;
- 
-    //no need to do this everytime!
-    //MS5837Reset();                 // reset the sensor
-    //MS5837ReadProm();             // read the calibration values
-    
-    
-    MS5837ConvertD1();             // start pressure conversion
-    D1 = MS5837ReadADC();        // read the pressure value
-    MS5837ConvertD2();             // start temperature conversion
-    D2 = MS5837ReadADC();         // read the temperature value
- 
-    /* calculation according MS5837-01BA data sheet DA5837-01BA_006 */
-    dT       = D2 - (C[5]* 256);
-    OFF      = (int64_t)C[2] * (1<<16) + ((int64_t)dT * (int64_t)C[4]) / (1<<7);
-    SENS     = (int64_t)C[1] * (1<<15) + ((int64_t)dT * (int64_t)C[3]) / (1<<8);
- 
-    temp     = 2000 + (dT * C[6]) / (1<<23);
-    T_MS5837 = (float) temp / 100.0f;                 // result of temperature in deg C in this var
-    press    = (((int64_t)D1 * SENS) / (1<<21) - OFF) / (1<<13);
-    P_MS5837 = (float) press / 10.0f;                 // result of pressure in mBar in this var
-}
\ No newline at end of file

--- a/Sensors/MS5837.h	Sun Jun 04 06:58:45 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,64 +0,0 @@
-#include "mbed.h"
- 
-#ifndef MS5837_H
-#define MS5837_H
- 
-#define MS5837_RX_DEPTH 3 //
-#define MS5837_TX_DEPTH 2 //
- 
-// choose your connection here
-#define ms5837_addr_no_CS  0x76 //0b1110110
- 
-#define ms5837_reset       0x1E // Sensor Reset
- 
-#define ms5837_convD1_256  0x40 // Convert D1 OSR 256
-#define ms5837_convD1_512  0x42 // Convert D1 OSR 512
-#define ms5837_convD1_1024 0x44 // Convert D1 OSR 1024
-#define ms5837_convD1_2048 0x46 // Convert D1 OSR 2048
-#define ms5837_convD1_4096 0x48 // Convert D1 OSR 4096
-#define ms5837_convD1_8192 0x4A // Convert D1 OSR 8192
- 
-#define ms5837_convD1 ms5837_convD1_4096 // choose your sampling rate here
- 
-#define ms5837_convD2_256  0x50 // Convert D2 OSR  256
-#define ms5837_convD2_512  0x52 // Convert D2 OSR  512
-#define ms5837_convD2_1024 0x54 // Convert D2 OSR 1024
-#define ms5837_convD2_2048 0x56 // Convert D2 OSR 2048
-#define ms5837_convD2_4096 0x58 // Convert D2 OSR 4096
-#define ms5837_convD2_8192 0x5A // Convert D2 OSR 8192
- 
-#define ms5837_convD2 ms5837_convD2_4096 // choose your sampling rate here
- 
-#define ms5837_ADCread     0x00 // read ADC command
-#define ms5837_PROMread    0xA0 // read PROM command base address
- 
-class MS5837{
-private:
-    int D1, D2, Temp, C[8];
-    float T_MS5837, P_MS5837;
-    /* Data buffers */
-    char ms5837_rx_data[MS5837_RX_DEPTH];
-    char ms5837_tx_data[MS5837_TX_DEPTH];
- 
-public:
-    MS5837 (PinName sda, PinName scl,
-            char ms5837_addr = ms5837_addr_no_CS  )
-            : i2c( sda, scl ), device_address( ms5837_addr << 1 ) {
-    }
-    void MS5837Init(void);
-    void MS5837Reset(void);
-    void MS5837ReadProm(void);
-    void MS5837ConvertD1(void);
-    void MS5837ConvertD2(void);
-    int32_t MS5837ReadADC(void);
-    float MS5837_Pressure (void);
-    float MS5837_Temperature (void);
-    void Barometer_MS5837(void);
- 
- 
-private:
-    I2C     i2c;
-    char    device_address;
- 
-};
-#endif
\ No newline at end of file

--- a/main.cpp	Sun Jun 04 06:58:45 2017 +0000
+++ b/main.cpp	Mon Feb 12 07:27:47 2018 +0000
@@ -1,6 +1,5 @@
 #include "IMU.h"
 #include "PID.h"
-#include "MS5837.h"
 #include "Motor.h"
 #include "HMC5883L.h"
 
@@ -11,69 +10,124 @@
   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
-double myPitch, sOut, setPoint;
-double k_p, k_i, k_d;
+
+DigitalOut  my_led(LED1);
 
-//Declare digital button input
-DigitalIn tuningButton(USER_BUTTON);
+// Individual pid parameters
+double myYaw, yawPoint, yawOut;
+double myPitch, pitchPoint, pitchOut;
+double myRoll, rollPoint, rollOut;
+double myDepth, depthPoint, depthOut;
 
+//Declare digital button input                  tuning parameters   //FIXME
+DigitalIn tuningButton(USER_BUTTON);            
 
-// Declare mpu object
+// Declare mpu6050 and compass object
 MPU6050 mpu1;
 HMC5883L compass;
 
-MS5837 pressure_sensor = MS5837(D14, D15, ms5837_addr_no_CS);
+// Declare analog input pin ( kp, ki, kd )      tuning parameters
+AnalogIn    tuneKnob(A0);
 
-// Declare motor objects
-Motor mBlack(D3,D2,D9); // pwm, fwd, rev
-Motor mWhite(D4,D5,D8);
+// Declare motor pins
+PwmOut      m1(D3);
+PwmOut      m2(D4);
+PwmOut      m3(D5);
+PwmOut      m4(D6);
 
-// Declare analog input pin 
-AnalogIn    kpKnob(A0);
-AnalogIn    kiKnob(A1);
-AnalogIn    kdKnob(A2);
+// PWM output variable for each motor
+int m1pwm, m2pwm, m3pwm, m4pwm;
 
 // Input, Output, SetPoint, kp, ki, kd, Controller Direction
-PID pidp(&myPitch, &sOut, &setPoint, 1, 1, 1, DIRECT);
+PID pidy(&myYaw, &yawOut, &yawPoint, 1, 1, 1, DIRECT);
+PID pidp(&myPitch, &pitchOut, &pitchPoint, 1, 1, 1, DIRECT);
+PID pidr(&myRoll, &rollOut, &rollPoint, 1, 1, 1, DIRECT);
+PID pidd(&myDepth, &depthOut, &depthPoint, 1, 1, 1, DIRECT);
 
 //Serial pc(USBTX, USBRX);
 
+// Serial communication between Arduino NANO
+RawSerial device(PA_11, PA_12);  //TX RX
+
+Timer calibrate;
+
+int depth = 1;
+
 int main()
 {   
     // Initialize IMU
     IMUinit(mpu1);
+    
+    // Initialize Magnetometer
     compass.init();
-    // Initialize pressure sensor
- //   pressure_sensor.MS5837Init();
+    
+    // Initialize PID's 
+    pidy.SetMode(AUTOMATIC);            // Yaw PID
+    pidy.SetOutputLimits(1500, 1700);
     
-    // Initialize PID 
-    pidp.SetMode(AUTOMATIC);    
-    pidp.SetOutputLimits(0.5, 1);
+    pidp.SetMode(AUTOMATIC);            // Pitch PID
+    pidp.SetOutputLimits(1500, 1700); 
+    
+    pidr.SetMode(AUTOMATIC);            // Roll PID
+    pidr.SetOutputLimits(1500, 1700);
     
-    //Default kp ki kd parameters
+    pidd.SetMode(AUTOMATIC);            // Depth PID
+    pidd.SetOutputLimits(1500, 1700); 
+
+    
+    // Default kp ki kd parameters
     float kpKnobVal = .028;
     float kiKnobVal = .01;
     float kdKnobVal = .025;
-    pidp.SetTunings(kpKnobVal, kiKnobVal, kdKnobVal);
-    setPoint = 0;
+    
+    // Configure PID's
+    pidd.SetTunings(kpKnobVal, kiKnobVal, kdKnobVal);
+    depthPoint = 0;
     
-   // float pressure;
+    //Calibrate IMU
+    calibrate.start();
+    while(calibrate.read() < 5);
+    {
+        IMUPrintData(mpu1, compass);
+        my_led = 1;
+    }
+    my_led = 0;
+    
+    calibrate.stop();
 
     while(1)
     {    
+        //-------------------------SENSE--------------------------------
     
-        // Read pressure sensor data
-       // pressure_sensor.Barometer_MS5837();
-       // pressure = pressure_sensor.MS5837_Temperature();
-       // pc.printf("pressure: %f \n", pressure);
+        // Read pressure sensor data if available
+        if (device.readable())
+        {
+            // Receive depth in inches as an integer
+            depth = device.getc();
+            
+            // Convert to feet
+            
+            // Display received data
+           // pc.printf("%d \n", pressure);
+        }
+        
+        // Obtain mpu data -> pass through filter -> obtain yaw pitch roll
+        IMUPrintData(mpu1, compass);
+        
+        // Assign feedback variables
+        myDepth = depth;
+        
+        //------------------------End SENSE----------------------------
+        
+        
+        //------------------------Tuning ADJUST-------------------------    FIXMEEEEEE
         
         // If button is pressed kp ki kd values can be adjusted
         if(!tuningButton)
         {
             // Read raw potentiometer values from k-knob and map to kpknobVal
-            kpKnobVal = map(kpKnob.read(), 0.000, 1.000, 0.000, .050);
-            kiKnobVal = map(kiKnob.read(), 0.000, 1.000, 0.000, 0.008);
-            kdKnobVal = map(kdKnob.read(), 0.000, 1.000, 0.000, .040);
+            kpKnobVal = map(tuneKnob.read(), 0.000, 1.000, 0.000, .050);
+           
             
             // Adjust tunings
             pidp.SetTunings(kpKnobVal,kiKnobVal,kdKnobVal);
@@ -82,28 +136,35 @@
         //print mapped joystick values
        // pc.printf("kp: %f -- ki: %f -- kd %f \n", kpKnobVal, kiKnobVal, kdKnobVal);  
         
-        // Obtain mpu data -> pass through filter -> obtain yaw pitch roll
-        IMUPrintData(mpu1, compass);
-        //myPitch = pitch;
+        //------------------------End tuning ADJUST-----------------------
         
-       // char textA[90];
-       // sprintf(textA, "%f,%f,%f,%f \n", heading, magdata[0], magdata[1], heading);
-       // pc.printf("%s", textA);
+        
+        //------------------------Motor Control LOOP----------------------
         
         // Compute output using pid controller
-        //pidp.Compute();
+        pidd.Compute();
+        
+        // Assign pid output pwm to individual pwm variables
+        m1pwm = depthOut;
+        m2pwm = depthOut;
+        m3pwm = depthOut;
+        m4pwm = depthOut;
         
-        // Send pwm output to Motors
-        //float s2Out = 1.5 - sOut;
-        //mWhite.speed(s2Out); 
-        //mBlack.speed(-sOut);
-                
-        //Display telemetry
+        // Output pwm
+        m1.pulsewidth_us(m1pwm);
+        m2.pulsewidth_us(m2pwm);
+        m3.pulsewidth_us(m3pwm);
+        m4.pulsewidth_us(m4pwm);
+        
+        //------------------------End Motor Control LOOP-------------------    
+        
+        
+        //------------------------Display TELEMETRY------------------------
         char text[90];
-        sprintf(text, "%f,%f,%f \n", yaw, pitch, roll);
+        sprintf(text, "%f,%f,%f,%d \n", yaw, pitch, roll, depth);
         pc.printf("%s", text);
+        //--------------------------End TELEMETRY--------------------------
         
-        wait(.01);
     }
 }