Marco Rubio
/
RTOS_Controller
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
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