Rogelio Vazquez
/
Robosub_test
first publish
Diff: main.cpp
- Revision:
- 2:359f1f075c72
- Parent:
- 0:ce3ac53af6e4
diff -r 11117343d223 -r 359f1f075c72 main.cpp --- a/main.cpp Mon May 15 21:26:09 2017 +0000 +++ b/main.cpp Sun Jun 04 06:46:28 2017 +0000 @@ -1,45 +1,109 @@ #include "IMU.h" #include "PID.h" -//#include "MS5837.h" +#include "MS5837.h" #include "Motor.h" +#include "HMC5883L.h" -//Declare IMU object -MPU6050 mpu1; +// 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); -//Input, Output, SetPoint, kp, ki, kd, Controller Direction +// 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() -{ - i2c.frequency(400000); // use fast (400 kHz) I2C - wait(.2); - //Initialize mpu] +{ + // Initialize IMU IMUinit(mpu1); + compass.init(); + // Initialize pressure sensor + // pressure_sensor.MS5837Init(); + + // Initialize PID pidp.SetMode(AUTOMATIC); pidp.SetOutputLimits(0.5, 1); - pidp.SetTunings(.028, 0.01, 0.025); + + //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) { - //other stuff .. ie state machines - IMUPrintData(mpu1); - myPitch = pitch; - pidp.Compute(); - float s2Out = 1.5 - sOut; - mWhite.speed(s2Out); - mBlack.speed(-sOut); + + // Read pressure sensor data + // pressure_sensor.Barometer_MS5837(); + // pressure = pressure_sensor.MS5837_Temperature(); + // pc.printf("pressure: %f \n", pressure); - char text[60]; - sprintf(text, "%f,%f,%f,%f,%f \n", yaw, pitch, roll,sOut, s2Out); + // 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); } }