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

simple test of porportional part of pid

Browse Source
This commit is contained in:
Brendan Haines 2015-02-21 19:02:33 -07:00
parent c99551a422
commit 187ee6eb84
4 changed files with 91 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
CopterController/Makefile
View File

@ -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 #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 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 $@ $(CXX) $^ -lwiringPi -o $@
MPU6050dmp_tester: I2Cdev.o MPU6050.o MPU6050dmp_tester.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 PCA9685tester: PCA9685.o PCA9685tester.o
$(CXX) $^ -lwiringPi -o $@ $(CXX) $^ -lwiringPi -o $@
echoServerAdvanced: echoServerAdvanced.o PCA9685.o
$(CXX) $^ -lwiringPi -o $@
clean: clean:
rm I2Cdev.o MPU6050.o MPU6050dmp.o MPU6050dmp_tester.o MPU6050dmp_tester pid.o pidTest.o pidTest PCA9685tester rm I2Cdev.o MPU6050.o MPU6050dmp.o MPU6050dmp_tester.o MPU6050dmp_tester PID.o main.o main PCA9685tester

23
CopterController/pid.cpp → CopterController/PID.cpp
View File

@ -2,10 +2,10 @@
* pid.cpp * pid.cpp
*/ */
#include "pid.h" #include "PID.h"
#include <ctime> #include <ctime>
pid::pid( double p, double i, double d ) PID::PID( double p, double i, double d )
{ {
pGain = p; pGain = p;
iGain = i; iGain = i;
@ -17,29 +17,30 @@ pid::pid( double p, double i, double d )
lastUpdate = clock(); lastUpdate = clock();
} }
double pid::update( double current, double desired ) double PID::update( double current, double desired )
{ {
double pAdjust, iAdjust, dAdjust; double pAdjust, iAdjust, dAdjust;
double difference; double difference;
double posChange; double posChange;
clock_t newTime; clock_t newTime;
clock_t deltaT;
newTime = clock();
deltaT = newTime - lastUpdate;
difference = desired - current; difference = desired - current;
// p // Porportional
pAdjust = pGain * difference; pAdjust = pGain * difference;
// i // Integral
newTime = clock(); integral += difference * deltaT;
integral += difference * (newTime - lastUpdate);
iAdjust = -iGain * integral; iAdjust = -iGain * integral;
lastUpdate = newTime;
// d // Differential
posChange = current - lastPosition; posChange = current - lastPosition;
dAdjust = -dGain * posChange; dAdjust = -dGain * posChange / deltaT ;
return pAdjust + iAdjust + dAdjust; return (1 + pAdjust) * (1 + iAdjust) * (1 + dAdjust) - 1;
} }

4
CopterController/pid.h → CopterController/PID.h
View File

@ -7,10 +7,10 @@
#include <ctime> #include <ctime>
class pid class PID
{ {
public: public:
pid( double p, double i, double d ); PID( double p = 0, double i = 0, double d = 0 );
double update( double current, double desired ); double update( double current, double desired );
private: private:

81
CopterController/pidTest.cpp → CopterController/main.cpp
View File

@ -1,6 +1,6 @@
#include <iostream> #include <iostream>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <cstdlib>
#include <string.h> #include <string.h>
#include <unistd.h> #include <unistd.h>
#include <sys/types.h> #include <sys/types.h>
@ -10,7 +10,9 @@
#include <sys/ioctl.h> #include <sys/ioctl.h>
#include "fcntl.h" #include "fcntl.h"
#include "pid.h"
#include "wiringPi.h"
#include "PID.h"
#include "PCA9685.h" #include "PCA9685.h"
#include "MPU6050_6Axis_MotionApps20.h" #include "MPU6050_6Axis_MotionApps20.h"
@ -24,11 +26,18 @@ int packetSize;
MPU6050 mpu; MPU6050 mpu;
// servo control stuff // servo control stuff
#define MOTOR_MIN 200 #define MOTOR_OFF 200
#define MOTOR_MIN 220
#define MOTOR_MAX 600 #define MOTOR_MAX 600
#define SERVO_FREQ 60 #define SERVO_FREQ 60
PCA9685 servo; 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 ) void setupTCP( int portno )
{ {
struct sockaddr_in serv_addr, cli_addr; struct sockaddr_in serv_addr, cli_addr;
@ -64,12 +73,16 @@ void setupTCP( int portno )
void setupPCA() void setupPCA()
{ {
servo = PCA9685(); servo = PCA9685( 0x40 );
servo.setFrequency( SERVO_FREQ ); servo.setFrequency( SERVO_FREQ );
servo.setPwm( 0, 0, MOTOR_MIN ); servo.setPwm( 0, 0, MOTOR_OFF );
servo.setPwm( 1, 0, MOTOR_MIN ); delay(1);
servo.setPwm( 2, 0, MOTOR_MIN ); servo.setPwm( 1, 0, MOTOR_OFF );
servo.setPwm( 3, 0, MOTOR_MIN ); 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(); setupPCA();
setupTCP( 51717 ); setupTCP( 51717 );
setupMPU(); setupMPU();
pid = PID( P_GAIN, 0, 0 );
} }
void loop() void loop()
@ -129,6 +143,19 @@ void loop()
Quaternion q; Quaternion q;
VectorFloat gravity; VectorFloat gravity;
float ypr[3]; 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 ); nread = read( newsockfd, buffer, 255 );
if( nread > 0 ) if( nread > 0 )
@ -137,13 +164,21 @@ void loop()
{ {
std::cout << "SHUTTING DOWN TCP SERVER" << std::endl; std::cout << "SHUTTING DOWN TCP SERVER" << std::endl;
servo.setPwm( 0, 0, MOTOR_MIN ); servo.setPwm( 0, 0, MOTOR_OFF );
servo.setPwm( 1, 0, MOTOR_MIN ); servo.setPwm( 1, 0, MOTOR_OFF );
servo.setPwm( 2, 0, MOTOR_MIN ); servo.setPwm( 2, 0, MOTOR_OFF );
servo.setPwm( 3, 0, MOTOR_MIN ); servo.setPwm( 3, 0, MOTOR_OFF );
std::cout << "\t0\t0\t0\t0" << std::endl;
close( newsockfd ); close( newsockfd );
close( sockfd ); 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 ); mpu.dmpGetYawPitchRoll( ypr, &q, &gravity );
std::cout << "pitch: " << ypr[1] << std::endl; 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[] ) int main( int argc, char* argv[] )