brendanhaines/RasPi
1
0
Fork 0
mirror of https://github.com/brendanhaines/RasPi.git synced 2024-09-19 19:55:50 -06:00
Code Issues Packages Projects Releases Wiki Activity

working reaction on pitch and roll axes (porportional only)

Browse Source
This commit is contained in:
Brendan Haines 2015-02-21 23:12:24 -07:00
parent 540b6d6fc6
commit 3d1933563d
1 changed files with 29 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
CopterController/main.cpp
View File

@ -35,9 +35,9 @@ MPU6050 mpu;
PCA9685 servo; PCA9685 servo;
// PID stuff // PID stuff
#define P_GAIN 1 #define P_GAIN 2
#define SIN_45 0.70710678118 #define SIN_45 0.70710678118
PID pid; PID pitchPID, rollPID, yawPID;
int throttle = (int)( ( MOTOR_MAX - MOTOR_MIN ) / 2 + MOTOR_MIN ); int throttle = (int)( ( MOTOR_MAX - MOTOR_MIN ) / 2 + MOTOR_MIN );
void setupTCP( int portno ) void setupTCP( int portno )
@ -134,19 +134,25 @@ void setup()
setupPCA(); setupPCA();
setupTCP( 51717 ); setupTCP( 51717 );
setupMPU(); setupMPU();
pid = PID( P_GAIN, 0, 0 ); pitchPID = PID( P_GAIN, 0, 0 );
rollPID = PID( P_GAIN, 0, 0 );
} }
void loop() void loop()
{ {
int fifoCount; int fifoCount;
int nread, nwrite; int nread;
uint8_t fifoBuffer[64]; uint8_t fifoBuffer[64];
Quaternion q; Quaternion q;
VectorFloat gravity; VectorFloat gravity;
float ypr[3];
float modifier = 1; float pitchAngle;
float rollAngle;
int16_t yawRate;
float pitchMod = 1;
float rollMod = 1;
float yawMod = 1;
int m0, m1, m2, m3; int m0, m1, m2, m3;
// Motor layout // Motor layout
@ -189,20 +195,30 @@ void loop()
{ {
std::cerr << "FIFO overflow" << std::endl; std::cerr << "FIFO overflow" << std::endl;
} }
while( fifoCount >= 84 )
{
mpu.getFIFOBytes( fifoBuffer, packetSize );
fifoCount = mpu.getFIFOCount();
}
if( fifoCount >= 42 ) if( fifoCount >= 42 )
{ {
mpu.getFIFOBytes( fifoBuffer, packetSize ); mpu.getFIFOBytes( fifoBuffer, packetSize );
mpu.dmpGetQuaternion( &q, fifoBuffer ); mpu.dmpGetQuaternion( &q, fifoBuffer );
mpu.dmpGetGravity( &gravity, &q ); mpu.dmpGetGravity( &gravity, &q );
mpu.dmpGetYawPitchRoll( ypr, &q, &gravity );
std::cout << "pitch: " << ypr[1] << std::endl; pitchAngle = atan( gravity.x / sqrt( gravity.y * gravity.y + gravity.z * gravity.z) );
modifier = pid.update( ypr[1], 0 ); rollAngle = atan( gravity.y / sqrt( gravity.x * gravity.x + gravity.z * gravity.z ) );
yawRate = mpu.getRotationZ();
m0 = (int)( ( 1 - modifier ) * throttle ); std::cout << "pitch: " << pitchAngle << "\troll: " << rollAngle << "\tyaw rate: " << yawRate << std::endl;
m1 = (int)( ( 1 + modifier ) * throttle ); pitchMod = pitchPID.update( pitchAngle, 0 );
m2 = (int)( ( 1 - modifier ) * throttle ); rollMod = rollPID.update( rollAngle, 0 );
m3 = (int)( ( 1 + modifier ) * throttle ); yawMod = yawPID.update( yawRate, 0 );
m0 = (int)( ( 1 - pitchMod ) * ( 1 - rollMod ) * throttle );
m1 = (int)( ( 1 + pitchMod ) * ( 1 - rollMod ) * throttle );
m2 = (int)( ( 1 - pitchMod ) * ( 1 + rollMod ) * throttle );
m3 = (int)( ( 1 + pitchMod ) * ( 1 + rollMod ) * throttle );
if( m0 < MOTOR_MIN ) m0 = MOTOR_MIN; if( m0 < MOTOR_MIN ) m0 = MOTOR_MIN;
if( m0 > MOTOR_MAX ) m0 = MOTOR_MAX; if( m0 > MOTOR_MAX ) m0 = MOTOR_MAX;