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
Embed:
(wiki syntax)
Show/hide line numbers
vessel.h
00001 #ifndef VESSEL_H 00002 #define VESSEL_H 00003 00004 #include "mbed.h" 00005 #include "MPU6050.h" 00006 #include "Servo.h" 00007 #include "IMU.h" 00008 #include "PID.h" 00009 /* 00010 Cameras 00011 FL ----- F ->--- FR 00012 | | | 00013 ˄ | | 00014 | | | 00015 L | R 00016 | | | 00017 | | ˅ 00018 | | | 00019 BL ---<- B ----- BR 00020 00021 0 ----- 1 ->--- 2 00022 | | | 00023 ˄ | | 00024 | | | 00025 7 | 3 00026 | | | 00027 | | ˅ 00028 | | | 00029 6 ---<- 5 ----- 4 00030 */ 00031 class Vessel 00032 { 00033 00034 private: 00035 // Servo m0; 00036 // Servo m1; 00037 // Servo m2; 00038 // Servo m3; 00039 // Servo m4; 00040 // Servo m5; 00041 // Servo m6; 00042 // Servo m7; 00043 00044 PwmOut m0; 00045 PwmOut m1; 00046 PwmOut m2; 00047 PwmOut m3; 00048 PwmOut m4; 00049 PwmOut m5; 00050 PwmOut m6; 00051 PwmOut m7; 00052 00053 PwmOut led1; 00054 MPU6050 mpu6050; 00055 double yawPoint, yawIn, yawOut; 00056 double rollPoint, rollIn, rollOut; 00057 double pitchPoint, pitchIn, pitchOut; 00058 double xPoint, xIn, xOut; 00059 double yPoint, yIn, yOut; 00060 double zPoint, zIn, zOut; 00061 PID pidy, pidr, pidp, pidX, pidY, pidZ; 00062 00063 public: 00064 void Start_IMU() { 00065 pc.printf("Starting up\n\r"); 00066 pc.baud(9600); 00067 i2c.frequency(400000); // use fast (400 kHz) I2C 00068 IMUinit(mpu6050); 00069 IMUPrintData(mpu6050); 00070 } 00071 00072 //Initialise all of the vessels starting parameters 00073 Vessel(): m0(D2),m1(D3),m2(D4),m3(D5),m4(D6),m5(D7),m6(D8),m7(D10), led1(LED1), 00074 pidy(&yawIn, &yawOut, &yawPoint,1,1,1, DIRECT), 00075 pidr(&rollIn, &rollOut, &rollPoint,1,1,1, DIRECT), 00076 pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT), 00077 pidX(&xIn, &xOut, &xPoint,1,1,1, DIRECT), 00078 pidY(&yIn, &yOut, &yPoint,1,1,1, DIRECT), 00079 pidZ(&zIn, &zOut, &zPoint,1,1,1, DIRECT){ 00080 00081 pidy.SetMode(AUTOMATIC); //Yaw PID 00082 pidy.SetOutputLimits(-255,255); 00083 yawPoint = 0; 00084 pidr.SetMode(AUTOMATIC); //Roll PID 00085 pidr.SetOutputLimits(-255,255); 00086 pitchPoint = 0; 00087 pidp.SetMode(AUTOMATIC); //Pitch PID 00088 pidp.SetOutputLimits(-255,255); 00089 rollPoint = 0; 00090 pidX.SetMode(AUTOMATIC); //Pitch PID 00091 pidX.SetOutputLimits(-255,255); 00092 xPoint = 0; 00093 pidY.SetMode(AUTOMATIC); //Pitch PID 00094 pidY.SetOutputLimits(-255,255); 00095 yPoint = 0; 00096 pidZ.SetMode(AUTOMATIC); //Pitch PID 00097 pidZ.SetOutputLimits(-255,255); 00098 zPoint = 0; 00099 00100 m0 = 0.5; 00101 m1 = 0.5; 00102 m2 = 0.5; 00103 m3 = 0.5; 00104 m4 = 0.5; 00105 m5 = 0.5; 00106 m6 = 0.5; 00107 m7 = 0.5; 00108 00109 Start_IMU(); 00110 pc.printf("Seagoat Initialized \n\r"); 00111 } 00112 00113 void SetYawPID(double Kp, double Ki, double Kd) { 00114 pidy.SetTunings(Kp, Ki, Kd); 00115 } 00116 00117 void SetRollPID(double Kp, double Ki, double Kd) { 00118 pidr.SetTunings(Kp, Ki, Kd); 00119 } 00120 00121 void SetPitchPID(double Kp, double Ki, double Kd) { 00122 pidp.SetTunings(Kp, Ki, Kd); 00123 } 00124 00125 void SetXPID(double Kp, double Ki, double Kd) { 00126 pidX.SetTunings(Kp, Ki, Kd); 00127 } 00128 00129 void SetYPID(double Kp, double Ki, double Kd) { 00130 pidY.SetTunings(Kp, Ki, Kd); 00131 } 00132 00133 void SetZPID(double Kp, double Ki, double Kd) { 00134 pidZ.SetTunings(Kp, Ki, Kd); 00135 } 00136 00137 //This is where the magic happens 00138 void motorTest(){ 00139 pwmSweep(m0); 00140 pwmSweep(m1); 00141 pwmSweep(m2); 00142 pwmSweep(m3); 00143 pwmSweep(m4); 00144 pwmSweep(m5); 00145 pwmSweep(m6); 00146 pwmSweep(m7); 00147 } 00148 00149 void pwmSweep(PwmOut motor){ 00150 for(float i = 0; i < 80; i++){ 00151 motor = i/255; 00152 wait(0.002); 00153 } 00154 // for(float i = 80; i >= 0; i--){ 00155 // motor = i/255; 00156 // wait(0.002); 00157 // } 00158 } 00159 void calibrate(){ 00160 IMUUpdate(mpu6050); 00161 pc.printf("Calibrating...\n\r"); 00162 } 00163 00164 void update(){ 00165 //Update IMU Values 00166 IMUUpdate(mpu6050); 00167 yawIn = yaw; 00168 rollIn = roll; 00169 pitchIn = pitch; 00170 xIn = ax; 00171 yIn = ay; 00172 zIn = az; 00173 00174 //Calculate PID values 00175 pidy.Compute(); 00176 pidr.Compute(); 00177 pidp.Compute(); 00178 pidX.Compute(); 00179 pidY.Compute(); 00180 pidZ.Compute(); 00181 00182 /* 00183 Cameras 00184 FL ----- F ->--- FR 00185 | | | 00186 ˄ | | 00187 | | | 00188 L | R 00189 | | | 00190 | | ˅ 00191 | | | 00192 BL ---<- B ----- BR 00193 00194 0 ----- 1 ->--- 2 00195 | | | 00196 ˄ | | 00197 | | | 00198 7 | 3 00199 | | | 00200 | | ˅ 00201 | | | 00202 6 ---<- 5 ----- 4 00203 00204 */ 00205 00206 //pc.printf("YAW: %f, %f, %f, %f, %f, %f\n\r", xOut, yOut, zOut, yawOut, pitchOut, rollOut); 00207 00208 //Values used in Dynamic Magnitude Calculations 00209 float accxs = xOut * xOut * abs(xOut) / xOut; 00210 float accys = yOut * yOut * abs(yOut) / yOut; 00211 float acczs = zOut * zOut * abs(zOut) / zOut; 00212 float yaws = yawOut * yawOut * abs(yawOut) / yawOut; 00213 float pitchs = pitchOut * pitchOut * abs(pitchOut) / pitchOut; 00214 float rolls = rollOut * rollOut * abs(rollOut) / rollOut; 00215 00216 //Values used for Influence calculations 00217 float zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut)) * 255; 00218 float yy = (abs(yOut) + abs(yawOut)) * 255; 00219 float xy = (abs(xOut) + abs(yawOut)) * 255; 00220 00221 // float zpr = (zOut + pitchOut + rollOut) * 255; 00222 // float yy = (yOut + yawOut) * 255; 00223 // float xy = (xOut + yawOut) * 255; 00224 00225 // if (abs(zpr)<255 && abs(zpr)>=0) zpr = 255; 00226 // if (abs(yy)<255 && abs(yy)>=0) yy = 255; 00227 // if (abs(xy)<255 && abs(xy)>=0) xy = 255; 00228 // if (abs(zpr)>-255 && abs(zpr)<0) zpr = -255; 00229 // if (abs(yy)>-255 && abs(yy)<0) yy = -255; 00230 // if (abs(xy)>-255 && abs(xy)<0) xy = -255; 00231 00232 if (abs(zpr)<255) zpr = 255; 00233 if (abs(yy)<255) yy = 255; 00234 if (abs(xy)<255) xy = 255; 00235 00236 //pc.printf("YAW: %f, %f, %f, %f, %f\n\r", zOut, pitchOut, rollOut, zpr, abs((acczs + pitchs + rolls) / zpr)); 00237 00238 //Spit out PID values 00239 00240 // m0 = abs((acczs + pitchs + rolls) / zpr);// 00241 // m1 = abs((accys + yaws) / yy); 00242 // m2 = abs((acczs + pitchs - rolls) / zpr);// 00243 // m3 = abs((accxs + yaws) / xy); 00244 // m4 = abs((acczs - pitchs - rolls) / zpr);// 00245 // m5 = abs((accys + yaws) / yy); 00246 // m6 = abs((acczs - pitchs + rolls) / zpr);// 00247 // m7 = abs((accxs + yaws) / yy); 00248 00249 m0 = (acczs + pitchs + rolls) / zpr + 0.5;// 00250 m1 = (accys + yaws) / yy + 0.5; 00251 m2 = (acczs + pitchs - rolls) / zpr + 0.5;// 00252 m3 = (accxs + yaws) / xy + 0.5; 00253 m4 = (acczs - pitchs - rolls) / zpr + 0.5;// 00254 m5 = (accys + yaws) / yy + 0.5; 00255 m6 = (acczs - pitchs + rolls) / zpr + 0.5;// 00256 m7 = (accxs + yaws) / yy + 0.5; 00257 00258 //pc.printf("%f,%f,%f,%f\n\r",accxs, yaws, yy, (accxs + yaws) / yy + 0.5); 00259 //pc.printf("%f,%f,%f,%f,%f \n\r",acczs, pitchs, rolls, zpr, (acczs + pitchs + rolls) / zpr + 0.5); 00260 //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)); 00261 // pc.printf("YAW: %f, %f, %f, %f\n\r", xOut, yawOut, yawIn, yawPoint); 00262 //pc.printf("YAW: %f, %f, %f, %f\n\r", yaw, yawOut, yawIn, yawPoint); 00263 //pc.printf("ACC: %f, %f, %f\n\r", ax, ay, az); 00264 //pc.printf("YPR: %f, %f, %f\n\r", yaw, pitch, roll); 00265 } 00266 }; 00267 #endif
Generated on Sat Jul 16 2022 12:25:49 by
1.7.2