Andrew Olguin / Mbed 2 deprecated RTOS_Controller_v3

Updated version with comments

Dependencies:   IMU MODSERIAL Servo mbed

Fork of RTOS_Controller_v3 by Marco Rubio

vessel.h@4:b37fd183e46a, 2016-07-09 (annotated)

Committer:
gelmes
Date:
Sat Jul 09 20:41:49 2016 +0000
Revision:
4:b37fd183e46a
Parent:
3:5ffe7e9c0bb3
Child:
5:07bbe020eb65
Implementing Update function that controls motors

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 4:b37fd183e46a 35 // Servo m0;
gelmes 4:b37fd183e46a 36 // Servo m1;
gelmes 4:b37fd183e46a 37 // Servo m2;
gelmes 4:b37fd183e46a 38 // Servo m3;
gelmes 4:b37fd183e46a 39 // Servo m4;
gelmes 4:b37fd183e46a 40 // Servo m5;
gelmes 4:b37fd183e46a 41 // Servo m6;
gelmes 4:b37fd183e46a 42 // Servo m7;
gelmes 4:b37fd183e46a 43
gelmes 4:b37fd183e46a 44 PwmOut m0;
gelmes 4:b37fd183e46a 45 PwmOut m1;
gelmes 4:b37fd183e46a 46 PwmOut m2;
gelmes 4:b37fd183e46a 47 PwmOut m3;
gelmes 4:b37fd183e46a 48 PwmOut m4;
gelmes 4:b37fd183e46a 49 PwmOut m5;
gelmes 4:b37fd183e46a 50 PwmOut m6;
gelmes 4:b37fd183e46a 51 PwmOut m7;
gelmes 3:5ffe7e9c0bb3 52
gelmes 3:5ffe7e9c0bb3 53 PwmOut led1;
gelmes 3:5ffe7e9c0bb3 54 MPU6050 mpu6050;
gelmes 3:5ffe7e9c0bb3 55 double yawPoint, yawIn, yawOut;
gelmes 3:5ffe7e9c0bb3 56 double rollPoint, rollIn, rollOut;
gelmes 3:5ffe7e9c0bb3 57 double pitchPoint, pitchIn, pitchOut;
gelmes 3:5ffe7e9c0bb3 58 PID pidy, pidr, pidp;
gelmes 3:5ffe7e9c0bb3 59
gelmes 3:5ffe7e9c0bb3 60 public:
gelmes 3:5ffe7e9c0bb3 61 void Start_IMU() {
gelmes 3:5ffe7e9c0bb3 62 pc.printf("Starting up\n\r");
gelmes 3:5ffe7e9c0bb3 63 pc.baud(9600);
gelmes 3:5ffe7e9c0bb3 64 i2c.frequency(400000); // use fast (400 kHz) I2C
gelmes 3:5ffe7e9c0bb3 65 IMUinit(mpu6050);
gelmes 3:5ffe7e9c0bb3 66 IMUPrintData(mpu6050);
gelmes 3:5ffe7e9c0bb3 67 }
gelmes 3:5ffe7e9c0bb3 68
gelmes 3:5ffe7e9c0bb3 69 //Initialise all of the vessels starting parameters
gelmes 3:5ffe7e9c0bb3 70 Vessel(): m0(D2),m1(D3),m2(D4),m3(D5),m4(D6),m5(D7),m6(D8),m7(D9), led1(LED1),
gelmes 3:5ffe7e9c0bb3 71 pidy(&yawIn, &yawOut, &yawPoint,1,1,1, DIRECT),
gelmes 3:5ffe7e9c0bb3 72 pidr(&rollIn, &rollOut, &rollPoint,1,1,1, DIRECT),
gelmes 3:5ffe7e9c0bb3 73 pidp(&pitchIn, &pitchOut, &pitchPoint,1,1,1, DIRECT) {
gelmes 3:5ffe7e9c0bb3 74
gelmes 3:5ffe7e9c0bb3 75 pidy.SetMode(AUTOMATIC); //Yaw PID
gelmes 4:b37fd183e46a 76 pidy.SetOutputLimits(-255,255);
gelmes 4:b37fd183e46a 77 yawPoint = 0;
gelmes 3:5ffe7e9c0bb3 78 pidr.SetMode(AUTOMATIC); //Yaw PID
gelmes 4:b37fd183e46a 79 pidr.SetOutputLimits(-255,255);
gelmes 4:b37fd183e46a 80 rollPoint = 0;
gelmes 3:5ffe7e9c0bb3 81 pidp.SetMode(AUTOMATIC); //Yaw PID
gelmes 4:b37fd183e46a 82 pidp.SetOutputLimits(-255,255);
gelmes 4:b37fd183e46a 83 rollPoint = 0;
gelmes 3:5ffe7e9c0bb3 84
gelmes 3:5ffe7e9c0bb3 85 Start_IMU();
gelmes 3:5ffe7e9c0bb3 86 pc.printf("Seagoat Ready to Go\n\r");
gelmes 3:5ffe7e9c0bb3 87 }
gelmes 3:5ffe7e9c0bb3 88
gelmes 3:5ffe7e9c0bb3 89 void SetYawPID(double Kp, double Ki, double Kd) {
gelmes 3:5ffe7e9c0bb3 90 pidy.SetTunings(Kp, Ki, Kd);
gelmes 3:5ffe7e9c0bb3 91 }
gelmes 3:5ffe7e9c0bb3 92
gelmes 3:5ffe7e9c0bb3 93 void SetRollPID(double Kp, double Ki, double Kd) {
gelmes 4:b37fd183e46a 94 pidr.SetTunings(Kp, Ki, Kd);
gelmes 3:5ffe7e9c0bb3 95 }
gelmes 3:5ffe7e9c0bb3 96
gelmes 3:5ffe7e9c0bb3 97 void SetPitchPID(double Kp, double Ki, double Kd) {
gelmes 4:b37fd183e46a 98 pidp.SetTunings(Kp, Ki, Kd);
gelmes 3:5ffe7e9c0bb3 99 }
gelmes 3:5ffe7e9c0bb3 100
gelmes 3:5ffe7e9c0bb3 101 //This is where the magic happens
gelmes 3:5ffe7e9c0bb3 102 void update(){
gelmes 4:b37fd183e46a 103 //Update IMU Values
gelmes 4:b37fd183e46a 104 IMUUpdate(mpu6050);
gelmes 4:b37fd183e46a 105 yawIn = yaw;
gelmes 4:b37fd183e46a 106 rollIn = roll;
gelmes 4:b37fd183e46a 107 pitchIn = pitch;
gelmes 4:b37fd183e46a 108
gelmes 4:b37fd183e46a 109 //Calculate PID values
gelmes 4:b37fd183e46a 110 pidy.Compute();
gelmes 4:b37fd183e46a 111 //pidr.Compute();
gelmes 4:b37fd183e46a 112 //pidp.Compute();
gelmes 4:b37fd183e46a 113
gelmes 4:b37fd183e46a 114 //Spit out PID values
gelmes 4:b37fd183e46a 115 double yo = abs(yawOut/255); //Dividing once to reduce overhead
gelmes 4:b37fd183e46a 116
gelmes 4:b37fd183e46a 117 m0 = yo;
gelmes 4:b37fd183e46a 118 m1 = yo;
gelmes 4:b37fd183e46a 119 m2 = yo;
gelmes 4:b37fd183e46a 120 m3 = yo;
gelmes 4:b37fd183e46a 121 m4 = yo;
gelmes 4:b37fd183e46a 122 m5 = yo;
gelmes 4:b37fd183e46a 123 m6 = yo;
gelmes 4:b37fd183e46a 124 m7 = yo;
gelmes 4:b37fd183e46a 125
gelmes 4:b37fd183e46a 126 pc.printf("yaw0: %f, %f, %f, %f\n\r", yaw, yawOut, yawIn, yawPoint);
gelmes 3:5ffe7e9c0bb3 127 }
gelmes 3:5ffe7e9c0bb3 128 };
gelmes 3:5ffe7e9c0bb3 129 #endif