Updated version with comments
Dependencies: IMU MODSERIAL Servo mbed
Fork of RTOS_Controller_v3 by
vessel.h
- Committer:
- aolgu003
- Date:
- 2016-07-29
- Revision:
- 7:396fa2a8648d
- Parent:
- 6:b45b74fd6a07
- Child:
- 8:2db98df6fbbb
File content as of revision 7:396fa2a8648d:
#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); /* Cameras FL ----- F ->--- FR | | | ˄ | | | | | L | R | | | | | ˅ | | | BL ---<- B ----- BR 0 ----- 1 ->--- 2 | | | ˄ | | | | | 7 | 3 | | | | | ˅ | | | 6 ---<- 5 ----- 4 */ #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; // PwmOut m0; // PwmOut m1; // PwmOut m2; // PwmOut m3; // PwmOut m4; // PwmOut m5; // PwmOut m6; // PwmOut m7; PwmOut led1; MPU6050 mpu6050; double yawPoint, yawIn, yawOut; double rollPoint, rollIn, rollOut; double pitchPoint, pitchIn, pitchOut; 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); } //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); } //This is where the magic happens // void motorTest(){ // pwmSweep(m0); // pwmSweep(m1); // pwmSweep(m2); // pwmSweep(m3); // pwmSweep(m4); // pwmSweep(m5); // pwmSweep(m6); // pwmSweep(m7); // } // // void pwmSweep(PwmOut motor){ // for(float i = 0; i < 80; i++){ // motor = i/255; // wait(0.002); // } // // for(float i = 80; i >= 0; i--){ //// motor = i/255; //// wait(0.002); //// } // } 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){ initMotors(); motorState = 1; pc.printf("Motors Detected"); yawPoint = yaw; } 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); //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; // float zpr = (zOut + pitchOut + rollOut) * 255; // float yy = (yOut + yawOut) * 255; // float xy = (xOut + yawOut) * 255; // if (abs(zpr)<255 && abs(zpr)>=0) zpr = 255; // if (abs(yy)<255 && abs(yy)>=0) yy = 255; // if (abs(xy)<255 && abs(xy)>=0) xy = 255; // if (abs(zpr)>-255 && abs(zpr)<0) zpr = -255; // if (abs(yy)>-255 && abs(yy)<0) yy = -255; // if (abs(xy)>-255 && abs(xy)<0) xy = -255; if (abs(zpr)<255) zpr = 255; if (abs(yy)<255) yy = 255; if (abs(xy)<255) xy = 255; //pc.printf("YAW: %f, %f, %f, %f, %f\n\r", zOut, pitchOut, rollOut, zpr, abs((acczs + pitchs + rolls) / zpr)); //Spit out PID values // m0 = abs((acczs + pitchs + rolls) / zpr);// // m1 = abs((accys + yaws) / yy); // m2 = abs((acczs + pitchs - rolls) / zpr);// // m3 = abs((accxs + yaws) / xy); // m4 = abs((acczs - pitchs - rolls) / zpr);// // m5 = abs((accys + yaws) / yy); // m6 = abs((acczs - pitchs + rolls) / zpr);// // m7 = abs((accxs + yaws) / yy); m0 = (acczs + pitchs + rolls - depths) / zpr + 0.5;// m1 = (accys + yaws) / yy + 0.5; m2 = (acczs + pitchs - rolls - depths) / -zpr + 0.5;// m3 = (accxs + yaws) / -xy + 0.5; m4 = (acczs - pitchs - rolls - depths) / zpr + 0.5;// m5 = (accys + yaws) / yy + 0.5; m6 = (acczs - pitchs + rolls - depths) / -zpr + 0.5;// m7 = (accxs + yaws) / yy + 0.5; //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(), acczs, yaws, pitchs, rolls, zpr, depths, (acczs + pitchs + rolls - depths) / zpr + 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 c = 0; string command; char buffer[BUFFER_SIZE] = {' '}; int buffer_iter = 0; //pc.printf("Checking for command\n"); // Note: you need to actually read from the serial to clear the RX interrupt if (pc.readable()) { pc.printf("Found command\n"); while (pc.readable()) { c = pc.getc(); pc.putc(c); buffer[buffer_iter] = c; buffer_iter++; } pc.printf("Command saved to buffer\n"); command = strtok (buffer," ,\n"); if (strcmp(command.c_str(), "a")) { this->yawPoint = atof(strtok (NULL, " ,\n")); this->pitchPoint = atof(strtok (NULL, " ,\n")); this->rollPoint = atof(strtok (NULL, " ,\n")); pc.printf("Received Attitude points: yawPoint: %f, pitchPoint: %f, rollPoint: %f\n", this->yawPoint, this->pitchPoint, this->rollPoint); } else if (strcmp(command.c_str(), "b")) { this->xPoint = atof(strtok (NULL, " ,\n")); this->yPoint = atof(strtok (NULL, " ,\n")); this->zPoint = atof(strtok (NULL, " ,\n")); pc.printf("Received X,Y,Z points: X: %f, Y: %f, Z: %f\n", this->xPoint, this->yPoint, this->zPoint); } else if (strcmp(command.c_str(), "c")) { this->p_gain = atof(strtok (NULL, " ,\n")); this->i_gain = atof(strtok (NULL, " ,\n")); this->d_gain = atof(strtok (NULL, " ,\n")); this->SetYawPID(this->p_gain, this->i_gain, this->d_gain); pc.printf("Received PID values P: %f, I: %f, D: %f\n", this->p_gain, this->i_gain, this->d_gain); } memset(buffer, ' ', sizeof(buffer)); buffer_iter = 0; fflush(stdout); } } void initMotors(){ neutralizeMotors(); m0.calibrate(); m1.calibrate(); m2.calibrate(); m3.calibrate(); m4.calibrate(); m5.calibrate(); m6.calibrate(); m7.calibrate(); } 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