From 187ee6eb84184d35e2c355265fd1a97419f87eb6 Mon Sep 17 00:00:00 2001 From: Brendan Date: Sat, 21 Feb 2015 19:02:33 -0700 Subject: [PATCH] simple test of porportional part of pid --- CopterController/Makefile | 14 ++-- CopterController/{pid.cpp => PID.cpp} | 23 +++--- CopterController/{pid.h => PID.h} | 4 +- CopterController/{pidTest.cpp => main.cpp} | 81 ++++++++++++++++++---- 4 files changed, 91 insertions(+), 31 deletions(-) rename CopterController/{pid.cpp => PID.cpp} (54%) rename CopterController/{pid.h => PID.h} (79%) rename CopterController/{pidTest.cpp => main.cpp} (65%) diff --git a/CopterController/Makefile b/CopterController/Makefile index 2aa8a84..52fbe77 100644 --- a/CopterController/Makefile +++ b/CopterController/Makefile @@ -1,12 +1,13 @@ -all: pid.o I2Cdev.o MPU6050.o PCA9685.o PCA9685tester pidTest +all: PID.o I2Cdev.o MPU6050.o PCA9685.o echoServerAdvanced PCA9685tester main #pidTest MPU6050dmp_tester PCA9685tester -HDRS = pid.h helper_3dmath.h I2Cdev.h MPU6050_6Axis_MotionApps20.h MPU6050.h PCA9685.h PCA9685_Addresses.h +HDRS = PID.h helper_3dmath.h I2Cdev.h MPU6050_6Axis_MotionApps20.h MPU6050.h PCA9685.h PCA9685_Addresses.h CXXFLAGS = -DDMP_FIFO_RATE=9 -Wall -lwiringPi -pid.o pidTest.o I2Cdev.o MPU6050.o MPU6050dmp_tester.o PCA9685.o PCA9685tester.o: $(HDRS) +I2Cdev.o MPU6050.o PCA9685.o PID.o: $(HDRS) +MPU6050dmp_tester.o PCA9685tester.o echoServerAdvanced.o main.o: $(HDRS) -pidTest: pidTest.o pid.o PCA9685.o I2Cdev.o MPU6050.o +main: main.o PID.o PCA9685.o I2Cdev.o MPU6050.o $(CXX) $^ -lwiringPi -o $@ MPU6050dmp_tester: I2Cdev.o MPU6050.o MPU6050dmp_tester.o @@ -15,5 +16,8 @@ MPU6050dmp_tester: I2Cdev.o MPU6050.o MPU6050dmp_tester.o PCA9685tester: PCA9685.o PCA9685tester.o $(CXX) $^ -lwiringPi -o $@ +echoServerAdvanced: echoServerAdvanced.o PCA9685.o + $(CXX) $^ -lwiringPi -o $@ + clean: - rm I2Cdev.o MPU6050.o MPU6050dmp.o MPU6050dmp_tester.o MPU6050dmp_tester pid.o pidTest.o pidTest PCA9685tester \ No newline at end of file + rm I2Cdev.o MPU6050.o MPU6050dmp.o MPU6050dmp_tester.o MPU6050dmp_tester PID.o main.o main PCA9685tester \ No newline at end of file diff --git a/CopterController/pid.cpp b/CopterController/PID.cpp similarity index 54% rename from CopterController/pid.cpp rename to CopterController/PID.cpp index 4860ce4..8f0747f 100644 --- a/CopterController/pid.cpp +++ b/CopterController/PID.cpp @@ -2,10 +2,10 @@ * pid.cpp */ -#include "pid.h" +#include "PID.h" #include -pid::pid( double p, double i, double d ) +PID::PID( double p, double i, double d ) { pGain = p; iGain = i; @@ -17,29 +17,30 @@ pid::pid( double p, double i, double d ) lastUpdate = clock(); } -double pid::update( double current, double desired ) +double PID::update( double current, double desired ) { double pAdjust, iAdjust, dAdjust; double difference; double posChange; clock_t newTime; + clock_t deltaT; + newTime = clock(); + deltaT = newTime - lastUpdate; difference = desired - current; - // p + // Porportional pAdjust = pGain * difference; - // i - newTime = clock(); - integral += difference * (newTime - lastUpdate); + // Integral + integral += difference * deltaT; iAdjust = -iGain * integral; - lastUpdate = newTime; - // d + // Differential posChange = current - lastPosition; - dAdjust = -dGain * posChange; + dAdjust = -dGain * posChange / deltaT ; - return pAdjust + iAdjust + dAdjust; + return (1 + pAdjust) * (1 + iAdjust) * (1 + dAdjust) - 1; } diff --git a/CopterController/pid.h b/CopterController/PID.h similarity index 79% rename from CopterController/pid.h rename to CopterController/PID.h index 694b591..e68cc49 100644 --- a/CopterController/pid.h +++ b/CopterController/PID.h @@ -7,10 +7,10 @@ #include -class pid +class PID { public: - pid( double p, double i, double d ); + PID( double p = 0, double i = 0, double d = 0 ); double update( double current, double desired ); private: diff --git a/CopterController/pidTest.cpp b/CopterController/main.cpp similarity index 65% rename from CopterController/pidTest.cpp rename to CopterController/main.cpp index 80a8903..4d4870b 100644 --- a/CopterController/pidTest.cpp +++ b/CopterController/main.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include #include #include #include @@ -10,7 +10,9 @@ #include #include "fcntl.h" -#include "pid.h" + +#include "wiringPi.h" +#include "PID.h" #include "PCA9685.h" #include "MPU6050_6Axis_MotionApps20.h" @@ -24,11 +26,18 @@ int packetSize; MPU6050 mpu; // servo control stuff -#define MOTOR_MIN 200 +#define MOTOR_OFF 200 +#define MOTOR_MIN 220 #define MOTOR_MAX 600 #define SERVO_FREQ 60 PCA9685 servo; +// PID stuff +#define P_GAIN 1 +#define SIN_45 0.70710678118 +PID pid; +int throttle = (int)( ( MOTOR_MAX - MOTOR_MIN ) / 2 + MOTOR_MIN ); + void setupTCP( int portno ) { struct sockaddr_in serv_addr, cli_addr; @@ -64,12 +73,16 @@ void setupTCP( int portno ) void setupPCA() { - servo = PCA9685(); + servo = PCA9685( 0x40 ); servo.setFrequency( SERVO_FREQ ); - servo.setPwm( 0, 0, MOTOR_MIN ); - servo.setPwm( 1, 0, MOTOR_MIN ); - servo.setPwm( 2, 0, MOTOR_MIN ); - servo.setPwm( 3, 0, MOTOR_MIN ); + servo.setPwm( 0, 0, MOTOR_OFF ); + delay(1); + servo.setPwm( 1, 0, MOTOR_OFF ); + delay(1); + servo.setPwm( 2, 0, MOTOR_OFF ); + delay(1); + servo.setPwm( 3, 0, MOTOR_OFF ); + delay(1); } /** @@ -119,6 +132,7 @@ void setup() setupPCA(); setupTCP( 51717 ); setupMPU(); + pid = PID( P_GAIN, 0, 0 ); } void loop() @@ -129,6 +143,19 @@ void loop() Quaternion q; VectorFloat gravity; float ypr[3]; + + float modifier = 1; + int m0, m1, m2, m3; + + // Motor layout + // front + // 3 1 + // \ / + // X + // / \ + // 2 0 + // CW: 0, 3 + // CCW: 1, 2 nread = read( newsockfd, buffer, 255 ); if( nread > 0 ) @@ -137,13 +164,21 @@ void loop() { std::cout << "SHUTTING DOWN TCP SERVER" << std::endl; - servo.setPwm( 0, 0, MOTOR_MIN ); - servo.setPwm( 1, 0, MOTOR_MIN ); - servo.setPwm( 2, 0, MOTOR_MIN ); - servo.setPwm( 3, 0, MOTOR_MIN ); + servo.setPwm( 0, 0, MOTOR_OFF ); + servo.setPwm( 1, 0, MOTOR_OFF ); + servo.setPwm( 2, 0, MOTOR_OFF ); + servo.setPwm( 3, 0, MOTOR_OFF ); + + std::cout << "\t0\t0\t0\t0" << std::endl; close( newsockfd ); close( sockfd ); + + exit( EXIT_SUCCESS ); + } + if( strncmp( buffer, "SET_MOTOR_", strlen( "SET_MOTOR_" ) ) == 0 ) + { + throttle = atoi( buffer + 12 ); } } @@ -160,8 +195,28 @@ void loop() mpu.dmpGetYawPitchRoll( ypr, &q, &gravity ); std::cout << "pitch: " << ypr[1] << std::endl; - } + modifier = pid.update( ypr[1], 0 ); + m0 = (int)( ( 1 - modifier ) * throttle ); + m1 = (int)( ( 1 + modifier ) * throttle ); + m2 = (int)( ( 1 - modifier ) * throttle ); + m3 = (int)( ( 1 + modifier ) * throttle ); + + if( m0 < MOTOR_MIN ) m0 = MOTOR_MIN; + if( m0 > MOTOR_MAX ) m0 = MOTOR_MAX; + if( m1 < MOTOR_MIN ) m1 = MOTOR_MIN; + if( m1 > MOTOR_MAX ) m1 = MOTOR_MAX; + if( m2 < MOTOR_MIN ) m2 = MOTOR_MIN; + if( m2 > MOTOR_MAX ) m2 = MOTOR_MAX; + if( m3 < MOTOR_MIN ) m3 = MOTOR_MIN; + if( m3 > MOTOR_MAX ) m3 = MOTOR_MAX; + + std::cout << "\t" << m0 << "\t" << m1 << "\t" << m2 << "\t" << m3 << std::endl; + servo.setPwm( 0, 0, m0 ); + servo.setPwm( 1, 0, m1 ); + servo.setPwm( 2, 0, m2 ); + servo.setPwm( 3, 0, m3 ); + } } int main( int argc, char* argv[] )