Robosub controller
Dependencies: IMU MODSERIAL Servo mbed
Fork of RTOS_Controller by
Diff: vessel.h
- Revision:
- 6:b45b74fd6a07
- Parent:
- 5:07bbe020eb65
- Child:
- 7:396fa2a8648d
--- a/vessel.h Tue Jul 26 17:22:33 2016 +0000 +++ b/vessel.h Wed Jul 27 02:45:45 2016 +0000 @@ -2,10 +2,12 @@ #define VESSEL_H #include "mbed.h" +#include "MODSERIAL.h" #include "MPU6050.h" #include "Servo.h" #include "IMU.h" #include "PID.h" +#include <string> /* Cameras FL ----- F ->--- FR @@ -28,6 +30,8 @@ | | | 6 ---<- 5 ----- 4 */ +#define BUFFER_SIZE 255 + class Vessel { @@ -58,8 +62,9 @@ double xPoint, xIn, xOut; double yPoint, yIn, yOut; double zPoint, zIn, zOut; + double p_gain, i_gain, d_gain; PID pidy, pidr, pidp, pidX, pidY, pidZ; - + char buffer[BUFFER_SIZE]; public: void Start_IMU() { pc.printf("Starting up\n\r"); @@ -76,7 +81,8 @@ 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){ + pidZ(&zIn, &zOut, &zPoint,1,1,1, DIRECT) + { pidy.SetMode(AUTOMATIC); //Yaw PID pidy.SetOutputLimits(-255,255); @@ -258,10 +264,54 @@ //pc.printf("%f,%f,%f,%f\n\r",accxs, yaws, yy, (accxs + yaws) / yy + 0.5); //pc.printf("%f,%f,%f,%f,%f \n\r",acczs, pitchs, rolls, zpr, (acczs + pitchs + rolls) / 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, %f\n\r", xOut, yawOut, yawIn, yawPoint); - //pc.printf("YAW: %f, %f, %f, %f\n\r", yaw, yawOut, yawIn, yawPoint); - //pc.printf("ACC: %f, %f, %f\n\r", ax, ay, az); + // pc.printf("YAW: %f,%f, %f\n\r", ax, ay, az); //pc.printf("YPR: %f, %f, %f\n\r", yaw, pitch, roll); } + +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); + } +} }; + #endif \ No newline at end of file