Working
Dependencies: IMU MODSERIAL Servo mbed
Fork of RTOS_Controller_v2 by
vessel.h
- Committer:
- gelmes
- Date:
- 2016-07-30
- Revision:
- 9:9aaa7f0c8960
- Parent:
- 8:2db98df6fbbb
- Child:
- 10:8cd741a65646
File content as of revision 9:9aaa7f0c8960:
#ifndef VESSEL_H #define VESSEL_H #include "mbed.h" #include "MODSERIAL.h" #include "MPU6050.h" #include "Servo.h" #include "IMU.h" #include "PID.h" #include <string> #include "MS5837.h" MS5837 pressure_sensor = MS5837(I2C_SDA, I2C_SCL, ms5837_addr_no_CS); #define BUFFER_SIZE 255 class Vessel { private: Servo m0; Servo m1; Servo m2; Servo m3; Servo m4; Servo m5; Servo m6; Servo m7; AnalogIn powerPin; int motorState; int runningState; Timer runningTime; // PwmOut m0; // PwmOut m1; // PwmOut m2; // PwmOut m3; // PwmOut m4; // PwmOut m5; // PwmOut m6; // PwmOut m7; PwmOut led1; MPU6050 mpu6050; double yawPoint, yawIn, yawOut, lastyawPoint; double rollPoint, rollIn, rollOut, lastrollPoint; double pitchPoint, pitchIn, pitchOut, lastpitchPoint; 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, 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); runningTime.start(); } //Initialise all of the vessels starting parameters 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), pidX(&xIn, &xOut, &xPoint,1,1,1, DIRECT), pidY(&yIn, &yOut, &yPoint,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); yawPoint = 0; pidr.SetMode(AUTOMATIC); //Roll PID pidr.SetOutputLimits(-255,255); pitchPoint = 0; pidp.SetMode(AUTOMATIC); //Pitch PID pidp.SetOutputLimits(-255,255); rollPoint = 0; pidX.SetMode(AUTOMATIC); //Pitch PID pidX.SetOutputLimits(-255,255); xPoint = 0; pidY.SetMode(AUTOMATIC); //Pitch PID pidY.SetOutputLimits(-255,255); yPoint = 0; 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; m3 = 0.5; m4 = 0.5; m5 = 0.5; m6 = 0.5; m7 = 0.5; motorState = 1; Start_IMU(); pc.printf("Seagoat Initialized \n\r"); } void SetYawPID(double Kp, double Ki, double Kd) { pidy.SetTunings(Kp, Ki, Kd); } void SetRollPID(double Kp, double Ki, double Kd) { pidr.SetTunings(Kp, Ki, Kd); } void SetPitchPID(double Kp, double Ki, double Kd) { pidp.SetTunings(Kp, Ki, Kd); } void SetXPID(double Kp, double Ki, double Kd) { pidX.SetTunings(Kp, Ki, Kd); } void SetYPID(double Kp, double Ki, double Kd) { pidY.SetTunings(Kp, Ki, Kd); } 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); } void calibrate() { IMUUpdate(mpu6050); pc.printf("Calibrating...\n\r"); //pressure_sensor.Barometer_MS5837(); //depth = pressure_sensor.MS5837_Pressure(); } 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) { runningTime.stop(); initMotors(); motorState = 1; runningState += 1; if(runningState == 2) runningState = 0; pc.printf("Motors Detected"); yawPoint = yaw; if(runningState == 1) pitchPoint = pitch; else pitchPoint = 0; dPoint = depth; runningTime.reset(); runningTime.start(); } 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(); pidp.Compute(); pidX.Compute(); pidY.Compute(); pidZ.Compute(); /* Cameras FL ----- F ->--- FR | | | ˄ | | | | | L | R | | | | | ˅ | | | BL ---<- B ----- BR 0 ----- 1 ->--- 2 | | | ˄ | | | | | 7 | 3 | | | | | ˅ | | | 6 ---<- 5 ----- 4 */ //pc.printf("YAW: %f, %f, %f, %f, %f, %f\n\r", xOut, yOut, zOut, yawOut, pitchOut, rollOut); float forwardThrust = 100; //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) + abs(dOut)) * 255; float yy = (abs(yOut) + abs(yawOut)) * 255; float xy = (abs(xOut) + abs(yawOut)) * 255; if (abs(zpr)<255) zpr = 255; if (abs(yy)<255) yy = 255; if (abs(xy)<255) xy = 255; if(runningState == 0) { if(runningTime < 1) { SetYawPID(1,0,0); SetRollPID(1,0,0); SetPitchPID(1,0,0); wait(1); } else{ m0 = (acczs + pitchs + rolls - depths) / zpr + 0.5;// m1 = (accxs + yaws) / -xy + 0.5; m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;// //m3 = (accys + yaws) / yy + 0.5; m3 = 0.7; m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;// m5 = (accxs + yaws) / -xy + 0.5; m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;// //m7 = (-accys + yaws) / -yy + 0.5; m7 = 0.7; } } else if(runningState == 1) { if(runningTime < 1) { SetYawPID(2,0,0.3); SetRollPID(2,0,0.3); SetPitchPID(2,0,0.3); wait(1); } else if(runningTime < 27) { m1 = (accxs + yaws) / -xy + 0.5; m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;// //m3 = (accys + yaws) / yy + 0.5; m3 = 0.7; m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;// m5 = (accxs + yaws) / -xy + 0.5; m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;// //m7 = (-accys + yaws) / -yy + 0.5; m7 = 0.7; } else if(runningTime < 5) { m1 = (accxs + yaws) / -xy + 0.5; m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;// //m3 = (accys + yaws) / yy + 0.5; m3 = 0.7; m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;// m5 = (accxs + yaws) / -xy + 0.5; m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;// //m7 = (-accys + yaws) / -yy + 0.5; m7 = 0.7; } } //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\n\r", ax, ay, az); //pc.printf("YPR: %f, %f, %f, %f\n\r", yaw, pitch, roll, depth); } 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); } } void initMotors() { neutralizeMotors(); wait(0.5); m0.calibrate(); m1.calibrate(); m2.calibrate(); m3.calibrate(); m4.calibrate(); m5.calibrate(); m6.calibrate(); m7.calibrate(); wait(3); } 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; } }; #endif