Andrew Olguin
Updated version with comments
Dependencies: IMU MODSERIAL Servo mbed
Fork of
RTOS_Controller_v3
by 
Marco Rubio
Diff: vessel.h
- Revision:
- 7:396fa2a8648d
- Parent:
- 6:b45b74fd6a07
- Child:
- 8:2db98df6fbbb
--- a/vessel.h Wed Jul 27 02:45:45 2016 +0000
+++ b/vessel.h Fri Jul 29 15:34:59 2016 +0000
@@ -8,6 +8,10 @@
#include "IMU.h"
#include "PID.h"
#include <string>
+#include "MS5837.h"
+
+MS5837 pressure_sensor = MS5837(I2C_SDA, I2C_SCL, ms5837_addr_no_CS);
+
/*
Cameras
FL ----- F ->--- FR
@@ -36,41 +40,48 @@
{
private:
-// Servo m0;
-// Servo m1;
-// Servo m2;
-// Servo m3;
-// Servo m4;
-// Servo m5;
-// Servo m6;
-// Servo m7;
-
- PwmOut m0;
- PwmOut m1;
- PwmOut m2;
- PwmOut m3;
- PwmOut m4;
- PwmOut m5;
- PwmOut m6;
- PwmOut m7;
+ Servo m0;
+ Servo m1;
+ Servo m2;
+ Servo m3;
+ Servo m4;
+ Servo m5;
+ Servo m6;
+ Servo m7;
+
+ AnalogIn powerPin;
+ int motorState;
+// PwmOut m0;
+// PwmOut m1;
+// PwmOut m2;
+// PwmOut m3;
+// PwmOut m4;
+// PwmOut m5;
+// PwmOut m6;
+// PwmOut m7;
PwmOut led1;
MPU6050 mpu6050;
+
double yawPoint, yawIn, yawOut;
double rollPoint, rollIn, rollOut;
double pitchPoint, pitchIn, pitchOut;
double xPoint, xIn, xOut;
double yPoint, yIn, yOut;
double zPoint, zIn, zOut;
+ double dPoint, dIn, dOut;
double p_gain, i_gain, d_gain;
- PID pidy, pidr, pidp, pidX, pidY, pidZ;
+ PID pidy, pidr, pidp, pidX, pidY, pidZ, pidd;
char buffer[BUFFER_SIZE];
public:
+ float depth;
+
void Start_IMU() {
pc.printf("Starting up\n\r");
pc.baud(9600);
i2c.frequency(400000); // use fast (400 kHz) I2C
IMUinit(mpu6050);
+ pressure_sensor.MS5837Init();
IMUPrintData(mpu6050);
}
@@ -78,11 +89,13 @@
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)
- {
+ pidZ(&zIn, &zOut, &zPoint,1,1,1, DIRECT),
+ pidd(&dIn, &dOut, &dPoint,1,1,1, DIRECT),
+ powerPin(A5)
+{
pidy.SetMode(AUTOMATIC); //Yaw PID
pidy.SetOutputLimits(-255,255);
@@ -102,7 +115,11 @@
pidZ.SetMode(AUTOMATIC); //Pitch PID
pidZ.SetOutputLimits(-255,255);
zPoint = 0;
-
+ pidd.SetMode(AUTOMATIC); //Pitch PID
+ pidd.SetOutputLimits(-255,255);
+ wait(0.5);
+ dPoint = depth;
+
m0 = 0.5;
m1 = 0.5;
m2 = 0.5;
@@ -112,6 +129,8 @@
m6 = 0.5;
m7 = 0.5;
+ motorState = 1;
+
Start_IMU();
pc.printf("Seagoat Initialized \n\r");
}
@@ -139,44 +158,67 @@
void SetZPID(double Kp, double Ki, double Kd) {
pidZ.SetTunings(Kp, Ki, Kd);
}
-
+ void SetdPID(double Kp, double Ki, double Kd) {
+ pidd.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--){
+// 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);
-// }
- }
- void calibrate(){
+// }
+// // for(float i = 80; i >= 0; i--){
+//// motor = i/255;
+//// wait(0.002);
+//// }
+// }
+ void calibrate() {
IMUUpdate(mpu6050);
pc.printf("Calibrating...\n\r");
+ //pressure_sensor.Barometer_MS5837();
+ //depth = pressure_sensor.MS5837_Pressure();
}
-
- void update(){
+
+ void update() {
//Update IMU Values
IMUUpdate(mpu6050);
+
+ //pressure_sensor.Barometer_MS5837();
+ depth = pressure_sensor.MS5837_Pressure();
+ //pc.printf("Pressure: %f %f\n", depth, dPoint);
+
+ //Detect if the switch is turned on
+ if(!motorState && powerPin.read() == 1){
+ initMotors();
+ motorState = 1;
+ pc.printf("Motors Detected");
+
+ yawPoint = yaw;
+ }
+ else if(powerPin.read() != 1){
+ motorState = 0;
+ neutralizeMotors();
+ }
+
yawIn = yaw;
rollIn = roll;
pitchIn = pitch;
xIn = ax;
yIn = ay;
zIn = az;
-
+
//Calculate PID values
pidy.Compute();
pidr.Compute();
@@ -184,7 +226,7 @@
pidX.Compute();
pidY.Compute();
pidZ.Compute();
-
+
/*
Cameras
FL ----- F ->--- FR
@@ -196,7 +238,7 @@
| | ˅
| | |
BL ---<- B ----- BR
-
+
0 ----- 1 ->--- 2
| | |
˄ | |
@@ -206,43 +248,44 @@
| | ˅
| | |
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 depths = dOut * dOut;
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 zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut) + abs(dOut)) * 255;
float yy = (abs(yOut) + abs(yawOut)) * 255;
- float xy = (abs(xOut) + 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;
-
+// 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(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(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
-
+
// m0 = abs((acczs + pitchs + rolls) / zpr);//
// m1 = abs((accys + yaws) / yy);
// m2 = abs((acczs + pitchs - rolls) / zpr);//
@@ -251,67 +294,92 @@
// m5 = abs((accys + yaws) / yy);
// m6 = abs((acczs - pitchs + rolls) / zpr);//
// m7 = abs((accxs + yaws) / yy);
-
- m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
+
+ m0 = (acczs + pitchs + rolls - depths) / 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;//
+ m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;//
+ m3 = (accxs + yaws) / -xy + 0.5;
+ m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;//
m5 = (accys + yaws) / yy + 0.5;
- m6 = (acczs - pitchs + rolls) / zpr + 0.5;//
+ m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;//
m7 = (accxs + yaws) / yy + 0.5;
-
+
//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("%f,%f,%f, %f,%f,%f, %f, %f \n\r",powerPin.read(), acczs, yaws, pitchs, rolls, zpr, depths, (acczs + pitchs + rolls - depths) / 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\n\r", ax, ay, az);
- //pc.printf("YPR: %f, %f, %f\n\r", yaw, pitch, roll);
+ //pc.printf("YPR: %f, %f, %f, %f\n\r", yaw, pitch, roll, depth);
+ }
+
+ void updateCommand() {
+ char c = 0;
+ string command;
+ char buffer[BUFFER_SIZE] = {' '};
+ int buffer_iter = 0;
+ //pc.printf("Checking for command\n");
+
+ // Note: you need to actually read from the serial to clear the RX interrupt
+ if (pc.readable()) {
+ pc.printf("Found command\n");
+ while (pc.readable()) {
+ c = pc.getc();
+ pc.putc(c);
+ buffer[buffer_iter] = c;
+ buffer_iter++;
+ }
+ pc.printf("Command saved to buffer\n");
+ command = strtok (buffer," ,\n");
+
+ if (strcmp(command.c_str(), "a")) {
+ this->yawPoint = atof(strtok (NULL, " ,\n"));
+ this->pitchPoint = atof(strtok (NULL, " ,\n"));
+ this->rollPoint = atof(strtok (NULL, " ,\n"));
+ pc.printf("Received Attitude points: yawPoint: %f, pitchPoint: %f, rollPoint: %f\n", this->yawPoint, this->pitchPoint, this->rollPoint);
+ } else if (strcmp(command.c_str(), "b")) {
+ this->xPoint = atof(strtok (NULL, " ,\n"));
+ this->yPoint = atof(strtok (NULL, " ,\n"));
+ this->zPoint = atof(strtok (NULL, " ,\n"));
+ pc.printf("Received X,Y,Z points: X: %f, Y: %f, Z: %f\n", this->xPoint, this->yPoint, this->zPoint);
+ } else if (strcmp(command.c_str(), "c")) {
+ this->p_gain = atof(strtok (NULL, " ,\n"));
+ this->i_gain = atof(strtok (NULL, " ,\n"));
+ this->d_gain = atof(strtok (NULL, " ,\n"));
+
+ this->SetYawPID(this->p_gain, this->i_gain, this->d_gain);
+ pc.printf("Received PID values P: %f, I: %f, D: %f\n", this->p_gain, this->i_gain, this->d_gain);
+
+ }
+
+ memset(buffer, ' ', sizeof(buffer));
+ buffer_iter = 0;
+ fflush(stdout);
+ }
+ }
+
+ void initMotors(){
+
+ neutralizeMotors();
+ m0.calibrate();
+ m1.calibrate();
+ m2.calibrate();
+ m3.calibrate();
+ m4.calibrate();
+ m5.calibrate();
+ m6.calibrate();
+ m7.calibrate();
}
-void updateCommand() {
- char c = 0;
- string command;
- char buffer[BUFFER_SIZE] = {' '};
- int buffer_iter = 0;
- pc.printf("Checking for command\n");
+ void neutralizeMotors(){
+ m0 = 0.5;
+ m1 = 0.5;
+ m2 = 0.5;
+ m3 = 0.5;
+ m4 = 0.5;
+ m5 = 0.5;
+ m6 = 0.5;
+ m7 = 0.5;
+ }
- // Note: you need to actually read from the serial to clear the RX interrupt
- if (pc.readable()) {
- pc.printf("Found command\n");
- while (pc.readable()) {
- c = pc.getc();
- pc.putc(c);
- buffer[buffer_iter] = c;
- buffer_iter++;
- }
- pc.printf("Command saved to buffer\n");
- command = strtok (buffer," ,\n");
-
- if (strcmp(command.c_str(), "a")) {
- this->yawPoint = atof(strtok (NULL, " ,\n"));
- this->pitchPoint = atof(strtok (NULL, " ,\n"));
- this->rollPoint = atof(strtok (NULL, " ,\n"));
- pc.printf("Received Attitude points: yawPoint: %f, pitchPoint: %f, rollPoint: %f\n", this->yawPoint, this->pitchPoint, this->rollPoint);
- } else if (strcmp(command.c_str(), "b")) {
- this->xPoint = atof(strtok (NULL, " ,\n"));
- this->yPoint = atof(strtok (NULL, " ,\n"));
- this->zPoint = atof(strtok (NULL, " ,\n"));
- pc.printf("Received X,Y,Z points: X: %f, Y: %f, Z: %f\n", this->xPoint, this->yPoint, this->zPoint);
- } else if (strcmp(command.c_str(), "c")) {
- this->p_gain = atof(strtok (NULL, " ,\n"));
- this->i_gain = atof(strtok (NULL, " ,\n"));
- this->d_gain = atof(strtok (NULL, " ,\n"));
-
- this->SetYawPID(this->p_gain, this->i_gain, this->d_gain);
- pc.printf("Received PID values P: %f, I: %f, D: %f\n", this->p_gain, this->i_gain, this->d_gain);
-
- }
-
- memset(buffer, ' ', sizeof(buffer));
- buffer_iter = 0;
- fflush(stdout);
- }
-}
};
#endif
\ No newline at end of file