Robosub controller
Dependencies: IMU MODSERIAL Servo mbed
Fork of RTOS_Controller by
Revision 7:396fa2a8648d, committed 2016-07-29
- Comitter:
- aolgu003
- Date:
- Fri Jul 29 15:34:59 2016 +0000
- Parent:
- 6:b45b74fd6a07
- Commit message:
- Fixed issues and integrated with the sub.
Changed in this revision
diff -r b45b74fd6a07 -r 396fa2a8648d IMU.h --- a/IMU.h Wed Jul 27 02:45:45 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,219 +0,0 @@ - -#include "MPU6050.h" -#include "communication.h" - -float sum = 0; -uint32_t sumCount = 0; - -Timer t; - -void IMUinit(MPU6050 &mpu6050) -{ - t.start(); - -// Read the WHO_AM_I register, this is a good test of communication - uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 - pc.printf("I AM 0x%x\n\r", whoami); - pc.printf("I SHOULD BE 0x68\n\r"); - - if (whoami == 0x68) { // WHO_AM_I should always be 0x68 - pc.printf("MPU6050 is online..."); - wait(1); - //lcd.clear(); - //lcd.printString("MPU6050 OK", 0, 0); - - - mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values - pc.printf("x-axis self test: acceleration trim within : "); - pc.printf("%f", SelfTest[0]); - pc.printf("% of factory value \n\r"); - pc.printf("y-axis self test: acceleration trim within : "); - pc.printf("%f", SelfTest[1]); - pc.printf("% of factory value \n\r"); - pc.printf("z-axis self test: acceleration trim within : "); - pc.printf("%f", SelfTest[2]); - pc.printf("% of factory value \n\r"); - pc.printf("x-axis self test: gyration trim within : "); - pc.printf("%f", SelfTest[3]); - pc.printf("% of factory value \n\r"); - pc.printf("y-axis self test: gyration trim within : "); - pc.printf("%f", SelfTest[4]); - pc.printf("% of factory value \n\r"); - pc.printf("z-axis self test: gyration trim within : "); - pc.printf("%f", SelfTest[5]); - pc.printf("% of factory value \n\r"); - wait(1); - - if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) { - mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration - mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers - mpu6050.initMPU6050(); - pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature - wait(2); - } else { - pc.printf("Device did not the pass self-test!\n\r"); - } - } else { - pc.printf("Could not connect to MPU6050: \n\r"); - pc.printf("%#x \n", whoami); - - while(1) ; // Loop forever if communication doesn't happen - } -} - - -void IMUPrintData(MPU6050 &mpu6050) -{ -// If data ready bit set, all data registers have new data - if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt - mpu6050.readAccelData(accelCount); // Read the x/y/z adc values - mpu6050.getAres(); - - // Now we'll calculate the accleration value into actual g's - ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set - ay = (float)accelCount[1]*aRes - accelBias[1]; - az = (float)accelCount[2]*aRes - accelBias[2]; - - mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values - mpu6050.getGres(); - - // Calculate the gyro value into actual degrees per second - gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set - gy = (float)gyroCount[1]*gRes; // - gyroBias[1]; - gz = (float)gyroCount[2]*gRes; // - gyroBias[2]; - - tempCount = mpu6050.readTempData(); // Read the x/y/z adc values - temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade - } - - Now = t.read_us(); - deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update - lastUpdate = Now; - - sum += deltat; - sumCount++; - - if(lastUpdate - firstUpdate > 10000000.0f) { - beta = 0.04; // decrease filter gain after stabilized - zeta = 0.015; // increasey bias drift gain after stabilized - } - - // Pass gyro rate as rad/s - mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); - - // Serial print and/or display at 0.5 s rate independent of data rates - delt_t = t.read_ms() - count; - if (delt_t > 500) { // update LCD once per half-second independent of read rate - - pc.printf("ax = %f", 1000*ax); - pc.printf(" ay = %f", 1000*ay); - pc.printf(" az = %f mg\n\r", 1000*az); - - pc.printf("gx = %f", gx); - pc.printf(" gy = %f", gy); - pc.printf(" gz = %f deg/s\n\r", gz); - - pc.printf(" temperature = %f C\n\r", temperature); - - pc.printf("q0 = %f\n\r", q[0]); - pc.printf("q1 = %f\n\r", q[1]); - pc.printf("q2 = %f\n\r", q[2]); - pc.printf("q3 = %f\n\r", q[3]); - - // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. - // In this coordinate system, the positive z-axis is down toward Earth. - // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. - // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. - // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. - // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. - // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be - // applied in the correct order which for this configuration is yaw, pitch, and then roll. - // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. - yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); - pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); - roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); - pitch *= 180.0f / PI; - yaw *= 180.0f / PI; - roll *= 180.0f / PI; - -// pc.printf("Yaw, Pitch, Roll: \n\r"); -// pc.printf("%f", yaw); -// pc.printf(", "); -// pc.printf("%f", pitch); -// pc.printf(", "); -// pc.printf("%f\n\r", roll); -// pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r"); - - pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll); - pc.printf("average rate = %f\n\r", (float) sumCount/sum); - - //myled= !myled; - count = t.read_ms(); - sum = 0; - sumCount = 0; - } -} - -void IMUUpdate(MPU6050 &mpu6050) -{ - // If data ready bit set, all data registers have new data - if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt - mpu6050.readAccelData(accelCount); // Read the x/y/z adc values - mpu6050.getAres(); - - // Now we'll calculate the accleration value into actual g's - ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set - ay = (float)accelCount[1]*aRes - accelBias[1]; - az = (float)accelCount[2]*aRes - accelBias[2]; - - mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values - mpu6050.getGres(); - - // Calculate the gyro value into actual degrees per second - gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set - gy = (float)gyroCount[1]*gRes; // - gyroBias[1]; - gz = (float)gyroCount[2]*gRes; // - gyroBias[2]; - - tempCount = mpu6050.readTempData(); // Read the x/y/z adc values - temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade - } - - Now = t.read_us(); - deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update - lastUpdate = Now; - - sum += deltat; - sumCount++; - - if(lastUpdate - firstUpdate > 10000000.0f) { - beta = 0.04; // decrease filter gain after stabilized - zeta = 0.015; // increasey bias drift gain after stabilized - } - - // Pass gyro rate as rad/s - mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); - - // Serial print and/or display at 0.5 s rate independent of data rates - delt_t = t.read_ms() - count; - - // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. - // In this coordinate system, the positive z-axis is down toward Earth. - // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. - // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. - // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. - // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. - // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be - // applied in the correct order which for this configuration is yaw, pitch, and then roll. - // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. - yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); - pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); - roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); - pitch *= 180.0f / PI; - yaw *= 180.0f / PI; - roll *= 180.0f / PI; - - count = t.read_ms(); - sum = 0; - sumCount = 0; - -}
diff -r b45b74fd6a07 -r 396fa2a8648d IMU.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMU.lib Fri Jul 29 15:34:59 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/aolgu003/code/IMU/#9a71704df32d
diff -r b45b74fd6a07 -r 396fa2a8648d MS5837/MS5837.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MS5837/MS5837.cpp Fri Jul 29 15:34:59 2016 +0000 @@ -0,0 +1,100 @@ +#include <stdlib.h> +#include "MS5837.h" + + +/* + * Sensor operating function according data sheet + */ + +void MS5837::MS5837Init(void) +{ + MS5837Reset(); + MS5837ReadProm(); + return; +} + +/* Send soft reset to the sensor */ +void MS5837::MS5837Reset(void) +{ + /* transmit out 1 byte reset command */ + ms5837_tx_data[0] = ms5837_reset; + if ( i2c.write( device_address, ms5837_tx_data, 1 ) ); + //printf("send soft reset"); + wait_ms(20); +} + +/* read the sensor calibration data from rom */ +void MS5837::MS5837ReadProm(void) +{ + uint8_t i,j; + for (i=0; i<8; i++) { + j = i; + ms5837_tx_data[0] = ms5837_PROMread + (j<<1); + if ( i2c.write( device_address, ms5837_tx_data, 1 ) ); + if ( i2c.read( device_address, ms5837_rx_data, 2 ) ); + C[i] = ms5837_rx_data[1] + (ms5837_rx_data[0]<<8); + } +} + +/* Start the sensor pressure conversion */ +void MS5837::MS5837ConvertD1(void) +{ + ms5837_tx_data[0] = ms5837_convD1; + if ( i2c.write( device_address, ms5837_tx_data, 1 ) ); +} + +/* Start the sensor temperature conversion */ +void MS5837:: MS5837ConvertD2(void) +{ + ms5837_tx_data[0] = ms5837_convD2; + if ( i2c.write( device_address, ms5837_tx_data, 1 ) ); +} + +/* Read the previous started conversion results */ +int32_t MS5837::MS5837ReadADC(void) +{ + int32_t adc; + wait_ms(150); + ms5837_tx_data[0] = ms5837_ADCread; + if ( i2c.write( device_address, ms5837_tx_data, 1 ) ); + if ( i2c.read( device_address, ms5837_rx_data, 3 ) ); + adc = ms5837_rx_data[2] + (ms5837_rx_data[1]<<8) + (ms5837_rx_data[0]<<16); + return (adc); +} + +/* return the results */ +float MS5837::MS5837_Pressure (void) +{ + return P_MS5837; +} +float MS5837::MS5837_Temperature (void) +{ + return T_MS5837; +} + +/* Sensor reading and calculation procedure */ +void MS5837::Barometer_MS5837(void) +{ + int32_t dT, temp; + int64_t OFF, SENS, press; + + //no need to do this everytime! + //MS5837Reset(); // reset the sensor + //MS5837ReadProm(); // read the calibration values + + + MS5837ConvertD1(); // start pressure conversion + D1 = MS5837ReadADC(); // read the pressure value + MS5837ConvertD2(); // start temperature conversion + D2 = MS5837ReadADC(); // read the temperature value + + /* calculation according MS5837-01BA data sheet DA5837-01BA_006 */ + dT = D2 - (C[5]* 256); + OFF = (int64_t)C[2] * (1<<16) + ((int64_t)dT * (int64_t)C[4]) / (1<<7); + SENS = (int64_t)C[1] * (1<<15) + ((int64_t)dT * (int64_t)C[3]) / (1<<8); + + temp = 2000 + (dT * C[6]) / (1<<23); + T_MS5837 = (float) temp / 100.0f; // result of temperature in deg C in this var + press = (((int64_t)D1 * SENS) / (1<<21) - OFF) / (1<<13); + P_MS5837 = (float) press / 10.0f; // result of pressure in mBar in this var +}
diff -r b45b74fd6a07 -r 396fa2a8648d MS5837/MS5837.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MS5837/MS5837.h Fri Jul 29 15:34:59 2016 +0000 @@ -0,0 +1,64 @@ +#include "mbed.h" + +#ifndef MS5837_H +#define MS5837_H + +#define MS5837_RX_DEPTH 3 // +#define MS5837_TX_DEPTH 2 // + +// choose your connection here +#define ms5837_addr_no_CS 0x76 //0b1110110 + +#define ms5837_reset 0x1E // Sensor Reset + +#define ms5837_convD1_256 0x40 // Convert D1 OSR 256 +#define ms5837_convD1_512 0x42 // Convert D1 OSR 512 +#define ms5837_convD1_1024 0x44 // Convert D1 OSR 1024 +#define ms5837_convD1_2048 0x46 // Convert D1 OSR 2048 +#define ms5837_convD1_4096 0x48 // Convert D1 OSR 4096 +#define ms5837_convD1_8192 0x4A // Convert D1 OSR 8192 + +#define ms5837_convD1 ms5837_convD1_4096 // choose your sampling rate here + +#define ms5837_convD2_256 0x50 // Convert D2 OSR 256 +#define ms5837_convD2_512 0x52 // Convert D2 OSR 512 +#define ms5837_convD2_1024 0x54 // Convert D2 OSR 1024 +#define ms5837_convD2_2048 0x56 // Convert D2 OSR 2048 +#define ms5837_convD2_4096 0x58 // Convert D2 OSR 4096 +#define ms5837_convD2_8192 0x5A // Convert D2 OSR 8192 + +#define ms5837_convD2 ms5837_convD2_4096 // choose your sampling rate here + +#define ms5837_ADCread 0x00 // read ADC command +#define ms5837_PROMread 0xA0 // read PROM command base address + +class MS5837{ +private: + int D1, D2, Temp, C[8]; + float T_MS5837, P_MS5837; + /* Data buffers */ + char ms5837_rx_data[MS5837_RX_DEPTH]; + char ms5837_tx_data[MS5837_TX_DEPTH]; + +public: + MS5837 (PinName sda, PinName scl, + char ms5837_addr = ms5837_addr_no_CS ) + : i2c( sda, scl ), device_address( ms5837_addr << 1 ) { + } + void MS5837Init(void); + void MS5837Reset(void); + void MS5837ReadProm(void); + void MS5837ConvertD1(void); + void MS5837ConvertD2(void); + int32_t MS5837ReadADC(void); + float MS5837_Pressure (void); + float MS5837_Temperature (void); + void Barometer_MS5837(void); + + +private: + I2C i2c; + char device_address; + +}; +#endif
diff -r b45b74fd6a07 -r 396fa2a8648d main.cpp --- a/main.cpp Wed Jul 27 02:45:45 2016 +0000 +++ b/main.cpp Fri Jul 29 15:34:59 2016 +0000 @@ -1,22 +1,23 @@ // Continuously sweep the servo through it's full range #include "mbed.h" #include "vessel.h" - +//#include "MS5803.h" +//MS5803 big_sensor = MS5803(I2C_SDA, I2C_SCL, ms5803_addrCH); int main() { Timer t; wait(3); Vessel seagoat; //Starts the seagoat - seagoat.SetYawPID(4,0,0); - seagoat.SetRollPID(4,0,0); - seagoat.SetPitchPID(4,0,0); + seagoat.SetYawPID(1,0,0); + seagoat.SetRollPID(1,0,0); + seagoat.SetPitchPID(1,0,0); seagoat.SetXPID(0,0,0); seagoat.SetYPID(0,0,0); seagoat.SetZPID(0,0,0); t.start(); - while(t.read() < 5){ + while(t.read() < 4){ seagoat.calibrate(); //pc.printf("%f \n", t.read()); @@ -24,9 +25,8 @@ t.stop(); pc.printf("Seagoat Ready to Go\n\r"); //seagoat.motorTest(); - + float depth = 0; while(1) { - //seagoat.motorTest(); seagoat.update(); seagoat.updateCommand();
diff -r b45b74fd6a07 -r 396fa2a8648d vessel.h --- a/vessel.h Wed Jul 27 02:45:45 2016 +0000 +++ b/vessel.h Fri Jul 29 15:34:59 2016 +0000 @@ -8,6 +8,10 @@ #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 @@ -36,41 +40,48 @@ { private: -// Servo m0; -// Servo m1; -// Servo m2; -// Servo m3; -// Servo m4; -// Servo m5; -// Servo m6; -// Servo m7; - - PwmOut m0; - PwmOut m1; - PwmOut m2; - PwmOut m3; - PwmOut m4; - PwmOut m5; - PwmOut m6; - PwmOut m7; + 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; + 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); } @@ -78,11 +89,13 @@ 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), + 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), + pidd(&dIn, &dOut, &dPoint,1,1,1, DIRECT), + powerPin(A5) +{ pidy.SetMode(AUTOMATIC); //Yaw PID pidy.SetOutputLimits(-255,255); @@ -102,7 +115,11 @@ 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; @@ -112,6 +129,8 @@ m6 = 0.5; m7 = 0.5; + motorState = 1; + Start_IMU(); pc.printf("Seagoat Initialized \n\r"); } @@ -139,44 +158,67 @@ 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--){ +// 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); -// } - } - void calibrate(){ +// } +// // 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(){ + + 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(); @@ -184,7 +226,7 @@ pidX.Compute(); pidY.Compute(); pidZ.Compute(); - + /* Cameras FL ----- F ->--- FR @@ -196,7 +238,7 @@ | | ˅ | | | BL ---<- B ----- BR - + 0 ----- 1 ->--- 2 | | | ˄ | | @@ -206,43 +248,44 @@ | | ˅ | | | 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)) * 255; + 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 xy = (abs(xOut) + abs(yawOut)) * 255; + // float zpr = (zOut + pitchOut + rollOut) * 255; // float yy = (yOut + yawOut) * 255; -// float xy = (xOut + 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(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(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);// @@ -251,67 +294,92 @@ // m5 = abs((accys + yaws) / yy); // m6 = abs((acczs - pitchs + rolls) / zpr);// // m7 = abs((accxs + yaws) / yy); - - m0 = (acczs + pitchs + rolls) / zpr + 0.5;// + + m0 = (acczs + pitchs + rolls - depths) / zpr + 0.5;// m1 = (accys + yaws) / yy + 0.5; - m2 = (acczs + pitchs - rolls) / zpr + 0.5;// - m3 = (accxs + yaws) / xy + 0.5; - m4 = (acczs - pitchs - rolls) / zpr + 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) / zpr + 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 \n\r",acczs, pitchs, rolls, zpr, (acczs + pitchs + rolls) / zpr + 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\n\r", yaw, pitch, roll); + //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 updateCommand() { - char c = 0; - string command; - char buffer[BUFFER_SIZE] = {' '}; - int buffer_iter = 0; - pc.printf("Checking for command\n"); + 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; + } - // 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