Diff: vessel.h

Revision:

3:5ffe7e9c0bb3

Child:

4:b37fd183e46a

diff -r aabc14a9a8c8 -r 5ffe7e9c0bb3 vessel.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/vessel.h	Mon Jul 04 18:56:23 2016 +0000
@@ -0,0 +1,97 @@
+#ifndef VESSEL_H
+#define VESSEL_H
+
+#include "mbed.h"
+#include "MPU6050.h"
+#include "Servo.h"
+#include "IMU.h"
+#include "PID.h"
+/*
+            Cameras
+      FL ----- F ->--- FR
+      |        |       |
+      ˄        |       |
+      |        |       |
+      L        |       R
+      |        |       |
+      |        |       ˅
+      |        |       |
+      BL ---<- B ----- BR
+
+      0  ----- 1 ->--- 2
+      |        |       |
+      ˄        |       |
+      |        |       |
+      7        |       3
+      |        |       |
+      |        |       ˅
+      |        |       |
+      6  ---<- 5 ----- 4
+*/
+class Vessel
+{
+
+private:
+    Servo m0;
+    Servo m1;
+    Servo m2;
+    Servo m3;
+    Servo m4;
+    Servo m5;
+    Servo m6;
+    Servo m7;
+
+    PwmOut led1;
+    MPU6050 mpu6050;
+    double yawPoint, yawIn, yawOut;
+    double rollPoint, rollIn, rollOut;
+    double pitchPoint, pitchIn, pitchOut;
+    PID pidy, pidr, pidp;
+
+public:
+    void Start_IMU() {
+        pc.printf("Starting up\n\r");
+        pc.baud(9600);
+        i2c.frequency(400000);  // use fast (400 kHz) I2C
+        IMUinit(mpu6050);
+        IMUPrintData(mpu6050);
+    }
+
+    //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),
+        pidy(&yawIn, &yawOut, &yawPoint,1,1,1, DIRECT),
+        pidr(&rollIn, &rollOut, &rollPoint,1,1,1, DIRECT),
+        pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT) {
+
+        pidy.SetMode(AUTOMATIC);  //Yaw PID
+        pidy.SetOutputLimits(0,255);
+        yawPoint = 125;
+        pidr.SetMode(AUTOMATIC);  //Yaw PID
+        pidr.SetOutputLimits(0,255);
+        rollPoint = 125;
+        pidp.SetMode(AUTOMATIC);  //Yaw PID
+        pidp.SetOutputLimits(0,255);
+        rollPoint = 125;
+        
+        Start_IMU();
+        pc.printf("Seagoat Ready to Go\n\r");
+    }
+
+    void SetYawPID(double Kp, double Ki, double Kd) {
+        pidy.SetTunings(Kp, Ki, Kd);
+    }
+
+    void SetRollPID(double Kp, double Ki, double Kd) {
+        pidy.SetTunings(Kp, Ki, Kd);
+    }
+
+    void SetPitchPID(double Kp, double Ki, double Kd) {
+        pidy.SetTunings(Kp, Ki, Kd);
+    }
+    
+    //This is where the magic happens
+    void update(){
+            
+    }
+};
+#endif
\ No newline at end of file