Script for a drone PID tuning interface using buttons, potentiometers and a 3-D printed platform.
Fork of Robosub_test by
main.cpp
- Committer:
- roger_wee
- Date:
- 2017-06-04
- Revision:
- 2:359f1f075c72
- Parent:
- 0:ce3ac53af6e4
- Child:
- 4:a51fa881cc4c
File content as of revision 2:359f1f075c72:
#include "IMU.h" #include "PID.h" #include "MS5837.h" #include "Motor.h" #include "HMC5883L.h" // Maps value from incoming analog signal to correct range // to be sent out to as pwm signal to motors float map(float x, float in_min, float in_max, float out_min, float out_max) { 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; //Declare digital button input DigitalIn tuningButton(USER_BUTTON); // Declare mpu object MPU6050 mpu1; HMC5883L compass; MS5837 pressure_sensor = MS5837(D14, D15, ms5837_addr_no_CS); // Declare motor objects Motor mBlack(D3,D2,D9); // pwm, fwd, rev Motor mWhite(D4,D5,D8); // Declare analog input pin AnalogIn kpKnob(A0); AnalogIn kiKnob(A1); AnalogIn kdKnob(A2); // Input, Output, SetPoint, kp, ki, kd, Controller Direction PID pidp(&myPitch, &sOut, &setPoint, 1, 1, 1, DIRECT); //Serial pc(USBTX, USBRX); int main() { // Initialize IMU IMUinit(mpu1); compass.init(); // Initialize pressure sensor // pressure_sensor.MS5837Init(); // Initialize PID pidp.SetMode(AUTOMATIC); pidp.SetOutputLimits(0.5, 1); //Default kp ki kd parameters float kpKnobVal = .028; float kiKnobVal = .01; float kdKnobVal = .025; pidp.SetTunings(kpKnobVal, kiKnobVal, kdKnobVal); setPoint = 0; // float pressure; while(1) { // Read pressure sensor data // pressure_sensor.Barometer_MS5837(); // pressure = pressure_sensor.MS5837_Temperature(); // pc.printf("pressure: %f \n", pressure); // 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); // Adjust tunings pidp.SetTunings(kpKnobVal,kiKnobVal,kdKnobVal); } //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; // char textA[90]; // sprintf(textA, "%f,%f,%f,%f \n", heading, magdata[0], magdata[1], heading); // pc.printf("%s", textA); // Compute output using pid controller //pidp.Compute(); // Send pwm output to Motors //float s2Out = 1.5 - sOut; //mWhite.speed(s2Out); //mBlack.speed(-sOut); //Display telemetry char text[90]; sprintf(text, "%f,%f,%f \n", yaw, pitch, roll); pc.printf("%s", text); wait(.01); } }