Rogelio Vazquez
UCR Robosub manual control / PID tuning interface
Revision 1:3f291f2f80d3, committed 2017-07-27
- Comitter:
- roger_wee
- Date:
- Thu Jul 27 05:45:10 2017 +0000
- Parent:
- 0:048a74dd7c3a
- Commit message:
- control edit
Changed in this revision
| Sensors/IMU.h | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/Sensors/IMU.h Sat Jul 22 05:58:03 2017 +0000
+++ b/Sensors/IMU.h Thu Jul 27 05:45:10 2017 +0000
@@ -117,7 +117,7 @@
gz *= PI/180.0f;
// Calculate position if time
- mpu6050.getDisplacement(ax,ay);
+ //mpu6050.getDisplacement(ax,ay);
// Pass gyro rate as rad/s
mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx, gy, gz, magdata[0], magdata[1], magdata[2]);
--- 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