![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
UCR Robosub manual control / PID tuning interface
Diff: main.cpp
- Revision:
- 1:3f291f2f80d3
- Parent:
- 0:048a74dd7c3a
--- a/main.cpp Sat Jul 22 05:58:03 2017 +0000 +++ b/main.cpp Thu Jul 27 05:45:10 2017 +0000 @@ -6,30 +6,58 @@ #include "HMC5883L.h" //------------Declare Objects------------------// + +// (m1) (m2) +// | | +// | | +// (Ltrhust)| |(Rthrust) +// | | +// | | +// (m3) (m4) + PwmOut m1(D3); PwmOut m2(D4); PwmOut m3(D5); PwmOut m4(D6); +PwmOut Lthrust(D7); +PwmOut Rthrust(D8); + // Declare mpu6050 and compass object MPU6050 mpu1; HMC5883L compass; -// Serial communication between Arduino NANO +// Serial communication between STM & Arduino NANO RawSerial device(PA_11, PA_12); //TX RX +// Serial communication between STM & Raspberry PI +RawSerial devicePI(D10,D2); + //-----------Global Variables------------------// char command = 0; Timer calibrate; +Timer data; int pwmMax = 1100; // Reversed due to motor orientation int pwmMin = 1500; +const int IDLE = 1500; +const int MAXYAW = 100; +const int MAXPITCH = 100; +const int MAXROLL = 100; + +int thrust = 0; + +int j = -1; // j == 1 --> FWD ... j == -1 --> REV + + // PWM output variable for each motor int m1pwm = pwmMin; int m2pwm = pwmMin; int m3pwm = pwmMin; int m4pwm = pwmMin; +int Lthrustpwm = pwmMin; +int Rthrustpwm = pwmMin; // Individual pid parameters double myYaw, yawPoint, yawOut; @@ -40,6 +68,8 @@ int depth = 1; double kpVal = 0; +double kiVal = 0; +double kdVal = 0; //-----------End Global Variables--------------// @@ -59,7 +89,7 @@ void calibrateFilter() { calibrate.start(); - while(calibrate.read() < 10) + while(calibrate.read() < 5) { IMUPrintData(mpu1, compass); @@ -76,6 +106,8 @@ m2.pulsewidth_us(m2pwm); m3.pulsewidth_us(m3pwm); m4.pulsewidth_us(m4pwm); + Lthrust.pulsewidth_us(Lthrustpwm); + Rthrust.pulsewidth_us(Rthrustpwm); } void neutralizeMotors() @@ -84,9 +116,20 @@ m2pwm = pwmMin; m3pwm = pwmMin; m4pwm = pwmMin; + Lthrustpwm = pwmMin; + Rthrustpwm = pwmMin; updateMotors(); } +void vesselGo() +{ + thrust = 200; +} +void vesselHover() +{ + thrust = 0; +} + void readUserInput() { if(pc.readable()) @@ -95,21 +138,29 @@ } } +void updateYawThrottle() +{ + Lthrustpwm = IDLE + (j*(thrust + yawOut)); + Rthrustpwm = IDLE + (j*(thrust + (MAXYAW-yawOut))); +} + void initializePIDs() { pidy.SetMode(AUTOMATIC); // Yaw PID - pidy.SetOutputLimits(pwmMin, pwmMax); + pidy.SetOutputLimits(0, MAXYAW); pidp.SetMode(AUTOMATIC); // Pitch PID - pidp.SetOutputLimits(pwmMin, pwmMax); + pidp.SetOutputLimits(0, MAXPITCH); pidr.SetMode(AUTOMATIC); // Roll PID - pidr.SetOutputLimits(pwmMin, pwmMax); + pidr.SetOutputLimits(0, MAXROLL); pidd.SetMode(AUTOMATIC); // Depth PID - pidd.SetOutputLimits(pwmMin, pwmMax); + pidd.SetOutputLimits(0,300); depthPoint = 0; + pitchPoint = 0; + rollPoint = 0; } void readDepth() @@ -127,7 +178,7 @@ void displayTelemetry() { char text[90]; - sprintf(text, "%f,%f,%f,%d \n", yaw, pitch, roll, depth); + sprintf(text, "%f,%f,%f,%d,%f \n", yaw, pitch, roll, depth, depthPoint); pc.printf("%s", text); } @@ -147,7 +198,7 @@ IMUinit(mpu1); compass.init(); - pc.printf("Initialize motors \n"); + pc.printf("Initialirze motors \n"); neutralizeMotors(); pc.printf("Initialize PID objects \n"); @@ -198,23 +249,46 @@ break; } updateMotors(); + displayTelemetry(); break; case preparePid: // pc.printf("Prepare PID \n"); switch(command) { - case 'I': // Increase Kp gain + case 'K': // Increase Kp gain kpVal += .1; break; - case 'R': + case 'k': if (kpVal > 0 ) // Decreae Kp gain { kpVal -= .1; } break; + case 'I': + kiVal += .1; + break; + + case 'i': + if (kiVal > 0) + { + kiVal -= .1; + } + break; + + case 'D': + kdVal += .1; + break; + + case 'd': + if (kdVal > 0) + { + kdVal -= .1; + } + break; + case 'S': // Increase depth depthPoint++; @@ -230,12 +304,19 @@ } // Set tunings - pidd.SetTunings(kpVal,0,0); //Set Ki Kd = 0 + pidd.SetTunings(kpVal,kiVal,kdVal); //Set Ki Kd = 0 + //pidy.SetTunings(kpVal,kiVal,kdVal); + //pidp.SetTunings(kpVal,kiVal,kdVal); + //pidr.SetTunings(kpVal,kiVal,kdVal); - // Print Set parameters - pc.printf("DepthPoint: %d \n", depthPoint); - pc.printf("Kp gain: %d \n", pidd.GetKp()); - + if (data.read_ms() % 500 == 0) + { + // Print Set parameters + pc.printf("DepthPoint: %d \n", depthPoint); + pc.printf("Kp gain: %d \n", pidd.GetKp()); + pc.printf("Ki gain: %d \n", pidd.GetKi()); + pc.printf("Kd gain: %d \n", pidd.GetKd()); + } break; case beginTune: @@ -247,12 +328,23 @@ // Assign feedback variables myDepth = depth; + //myYaw = yaw; + //myPitch = pitch; + //myRoll = Roll; // Compute PID pidd.Compute(); + //pidy.Compute(); + //pidp.Compute(); + //pidr.Compute(); // Output pwm to motors // FIXME : account for pitch, roll and maybe yaw depending on motor configuration - m1pwm = m2pwm = m3pwm = m4pwm = depthOut; + m1pwm = IDLE + depthOut; // + pitchOut + m2pwm = IDLE + depthOut; // + (MAXPITCH - pitchOut) + m3pwm = IDLE + depthOut; // + pitchOut + m4pwm = IDLE + depthOut; // + (MAXPITCH - pitchOut) + + //updateYawThrottle(); updateMotors(); // Display telemetry @@ -307,7 +399,7 @@ int main() { // Initialize control state controlState = init; - + data.start(); while(1) { // Read user input