Andrew Olguin / Mbed 2 deprecated RTOS_Controller_v2

Robosub controller

Dependencies:   IMU MODSERIAL Servo mbed

Fork of RTOS_Controller by Marco Rubio

vessel.h@3:5ffe7e9c0bb3, 2016-07-04 (annotated)

Committer:
gelmes
Date:
Mon Jul 04 18:56:23 2016 +0000
Revision:
3:5ffe7e9c0bb3
Child:
4:b37fd183e46a
Quaternions are holding me back

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gelmes 3:5ffe7e9c0bb3 1 #ifndef VESSEL_H
gelmes 3:5ffe7e9c0bb3 2 #define VESSEL_H
gelmes 3:5ffe7e9c0bb3 3
gelmes 3:5ffe7e9c0bb3 4 #include "mbed.h"
gelmes 3:5ffe7e9c0bb3 5 #include "MPU6050.h"
gelmes 3:5ffe7e9c0bb3 6 #include "Servo.h"
gelmes 3:5ffe7e9c0bb3 7 #include "IMU.h"
gelmes 3:5ffe7e9c0bb3 8 #include "PID.h"
gelmes 3:5ffe7e9c0bb3 9 /*
gelmes 3:5ffe7e9c0bb3 10 Cameras
gelmes 3:5ffe7e9c0bb3 11 FL ----- F ->--- FR
gelmes 3:5ffe7e9c0bb3 12 | | |
gelmes 3:5ffe7e9c0bb3 13 ˄ | |
gelmes 3:5ffe7e9c0bb3 14 | | |
gelmes 3:5ffe7e9c0bb3 15 L | R
gelmes 3:5ffe7e9c0bb3 16 | | |
gelmes 3:5ffe7e9c0bb3 17 | | ˅
gelmes 3:5ffe7e9c0bb3 18 | | |
gelmes 3:5ffe7e9c0bb3 19 BL ---<- B ----- BR
gelmes 3:5ffe7e9c0bb3 20
gelmes 3:5ffe7e9c0bb3 21 0 ----- 1 ->--- 2
gelmes 3:5ffe7e9c0bb3 22 | | |
gelmes 3:5ffe7e9c0bb3 23 ˄ | |
gelmes 3:5ffe7e9c0bb3 24 | | |
gelmes 3:5ffe7e9c0bb3 25 7 | 3
gelmes 3:5ffe7e9c0bb3 26 | | |
gelmes 3:5ffe7e9c0bb3 27 | | ˅
gelmes 3:5ffe7e9c0bb3 28 | | |
gelmes 3:5ffe7e9c0bb3 29 6 ---<- 5 ----- 4
gelmes 3:5ffe7e9c0bb3 30 */
gelmes 3:5ffe7e9c0bb3 31 class Vessel
gelmes 3:5ffe7e9c0bb3 32 {
gelmes 3:5ffe7e9c0bb3 33
gelmes 3:5ffe7e9c0bb3 34 private:
gelmes 3:5ffe7e9c0bb3 35 Servo m0;
gelmes 3:5ffe7e9c0bb3 36 Servo m1;
gelmes 3:5ffe7e9c0bb3 37 Servo m2;
gelmes 3:5ffe7e9c0bb3 38 Servo m3;
gelmes 3:5ffe7e9c0bb3 39 Servo m4;
gelmes 3:5ffe7e9c0bb3 40 Servo m5;
gelmes 3:5ffe7e9c0bb3 41 Servo m6;
gelmes 3:5ffe7e9c0bb3 42 Servo m7;
gelmes 3:5ffe7e9c0bb3 43
gelmes 3:5ffe7e9c0bb3 44 PwmOut led1;
gelmes 3:5ffe7e9c0bb3 45 MPU6050 mpu6050;
gelmes 3:5ffe7e9c0bb3 46 double yawPoint, yawIn, yawOut;
gelmes 3:5ffe7e9c0bb3 47 double rollPoint, rollIn, rollOut;
gelmes 3:5ffe7e9c0bb3 48 double pitchPoint, pitchIn, pitchOut;
gelmes 3:5ffe7e9c0bb3 49 PID pidy, pidr, pidp;
gelmes 3:5ffe7e9c0bb3 50
gelmes 3:5ffe7e9c0bb3 51 public:
gelmes 3:5ffe7e9c0bb3 52 void Start_IMU() {
gelmes 3:5ffe7e9c0bb3 53 pc.printf("Starting up\n\r");
gelmes 3:5ffe7e9c0bb3 54 pc.baud(9600);
gelmes 3:5ffe7e9c0bb3 55 i2c.frequency(400000); // use fast (400 kHz) I2C
gelmes 3:5ffe7e9c0bb3 56 IMUinit(mpu6050);
gelmes 3:5ffe7e9c0bb3 57 IMUPrintData(mpu6050);
gelmes 3:5ffe7e9c0bb3 58 }
gelmes 3:5ffe7e9c0bb3 59
gelmes 3:5ffe7e9c0bb3 60 //Initialise all of the vessels starting parameters
gelmes 3:5ffe7e9c0bb3 61 Vessel(): m0(D2),m1(D3),m2(D4),m3(D5),m4(D6),m5(D7),m6(D8),m7(D9), led1(LED1),
gelmes 3:5ffe7e9c0bb3 62 pidy(&yawIn, &yawOut, &yawPoint,1,1,1, DIRECT),
gelmes 3:5ffe7e9c0bb3 63 pidr(&rollIn, &rollOut, &rollPoint,1,1,1, DIRECT),
gelmes 3:5ffe7e9c0bb3 64 pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT) {
gelmes 3:5ffe7e9c0bb3 65
gelmes 3:5ffe7e9c0bb3 66 pidy.SetMode(AUTOMATIC); //Yaw PID
gelmes 3:5ffe7e9c0bb3 67 pidy.SetOutputLimits(0,255);
gelmes 3:5ffe7e9c0bb3 68 yawPoint = 125;
gelmes 3:5ffe7e9c0bb3 69 pidr.SetMode(AUTOMATIC); //Yaw PID
gelmes 3:5ffe7e9c0bb3 70 pidr.SetOutputLimits(0,255);
gelmes 3:5ffe7e9c0bb3 71 rollPoint = 125;
gelmes 3:5ffe7e9c0bb3 72 pidp.SetMode(AUTOMATIC); //Yaw PID
gelmes 3:5ffe7e9c0bb3 73 pidp.SetOutputLimits(0,255);
gelmes 3:5ffe7e9c0bb3 74 rollPoint = 125;
gelmes 3:5ffe7e9c0bb3 75
gelmes 3:5ffe7e9c0bb3 76 Start_IMU();
gelmes 3:5ffe7e9c0bb3 77 pc.printf("Seagoat Ready to Go\n\r");
gelmes 3:5ffe7e9c0bb3 78 }
gelmes 3:5ffe7e9c0bb3 79
gelmes 3:5ffe7e9c0bb3 80 void SetYawPID(double Kp, double Ki, double Kd) {
gelmes 3:5ffe7e9c0bb3 81 pidy.SetTunings(Kp, Ki, Kd);
gelmes 3:5ffe7e9c0bb3 82 }
gelmes 3:5ffe7e9c0bb3 83
gelmes 3:5ffe7e9c0bb3 84 void SetRollPID(double Kp, double Ki, double Kd) {
gelmes 3:5ffe7e9c0bb3 85 pidy.SetTunings(Kp, Ki, Kd);
gelmes 3:5ffe7e9c0bb3 86 }
gelmes 3:5ffe7e9c0bb3 87
gelmes 3:5ffe7e9c0bb3 88 void SetPitchPID(double Kp, double Ki, double Kd) {
gelmes 3:5ffe7e9c0bb3 89 pidy.SetTunings(Kp, Ki, Kd);
gelmes 3:5ffe7e9c0bb3 90 }
gelmes 3:5ffe7e9c0bb3 91
gelmes 3:5ffe7e9c0bb3 92 //This is where the magic happens
gelmes 3:5ffe7e9c0bb3 93 void update(){
gelmes 3:5ffe7e9c0bb3 94
gelmes 3:5ffe7e9c0bb3 95 }
gelmes 3:5ffe7e9c0bb3 96 };
gelmes 3:5ffe7e9c0bb3 97 #endif