Marco Rubio
Controller for Seagoat in the RoboSub competition
Fork of
ESC
by 
Matteo Terruzzi
Revision 5:07bbe020eb65, committed 2016-07-26
- Comitter:
- gelmes
- Date:
- Tue Jul 26 17:22:33 2016 +0000
- Parent:
- 4:b37fd183e46a
- Commit message:
- This is a working implementation of the Controller;
Changed in this revision
--- a/PID.cpp Sat Jul 09 20:41:49 2016 +0000
+++ b/PID.cpp Tue Jul 26 17:22:33 2016 +0000
@@ -55,7 +55,8 @@
double error = *mySetpoint - input;
- pt.printf("pid1: %f, %f, %f, %f\n\r", error, error, *mySetpoint, input);
+ //pt.printf("pid1: %f, %f, %f, %f\n\r", error, error, *mySetpoint, input);
+
ITerm+= (ki * error);
if(ITerm > outMax) ITerm= outMax;
else if(ITerm < outMin) ITerm= outMin;
@@ -64,13 +65,13 @@
/*Compute PID Output*/
double output = kp * error + ITerm- kd * dInput;
- pt.printf("pid2: %f, %f, %f, %f, %f\n\r", error, output, *mySetpoint, input, ITerm);
+ //pt.printf("pid2: %f, %f, %f, %f, %f\n\r", error, output, *mySetpoint, input, ITerm);
if(output > outMax) output = outMax;
else if(output < outMin) output = outMin;
*myOutput = output;
- pt.printf("pid3: %f, %f, %f, %f, %f\n\r", error, output, *mySetpoint, input, ki);
+ //pt.printf("pid3: %f, %f, %f, %f, %f\n\r", error, output, *mySetpoint, input, ki);
/*Remember some variables for next time*/
lastInput = input;
--- a/main.cpp Sat Jul 09 20:41:49 2016 +0000
+++ b/main.cpp Tue Jul 26 17:22:33 2016 +0000
@@ -4,13 +4,32 @@
int main()
{
+ Timer t;
+
+ wait(3);
Vessel seagoat; //Starts the seagoat
- seagoat.SetYawPID(1,0,0);
- seagoat.SetRollPID(1,0,0);
- seagoat.SetPitchPID(1,0,0);
+ seagoat.SetYawPID(4,0,0);
+ seagoat.SetRollPID(4,0,0);
+ seagoat.SetPitchPID(4,0,0);
+ seagoat.SetXPID(0,0,0);
+ seagoat.SetYPID(0,0,0);
+ seagoat.SetZPID(0,0,0);
+
+ t.start();
+ while(t.read() < 5){
+ seagoat.calibrate();
+
+ //pc.printf("%f \n", t.read());
+ }
+ t.stop();
+ pc.printf("Seagoat Ready to Go\n\r");
+ //seagoat.motorTest();
while(1) {
+
+ //seagoat.motorTest();
seagoat.update();
- wait(0.1);
+
+ wait(0.01);
}
}
\ No newline at end of file
--- a/vessel.h Sat Jul 09 20:41:49 2016 +0000
+++ b/vessel.h Tue Jul 26 17:22:33 2016 +0000
@@ -55,7 +55,10 @@
double yawPoint, yawIn, yawOut;
double rollPoint, rollIn, rollOut;
double pitchPoint, pitchIn, pitchOut;
- PID pidy, pidr, pidp;
+ double xPoint, xIn, xOut;
+ double yPoint, yIn, yOut;
+ double zPoint, zIn, zOut;
+ PID pidy, pidr, pidp, pidX, pidY, pidZ;
public:
void Start_IMU() {
@@ -67,23 +70,44 @@
}
//Initialise all of the vessels starting parameters
- Vessel(): m0(D2),m1(D3),m2(D4),m3(D5),m4(D6),m5(D7),m6(D8),m7(D9), led1(LED1),
+ Vessel(): m0(D2),m1(D3),m2(D4),m3(D5),m4(D6),m5(D7),m6(D8),m7(D10), led1(LED1),
pidy(&yawIn, &yawOut, &yawPoint,1,1,1, DIRECT),
pidr(&rollIn, &rollOut, &rollPoint,1,1,1, DIRECT),
- pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT) {
+ pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT),
+ pidX(&xIn, &xOut, &xPoint,1,1,1, DIRECT),
+ pidY(&yIn, &yOut, &yPoint,1,1,1, DIRECT),
+ pidZ(&zIn, &zOut, &zPoint,1,1,1, DIRECT){
pidy.SetMode(AUTOMATIC); //Yaw PID
pidy.SetOutputLimits(-255,255);
yawPoint = 0;
- pidr.SetMode(AUTOMATIC); //Yaw PID
+ pidr.SetMode(AUTOMATIC); //Roll PID
pidr.SetOutputLimits(-255,255);
- rollPoint = 0;
- pidp.SetMode(AUTOMATIC); //Yaw PID
+ pitchPoint = 0;
+ pidp.SetMode(AUTOMATIC); //Pitch PID
pidp.SetOutputLimits(-255,255);
rollPoint = 0;
+ pidX.SetMode(AUTOMATIC); //Pitch PID
+ pidX.SetOutputLimits(-255,255);
+ xPoint = 0;
+ pidY.SetMode(AUTOMATIC); //Pitch PID
+ pidY.SetOutputLimits(-255,255);
+ yPoint = 0;
+ pidZ.SetMode(AUTOMATIC); //Pitch PID
+ pidZ.SetOutputLimits(-255,255);
+ zPoint = 0;
+
+ m0 = 0.5;
+ m1 = 0.5;
+ m2 = 0.5;
+ m3 = 0.5;
+ m4 = 0.5;
+ m5 = 0.5;
+ m6 = 0.5;
+ m7 = 0.5;
Start_IMU();
- pc.printf("Seagoat Ready to Go\n\r");
+ pc.printf("Seagoat Initialized \n\r");
}
void SetYawPID(double Kp, double Ki, double Kd) {
@@ -97,33 +121,147 @@
void SetPitchPID(double Kp, double Ki, double Kd) {
pidp.SetTunings(Kp, Ki, Kd);
}
+
+ void SetXPID(double Kp, double Ki, double Kd) {
+ pidX.SetTunings(Kp, Ki, Kd);
+ }
+
+ void SetYPID(double Kp, double Ki, double Kd) {
+ pidY.SetTunings(Kp, Ki, Kd);
+ }
+
+ void SetZPID(double Kp, double Ki, double Kd) {
+ pidZ.SetTunings(Kp, Ki, Kd);
+ }
//This is where the magic happens
+ void motorTest(){
+ pwmSweep(m0);
+ pwmSweep(m1);
+ pwmSweep(m2);
+ pwmSweep(m3);
+ pwmSweep(m4);
+ pwmSweep(m5);
+ pwmSweep(m6);
+ pwmSweep(m7);
+ }
+
+ void pwmSweep(PwmOut motor){
+ for(float i = 0; i < 80; i++){
+ motor = i/255;
+ wait(0.002);
+ }
+ // for(float i = 80; i >= 0; i--){
+// motor = i/255;
+// wait(0.002);
+// }
+ }
+ void calibrate(){
+ IMUUpdate(mpu6050);
+ pc.printf("Calibrating...\n\r");
+ }
+
void update(){
//Update IMU Values
IMUUpdate(mpu6050);
yawIn = yaw;
rollIn = roll;
pitchIn = pitch;
+ xIn = ax;
+ yIn = ay;
+ zIn = az;
//Calculate PID values
pidy.Compute();
- //pidr.Compute();
- //pidp.Compute();
+ pidr.Compute();
+ pidp.Compute();
+ pidX.Compute();
+ pidY.Compute();
+ pidZ.Compute();
+
+ /*
+ Cameras
+ FL ----- F ->--- FR
+ | | |
+ ˄ | |
+ | | |
+ L | R
+ | | |
+ | | ˅
+ | | |
+ BL ---<- B ----- BR
+
+ 0 ----- 1 ->--- 2
+ | | |
+ ˄ | |
+ | | |
+ 7 | 3
+ | | |
+ | | ˅
+ | | |
+ 6 ---<- 5 ----- 4
+
+ */
+
+ //pc.printf("YAW: %f, %f, %f, %f, %f, %f\n\r", xOut, yOut, zOut, yawOut, pitchOut, rollOut);
+
+ //Values used in Dynamic Magnitude Calculations
+ float accxs = xOut * xOut * abs(xOut) / xOut;
+ float accys = yOut * yOut * abs(yOut) / yOut;
+ float acczs = zOut * zOut * abs(zOut) / zOut;
+ float yaws = yawOut * yawOut * abs(yawOut) / yawOut;
+ float pitchs = pitchOut * pitchOut * abs(pitchOut) / pitchOut;
+ float rolls = rollOut * rollOut * abs(rollOut) / rollOut;
+
+ //Values used for Influence calculations
+ float zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut)) * 255;
+ float yy = (abs(yOut) + abs(yawOut)) * 255;
+ float xy = (abs(xOut) + abs(yawOut)) * 255;
+
+// float zpr = (zOut + pitchOut + rollOut) * 255;
+// float yy = (yOut + yawOut) * 255;
+// float xy = (xOut + yawOut) * 255;
+
+// if (abs(zpr)<255 && abs(zpr)>=0) zpr = 255;
+// if (abs(yy)<255 && abs(yy)>=0) yy = 255;
+// if (abs(xy)<255 && abs(xy)>=0) xy = 255;
+// if (abs(zpr)>-255 && abs(zpr)<0) zpr = -255;
+// if (abs(yy)>-255 && abs(yy)<0) yy = -255;
+// if (abs(xy)>-255 && abs(xy)<0) xy = -255;
+
+ if (abs(zpr)<255) zpr = 255;
+ if (abs(yy)<255) yy = 255;
+ if (abs(xy)<255) xy = 255;
+
+ //pc.printf("YAW: %f, %f, %f, %f, %f\n\r", zOut, pitchOut, rollOut, zpr, abs((acczs + pitchs + rolls) / zpr));
//Spit out PID values
- double yo = abs(yawOut/255); //Dividing once to reduce overhead
+
+// m0 = abs((acczs + pitchs + rolls) / zpr);//
+// m1 = abs((accys + yaws) / yy);
+// m2 = abs((acczs + pitchs - rolls) / zpr);//
+// m3 = abs((accxs + yaws) / xy);
+// m4 = abs((acczs - pitchs - rolls) / zpr);//
+// m5 = abs((accys + yaws) / yy);
+// m6 = abs((acczs - pitchs + rolls) / zpr);//
+// m7 = abs((accxs + yaws) / yy);
- m0 = yo;
- m1 = yo;
- m2 = yo;
- m3 = yo;
- m4 = yo;
- m5 = yo;
- m6 = yo;
- m7 = yo;
+ m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
+ m1 = (accys + yaws) / yy + 0.5;
+ m2 = (acczs + pitchs - rolls) / zpr + 0.5;//
+ m3 = (accxs + yaws) / xy + 0.5;
+ m4 = (acczs - pitchs - rolls) / zpr + 0.5;//
+ m5 = (accys + yaws) / yy + 0.5;
+ m6 = (acczs - pitchs + rolls) / zpr + 0.5;//
+ m7 = (accxs + yaws) / yy + 0.5;
- pc.printf("yaw0: %f, %f, %f, %f\n\r", yaw, yawOut, yawIn, yawPoint);
+ //pc.printf("%f,%f,%f,%f\n\r",accxs, yaws, yy, (accxs + yaws) / yy + 0.5);
+ //pc.printf("%f,%f,%f,%f,%f \n\r",acczs, pitchs, rolls, zpr, (acczs + pitchs + rolls) / zpr + 0.5);
+ //pc.printf("YAW: %f, %f, %f, %f, %f, %f, %f, %f\n\r", abs((acczs + pitchs + rolls) / zpr),abs((accys + yaws) / yy),abs((acczs + pitchs - rolls) / zpr),abs((accxs + yaws) / xy),abs((acczs - pitchs - rolls) / zpr),abs((accys + yaws) / yy),abs((acczs - pitchs + rolls) / zpr),abs((accxs + yaws) / yy));
+ // pc.printf("YAW: %f, %f, %f, %f\n\r", xOut, yawOut, yawIn, yawPoint);
+ //pc.printf("YAW: %f, %f, %f, %f\n\r", yaw, yawOut, yawIn, yawPoint);
+ //pc.printf("ACC: %f, %f, %f\n\r", ax, ay, az);
+ //pc.printf("YPR: %f, %f, %f\n\r", yaw, pitch, roll);
}
};
#endif
\ No newline at end of file