Rogelio Vazquez / Mbed 2 deprecated Drone_PID_tuning_Platform

Script for a drone PID tuning interface using buttons, potentiometers and a 3-D printed platform.

Dependencies:   mbed HMC5883L

Fork of Robosub_test by Rogelio Vazquez

main.cpp@4:a51fa881cc4c, 2018-02-12 (annotated)

Committer:
roger_wee
Date:
Mon Feb 12 07:27:47 2018 +0000
Revision:
4:a51fa881cc4c
Parent:
2:359f1f075c72 
Script for a Drone PID tuning plaform

Who changed what in which revision?

UserRevisionLine numberNew contents of line
roger_wee 0:ce3ac53af6e4 1 #include "IMU.h"
roger_wee 0:ce3ac53af6e4 2 #include "PID.h"
roger_wee 0:ce3ac53af6e4 3 #include "Motor.h"
roger_wee 2:359f1f075c72 4 #include "HMC5883L.h"
roger_wee 0:ce3ac53af6e4 5
roger_wee 2:359f1f075c72 6 // Maps value from incoming analog signal to correct range
roger_wee 2:359f1f075c72 7 // to be sent out to as pwm signal to motors
roger_wee 2:359f1f075c72 8 float map(float x, float in_min, float in_max, float out_min, float out_max)
roger_wee 2:359f1f075c72 9 {
roger_wee 2:359f1f075c72 10 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
roger_wee 2:359f1f075c72 11 }
roger_wee 0:ce3ac53af6e4 12
roger_wee 4:a51fa881cc4c 13
roger_wee 4:a51fa881cc4c 14 DigitalOut my_led(LED1);
roger_wee 0:ce3ac53af6e4 15
roger_wee 4:a51fa881cc4c 16 // Individual pid parameters
roger_wee 4:a51fa881cc4c 17 double myYaw, yawPoint, yawOut;
roger_wee 4:a51fa881cc4c 18 double myPitch, pitchPoint, pitchOut;
roger_wee 4:a51fa881cc4c 19 double myRoll, rollPoint, rollOut;
roger_wee 4:a51fa881cc4c 20 double myDepth, depthPoint, depthOut;
roger_wee 2:359f1f075c72 21
roger_wee 4:a51fa881cc4c 22 //Declare digital button input tuning parameters //FIXME
roger_wee 4:a51fa881cc4c 23 DigitalIn tuningButton(USER_BUTTON);
roger_wee 2:359f1f075c72 24
roger_wee 4:a51fa881cc4c 25 // Declare mpu6050 and compass object
roger_wee 2:359f1f075c72 26 MPU6050 mpu1;
roger_wee 2:359f1f075c72 27 HMC5883L compass;
roger_wee 2:359f1f075c72 28
roger_wee 4:a51fa881cc4c 29 // Declare analog input pin ( kp, ki, kd ) tuning parameters
roger_wee 4:a51fa881cc4c 30 AnalogIn tuneKnob(A0);
roger_wee 2:359f1f075c72 31
roger_wee 4:a51fa881cc4c 32 // Declare motor pins
roger_wee 4:a51fa881cc4c 33 PwmOut m1(D3);
roger_wee 4:a51fa881cc4c 34 PwmOut m2(D4);
roger_wee 4:a51fa881cc4c 35 PwmOut m3(D5);
roger_wee 4:a51fa881cc4c 36 PwmOut m4(D6);
roger_wee 0:ce3ac53af6e4 37
roger_wee 4:a51fa881cc4c 38 // PWM output variable for each motor
roger_wee 4:a51fa881cc4c 39 int m1pwm, m2pwm, m3pwm, m4pwm;
roger_wee 2:359f1f075c72 40
roger_wee 2:359f1f075c72 41 // Input, Output, SetPoint, kp, ki, kd, Controller Direction
roger_wee 4:a51fa881cc4c 42 PID pidy(&myYaw, &yawOut, &yawPoint, 1, 1, 1, DIRECT);
roger_wee 4:a51fa881cc4c 43 PID pidp(&myPitch, &pitchOut, &pitchPoint, 1, 1, 1, DIRECT);
roger_wee 4:a51fa881cc4c 44 PID pidr(&myRoll, &rollOut, &rollPoint, 1, 1, 1, DIRECT);
roger_wee 4:a51fa881cc4c 45 PID pidd(&myDepth, &depthOut, &depthPoint, 1, 1, 1, DIRECT);
roger_wee 0:ce3ac53af6e4 46
roger_wee 2:359f1f075c72 47 //Serial pc(USBTX, USBRX);
roger_wee 2:359f1f075c72 48
roger_wee 4:a51fa881cc4c 49 // Serial communication between Arduino NANO
roger_wee 4:a51fa881cc4c 50 RawSerial device(PA_11, PA_12); //TX RX
roger_wee 4:a51fa881cc4c 51
roger_wee 4:a51fa881cc4c 52 Timer calibrate;
roger_wee 4:a51fa881cc4c 53
roger_wee 4:a51fa881cc4c 54 int depth = 1;
roger_wee 4:a51fa881cc4c 55
roger_wee 0:ce3ac53af6e4 56 int main()
roger_wee 2:359f1f075c72 57 {
roger_wee 2:359f1f075c72 58 // Initialize IMU
roger_wee 0:ce3ac53af6e4 59 IMUinit(mpu1);
roger_wee 4:a51fa881cc4c 60
roger_wee 4:a51fa881cc4c 61 // Initialize Magnetometer
roger_wee 2:359f1f075c72 62 compass.init();
roger_wee 4:a51fa881cc4c 63
roger_wee 4:a51fa881cc4c 64 // Initialize PID's
roger_wee 4:a51fa881cc4c 65 pidy.SetMode(AUTOMATIC); // Yaw PID
roger_wee 4:a51fa881cc4c 66 pidy.SetOutputLimits(1500, 1700);
roger_wee 2:359f1f075c72 67
roger_wee 4:a51fa881cc4c 68 pidp.SetMode(AUTOMATIC); // Pitch PID
roger_wee 4:a51fa881cc4c 69 pidp.SetOutputLimits(1500, 1700);
roger_wee 4:a51fa881cc4c 70
roger_wee 4:a51fa881cc4c 71 pidr.SetMode(AUTOMATIC); // Roll PID
roger_wee 4:a51fa881cc4c 72 pidr.SetOutputLimits(1500, 1700);
roger_wee 2:359f1f075c72 73
roger_wee 4:a51fa881cc4c 74 pidd.SetMode(AUTOMATIC); // Depth PID
roger_wee 4:a51fa881cc4c 75 pidd.SetOutputLimits(1500, 1700);
roger_wee 4:a51fa881cc4c 76
roger_wee 4:a51fa881cc4c 77
roger_wee 4:a51fa881cc4c 78 // Default kp ki kd parameters
roger_wee 2:359f1f075c72 79 float kpKnobVal = .028;
roger_wee 2:359f1f075c72 80 float kiKnobVal = .01;
roger_wee 2:359f1f075c72 81 float kdKnobVal = .025;
roger_wee 4:a51fa881cc4c 82
roger_wee 4:a51fa881cc4c 83 // Configure PID's
roger_wee 4:a51fa881cc4c 84 pidd.SetTunings(kpKnobVal, kiKnobVal, kdKnobVal);
roger_wee 4:a51fa881cc4c 85 depthPoint = 0;
roger_wee 0:ce3ac53af6e4 86
roger_wee 4:a51fa881cc4c 87 //Calibrate IMU
roger_wee 4:a51fa881cc4c 88 calibrate.start();
roger_wee 4:a51fa881cc4c 89 while(calibrate.read() < 5);
roger_wee 4:a51fa881cc4c 90 {
roger_wee 4:a51fa881cc4c 91 IMUPrintData(mpu1, compass);
roger_wee 4:a51fa881cc4c 92 my_led = 1;
roger_wee 4:a51fa881cc4c 93 }
roger_wee 4:a51fa881cc4c 94 my_led = 0;
roger_wee 4:a51fa881cc4c 95
roger_wee 4:a51fa881cc4c 96 calibrate.stop();
roger_wee 2:359f1f075c72 97
roger_wee 0:ce3ac53af6e4 98 while(1)
roger_wee 0:ce3ac53af6e4 99 {
roger_wee 4:a51fa881cc4c 100 //-------------------------SENSE--------------------------------
roger_wee 2:359f1f075c72 101
roger_wee 4:a51fa881cc4c 102 // Read pressure sensor data if available
roger_wee 4:a51fa881cc4c 103 if (device.readable())
roger_wee 4:a51fa881cc4c 104 {
roger_wee 4:a51fa881cc4c 105 // Receive depth in inches as an integer
roger_wee 4:a51fa881cc4c 106 depth = device.getc();
roger_wee 4:a51fa881cc4c 107
roger_wee 4:a51fa881cc4c 108 // Convert to feet
roger_wee 4:a51fa881cc4c 109
roger_wee 4:a51fa881cc4c 110 // Display received data
roger_wee 4:a51fa881cc4c 111 // pc.printf("%d \n", pressure);
roger_wee 4:a51fa881cc4c 112 }
roger_wee 4:a51fa881cc4c 113
roger_wee 4:a51fa881cc4c 114 // Obtain mpu data -> pass through filter -> obtain yaw pitch roll
roger_wee 4:a51fa881cc4c 115 IMUPrintData(mpu1, compass);
roger_wee 4:a51fa881cc4c 116
roger_wee 4:a51fa881cc4c 117 // Assign feedback variables
roger_wee 4:a51fa881cc4c 118 myDepth = depth;
roger_wee 4:a51fa881cc4c 119
roger_wee 4:a51fa881cc4c 120 //------------------------End SENSE----------------------------
roger_wee 4:a51fa881cc4c 121
roger_wee 4:a51fa881cc4c 122
roger_wee 4:a51fa881cc4c 123 //------------------------Tuning ADJUST------------------------- FIXMEEEEEE
roger_wee 0:ce3ac53af6e4 124
roger_wee 2:359f1f075c72 125 // If button is pressed kp ki kd values can be adjusted
roger_wee 2:359f1f075c72 126 if(!tuningButton)
roger_wee 2:359f1f075c72 127 {
roger_wee 2:359f1f075c72 128 // Read raw potentiometer values from k-knob and map to kpknobVal
roger_wee 4:a51fa881cc4c 129 kpKnobVal = map(tuneKnob.read(), 0.000, 1.000, 0.000, .050);
roger_wee 4:a51fa881cc4c 130
roger_wee 2:359f1f075c72 131
roger_wee 2:359f1f075c72 132 // Adjust tunings
roger_wee 2:359f1f075c72 133 pidp.SetTunings(kpKnobVal,kiKnobVal,kdKnobVal);
roger_wee 2:359f1f075c72 134
roger_wee 2:359f1f075c72 135 }
roger_wee 2:359f1f075c72 136 //print mapped joystick values
roger_wee 2:359f1f075c72 137 // pc.printf("kp: %f -- ki: %f -- kd %f \n", kpKnobVal, kiKnobVal, kdKnobVal);
roger_wee 2:359f1f075c72 138
roger_wee 4:a51fa881cc4c 139 //------------------------End tuning ADJUST-----------------------
roger_wee 2:359f1f075c72 140
roger_wee 4:a51fa881cc4c 141
roger_wee 4:a51fa881cc4c 142 //------------------------Motor Control LOOP----------------------
roger_wee 2:359f1f075c72 143
roger_wee 2:359f1f075c72 144 // Compute output using pid controller
roger_wee 4:a51fa881cc4c 145 pidd.Compute();
roger_wee 4:a51fa881cc4c 146
roger_wee 4:a51fa881cc4c 147 // Assign pid output pwm to individual pwm variables
roger_wee 4:a51fa881cc4c 148 m1pwm = depthOut;
roger_wee 4:a51fa881cc4c 149 m2pwm = depthOut;
roger_wee 4:a51fa881cc4c 150 m3pwm = depthOut;
roger_wee 4:a51fa881cc4c 151 m4pwm = depthOut;
roger_wee 2:359f1f075c72 152
roger_wee 4:a51fa881cc4c 153 // Output pwm
roger_wee 4:a51fa881cc4c 154 m1.pulsewidth_us(m1pwm);
roger_wee 4:a51fa881cc4c 155 m2.pulsewidth_us(m2pwm);
roger_wee 4:a51fa881cc4c 156 m3.pulsewidth_us(m3pwm);
roger_wee 4:a51fa881cc4c 157 m4.pulsewidth_us(m4pwm);
roger_wee 4:a51fa881cc4c 158
roger_wee 4:a51fa881cc4c 159 //------------------------End Motor Control LOOP-------------------
roger_wee 4:a51fa881cc4c 160
roger_wee 4:a51fa881cc4c 161
roger_wee 4:a51fa881cc4c 162 //------------------------Display TELEMETRY------------------------
roger_wee 2:359f1f075c72 163 char text[90];
roger_wee 4:a51fa881cc4c 164 sprintf(text, "%f,%f,%f,%d \n", yaw, pitch, roll, depth);
roger_wee 0:ce3ac53af6e4 165 pc.printf("%s", text);
roger_wee 4:a51fa881cc4c 166 //--------------------------End TELEMETRY--------------------------
roger_wee 0:ce3ac53af6e4 167
roger_wee 0:ce3ac53af6e4 168 }
roger_wee 0:ce3ac53af6e4 169 }
roger_wee 0:ce3ac53af6e4 170