Marco Rubio
Working
Dependencies: IMU MODSERIAL Servo mbed
Fork of
RTOS_Controller_v2
by 
Andrew Olguin
Revision 10:8cd741a65646, committed 2016-07-30
- Comitter:
- gelmes
- Date:
- Sat Jul 30 21:32:40 2016 +0000
- Parent:
- 9:9aaa7f0c8960
- Commit message:
- Working switching implementation;
Changed in this revision
| communication.h | Show annotated file Show diff for this revision Revisions of this file |
| vessel.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/communication.h Sat Jul 30 17:19:21 2016 +0000 +++ b/communication.h Sat Jul 30 21:32:40 2016 +0000 @@ -1,6 +1,5 @@ #ifndef COMMUNICATION_H #define COMMUNICATION_H MODSERIAL pc(USBTX, USBRX); // tx, rx -MODSERIAL device(PA_2, PA_3); // tx, rx #endif \ No newline at end of file
--- a/vessel.h Sat Jul 30 17:19:21 2016 +0000
+++ b/vessel.h Sat Jul 30 21:32:40 2016 +0000
@@ -99,7 +99,7 @@
pidd.SetMode(AUTOMATIC); //Pitch PID
pidd.SetOutputLimits(-255,255);
wait(0.5);
- dPoint = depth;
+ dPoint = depth;
m0 = 0.5;
m1 = 0.5;
@@ -111,6 +111,7 @@
m7 = 0.5;
motorState = 1;
+ runningState = -1;
Start_IMU();
pc.printf("Seagoat Initialized \n\r");
@@ -155,7 +156,7 @@
IMUUpdate(mpu6050);
//pressure_sensor.Barometer_MS5837();
- depth = pressure_sensor.MS5837_Pressure();
+ //depth = pressure_sensor.MS5837_Pressure();
//pc.printf("Pressure: %f %f\n", depth, dPoint);
//Detect if the switch is turned on
@@ -169,14 +170,13 @@
pc.printf("Motors Detected");
yawPoint = yaw;
- if(runningState == 1) pitchPoint = pitch;
+ if(runningState == 0) pitchPoint = pitch;
else pitchPoint = 0;
dPoint = depth;
runningTime.reset();
runningTime.start();
} else if(powerPin.read() != 1) {
motorState = 0;
- neutralizeMotors();
}
yawIn = yaw;
@@ -232,7 +232,7 @@
float rolls = rollOut * rollOut * abs(rollOut) / rollOut;
//Values used for Influence calculations
- float zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut) + abs(dOut)) * 255;
+ float zpr = (abs(zOut) + abs(pitchOut) + abs(rollOut)) * 255;
float yy = (abs(yOut) + abs(yawOut)) * 255;
float xy = (abs(xOut) + abs(yawOut)) * 255;
@@ -247,15 +247,31 @@
SetPitchPID(1,0,0);
wait(1);
}
- else{
- m0 = (acczs + pitchs + rolls - depths) / zpr + 0.5;//
+ else if(runningTime < 15){ //15
+ m0 = (acczs + pitchs + rolls) / zpr + 0.5;
m1 = (accxs + yaws) / -xy + 0.5;
- m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;//
+ m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
//m3 = (accys + yaws) / yy + 0.5;
m3 = 0.7;
- m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;//
+ m4 = (acczs - pitchs - rolls) / zpr + 0.5;
m5 = (accxs + yaws) / -xy + 0.5;
- m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;//
+ m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
+ //m7 = (-accys + yaws) / -yy + 0.5;
+ m7 = 0.7;
+ }
+ else if(runningTime < 16){ //16
+ neutralizeMotors();
+ pitchPoint = 0;
+ }
+ else{
+ m0 = (acczs + pitchs + rolls) / zpr + 0.5;
+ m1 = (accxs + yaws) / -xy + 0.5;
+ m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
+ //m3 = (accys + yaws) / yy + 0.5;
+ m3 = 0.7;
+ m4 = (acczs - pitchs - rolls) / zpr + 0.5;
+ m5 = (accxs + yaws) / -xy + 0.5;
+ m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
//m7 = (-accys + yaws) / -yy + 0.5;
m7 = 0.7;
}
@@ -265,52 +281,124 @@
SetYawPID(2,0,0.3);
SetRollPID(2,0,0.3);
SetPitchPID(2,0,0.3);
+ lastyawPoint = yawPoint;
wait(1);
- } else if(runningTime < 27) {
+ } else if(runningTime < 27) { //27
+ //Go straight to Gate
+ m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
m1 = (accxs + yaws) / -xy + 0.5;
- m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;//
- //m3 = (accys + yaws) / yy + 0.5;
+ m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
m3 = 0.7;
- m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;//
+ m4 = (acczs - pitchs - rolls) / zpr + 0.5;
m5 = (accxs + yaws) / -xy + 0.5;
- m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;//
- //m7 = (-accys + yaws) / -yy + 0.5;
+ m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
m7 = 0.7;
- } else if(runningTime < 5) {
+ } else if(runningTime < 28) { //28
+ neutralizeMotors();
+ //Set turning 180 angle aim for Gate
+ yawPoint = correctAngle(lastyawPoint - 160);
+ } else if(runningTime < 32) { //32
+ //allign to new yaw
+ m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
+ m1 = (accxs + yaws) / -xy + 0.5;
+ m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
+ m3 = 0.5;
+ m4 = (acczs - pitchs - rolls) / zpr + 0.5;
+ m5 = (accxs + yaws) / -xy + 0.5;
+ m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
+ m7 = 0.5;
+ } else if(runningTime < 42) { //42
+ //go go go
+ m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
m1 = (accxs + yaws) / -xy + 0.5;
- m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;//
- //m3 = (accys + yaws) / yy + 0.5;
+ m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
+ m3 = 0.3;
+ m4 = (acczs - pitchs - rolls) / zpr + 0.5;
+ m5 = (accxs + yaws) / -xy + 0.5;
+ m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
+ m7 = 0.3;
+ } else if(runningTime < 43) { //43
+ neutralizeMotors();
+ //Set turning 180 angle aim for Gate
+ yawPoint = correctAngle(lastyawPoint - 70);
+ } else if(runningTime < 47) { //47
+ m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
+ m1 = (accxs + yaws) / -xy + 0.5;
+ m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
+ m3 = 0.5;
+ m4 = (acczs - pitchs - rolls) / zpr + 0.5;
+ m5 = (accxs + yaws) / -xy + 0.5;
+ m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
+ m7 = 0.5;
+ } else if(runningTime < 52) { //52
+ m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
+ m1 = (accxs + yaws) / -xy + 0.5;
+ m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
m3 = 0.7;
- m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;//
+ m4 = (acczs - pitchs - rolls) / zpr + 0.5;
m5 = (accxs + yaws) / -xy + 0.5;
- m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;//
- //m7 = (-accys + yaws) / -yy + 0.5;
+ m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
m7 = 0.7;
}
+ // else if(runningTime < 36) { //53
+// neutralizeMotors();
+// //Go Upside down
+// rollPoint = correctAngle(lastrollPoint + 170);
+// pc.printf("%f\n", rollPoint);
+// } else if(runningTime < 40) { //57
+// m0 = (acczs + pitchs + rolls) / zpr + 0.5;//
+// m1 = (accxs + yaws) / -xy + 0.5;
+// m2 = (acczs + pitchs - rolls) / -zpr + 0.5;
+// m3 = 0.5;
+// m4 = (acczs - pitchs - rolls) / zpr + 0.5;
+// m5 = (accxs + yaws) / -xy + 0.5;
+// m6 = (acczs - pitchs + rolls) / -zpr + 0.5;
+// m7 = 0.5;
+// }
+ else if(runningTime < 58) { //63
+ //Surface
+ m0 = 0.7;
+ m1 = 0.5;
+ m2 = 0.7;
+ m3 = 0.5;
+ m4 = 0.7;
+ m5 = 0.5;
+ m6 = 0.7;
+ m7 = 0.5;
+ }
+ else{
+ neutralizeMotors();
+ }
}
//pc.printf("%f,%f,%f,%f\n\r",accxs, yaws, yy, (accxs + yaws) / yy + 0.5);
//pc.printf("%f,%f,%f, %f,%f,%f, %f, %f \n\r",powerPin.read(), ay, yaws, pitchs, rolls, yy, accys, (-accys + yaws - (forwardThrust * forwardThrust)) / -yy + 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, %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, %f\n\r", yaw, pitch, roll, depth);
}
-
+
+ float correctAngle(float angle){
+ if(angle > 180) return (angle - 360);
+ else if(angle < -180) return (angle + 360);
+ else return angle;
+ }
+
void updateCommand() {
- char a = 0;
- char i = 0;
- char buffer[10] = {' '};
- if (device.readable()) {
- while(a != 'd') {
- a = device.getc();
- if ((a >= '0' && a <='9') || a == '.') {
- buffer[i] = a;
- i++;
- }
- }
- depth = atof(buffer);
- pc.printf("Depth: '%f'\n", depth);
- }
+ //char a = 0;
+// char i = 0;
+// char buffer[10] = {' '};
+// if (device.readable()) {
+// while(a != 'd') {
+// a = device.getc();
+// if ((a >= '0' && a <='9') || a == '.') {
+// buffer[i] = a;
+// i++;
+// }
+// }
+// depth = atof(buffer);
+// pc.printf("Depth: '%f'\n", depth);
+// }
}
void initMotors() {