Revision 10:953afcbcebfc, committed 2012-10-17
- Comitter:
- maetugr
- Date:
- Wed Oct 17 08:37:08 2012 +0000
- Parent:
- 9:4e0c3936c756
- Child:
- 11:9bf69bc6df45
- Commit message:
- neue Acc_Winkelberechnung, vor Kompassklassenrevision
Changed in this revision
--- a/HMC5883/HMC5883.cpp Tue Oct 16 10:21:32 2012 +0000
+++ b/HMC5883/HMC5883.cpp Wed Oct 17 08:37:08 2012 +0000
@@ -10,10 +10,10 @@
Init();
// MYINIT ----------
//Kompass kalibrieren --> Problem fremde Magnetfelder!
- //AutoCalibration = 1;
- short MagRawMin[3]= {-400, -400, -400}; //Gespeicherte Werte verwenden
- short MagRawMax[3]= {400, 400, 400};
- Calibrate(MagRawMin, MagRawMax);
+ AutoCalibration = 1;
+ //short MagRawMin[3]= {-400, -400, -400}; //Gespeicherte Werte verwenden
+ //short MagRawMax[3]= {400, 400, 400};
+ //Calibrate(MagRawMin, MagRawMax);
//Calibrate(20);
// MYINIT ----------
}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib Wed Oct 17 08:37:08 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
--- a/RC/RC_Channel.cpp Tue Oct 16 10:21:32 2012 +0000
+++ b/RC/RC_Channel.cpp Wed Oct 17 08:37:08 2012 +0000
@@ -3,8 +3,10 @@
RC_Channel::RC_Channel(PinName mypin) : myinterrupt(mypin)
{
+ time = -1; // start value to see if there was any value yet
myinterrupt.rise(this, &RC_Channel::rise);
myinterrupt.fall(this, &RC_Channel::fall);
+ timeoutchecker.attach(this, &RC_Channel::timeoutcheck, 1);
}
int RC_Channel::read()
@@ -22,4 +24,11 @@
timer.stop();
time = timer.read_us();
timer.reset();
+ timer.start();
+}
+
+void RC_Channel::timeoutcheck()
+{
+ if (timer.read() > 0.3)
+ time = 0;
}
\ No newline at end of file
--- a/RC/RC_Channel.h Tue Oct 16 10:21:32 2012 +0000
+++ b/RC/RC_Channel.h Wed Oct 17 08:37:08 2012 +0000
@@ -6,15 +6,18 @@
class RC_Channel
{
public:
- RC_Channel(PinName mypin); // noooo p19/p20!!!!
- int read();
+ RC_Channel(PinName mypin); // NO p19/p20!!!!, they don't support InterruptIn
+ int read(); // read the last measured data
private:
- InterruptIn myinterrupt;
- void rise();
- void fall();
- Timer timer;
- int time;
+ InterruptIn myinterrupt; // interrupt on the pin to react when signal falls or rises
+ void rise(); // start the time measurement when signal rises
+ void fall(); // stop the time mesurement and save the value when signal falls
+ Timer timer; // timer to measure the up time of the signal and if the signal timed out
+ int time; // last measurement data
+
+ Ticker timeoutchecker; // Ticker to see if signal broke down
+ void timeoutcheck(); // to check for timeout, checked every second
};
#endif
\ No newline at end of file
--- a/Sensors/Acc/ADXL345.cpp Tue Oct 16 10:21:32 2012 +0000
+++ b/Sensors/Acc/ADXL345.cpp Wed Oct 17 08:37:08 2012 +0000
@@ -45,13 +45,19 @@
writeReg(ADXL345_POWER_CTL_REG, 0x08); // set mode
}
-void ADXL345::read(int a[3]){
+void ADXL345::read(){
char buffer[6];
readMultiReg(ADXL345_DATAX0_REG, buffer, 6);
- a[0] = (short) ((int)buffer[1] << 8 | (int)buffer[0]);
- a[1] = (short) ((int)buffer[3] << 8 | (int)buffer[2]);
- a[2] = (short) ((int)buffer[5] << 8 | (int)buffer[4]);
+ data[0] = (short) ((int)buffer[1] << 8 | (int)buffer[0]);
+ data[1] = (short) ((int)buffer[3] << 8 | (int)buffer[2]);
+ data[2] = (short) ((int)buffer[5] << 8 | (int)buffer[4]);
+
+ // calculate the angles for roll and pitch (0,1)
+ float R = sqrt(pow((float)data[0],2) + pow((float)data[1],2) + pow((float)data[2],2));
+ angle[0] = -(Rad2Deg * acos((float)data[1] / R)-90);
+ angle[1] = Rad2Deg * acos((float)data[0] / R)-90;
+ angle[2] = Rad2Deg * acos((float)data[2] / R);
}
int ADXL345::writeReg(char address, char data){
--- a/Sensors/Acc/ADXL345.h Tue Oct 16 10:21:32 2012 +0000
+++ b/Sensors/Acc/ADXL345.h Wed Oct 17 08:37:08 2012 +0000
@@ -64,14 +64,17 @@
#define ADXL345_Y 0x01
#define ADXL345_Z 0x02
+#define Rad2Deg 57.295779513082320876798154814105
+
typedef char byte;
class ADXL345
{
public:
ADXL345(PinName sda, PinName scl); // constructor, uses i2c
- void read(int a[3]); // read all axis to array
-
+ void read(); // read all axis to array
+ int data[3]; // where the measured data is saved
+ float angle[3]; // where the calculated angles are stored
private:
I2C i2c; // i2c object to communicate
--- a/Sensors/Gyro/L3G4200D.cpp Tue Oct 16 10:21:32 2012 +0000
+++ b/Sensors/Gyro/L3G4200D.cpp Wed Oct 17 08:37:08 2012 +0000
@@ -38,12 +38,11 @@
offset[j] = 0;
float Gyro_calib[3] = {0,0,0}; // temporary to sum up
- float Gyro_data[3];
for (int i = 0; i < count; i++) {
- read(Gyro_data);
+ read();
for (int j = 0; j < 3; j++)
- Gyro_calib[j] += Gyro_data[j];
+ Gyro_calib[j] += data[j];
wait(0.001); // maybe less or no wait !!
}
@@ -73,7 +72,7 @@
}
// Reads the 3 gyro channels and stores them in vector g
-void L3G4200D::read(float g[3])
+void L3G4200D::read()
{
byte buffer[6]; // 8-Bit pieces of axis data
// assert the MSB of the address to get the gyro
@@ -93,13 +92,13 @@
uint8_t zla = buffer[4];
uint8_t zha = buffer[5];
- g[0] = (short) (xha << 8 | xla);
- g[1] = (short) (yha << 8 | yla);
- g[2] = (short) (zha << 8 | zla);
+ data[0] = (short) (xha << 8 | xla);
+ data[1] = (short) (yha << 8 | yla);
+ data[2] = (short) (zha << 8 | zla);
//with offset of calibration
for (int j = 0; j < 3; j++)
- g[j] -= offset[j];
+ data[j] -= offset[j];
}
// Reads the gyros Temperature
--- a/Sensors/Gyro/L3G4200D.h Tue Oct 16 10:21:32 2012 +0000
+++ b/Sensors/Gyro/L3G4200D.h Wed Oct 17 08:37:08 2012 +0000
@@ -43,8 +43,9 @@
{
public:
L3G4200D(PinName sda, PinName scl); // constructor, uses i2c
- void read(float g[3]); // read all axis to array
+ void read(); // read all axis to array
int readTemp(); // read temperature from sensor
+ float data[3]; // where the measured data is saved
private:
float offset[3]; // offset that's subtracted from every measurement
--- a/main.cpp Tue Oct 16 10:21:32 2012 +0000
+++ b/main.cpp Wed Oct 17 08:37:08 2012 +0000
@@ -1,51 +1,51 @@
#include "mbed.h" // Standard Library
#include "LED.h" // LEDs framework for blinking ;)
+#include "PC.h" // Serial Port via USB for debugging in TeraTerm (driver needed: https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe)
#include "L3G4200D.h" // Gyro (Gyroscope)
#include "ADXL345.h" // Acc (Accelerometer)
#include "HMC5883.h" // Comp (Compass)
#include "BMP085.h" // Alt (Altitude sensor)
#include "RC_Channel.h" // RemoteControl Chnnels with PPM
#include "Servo.h" // Motor PPM
-#include "PC.h" // Serial Port via USB for debugging
+#include "PID.h" // PID Library from Aaron Berk
-#define PI 3.1415926535897932384626433832795
-#define Rad2Deg 57.295779513082320876798154814105
+#define PI 3.1415926535897932384626433832795
+#define Rad2Deg 57.295779513082320876798154814105
+#define RATE 0.02 // speed of Ticker/PID
Timer GlobalTimer; // global time to calculate processing speed
Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC
+PID controller(1.0, 0.0, 0.0, RATE); // test PID controller for throttle
+PID pid(1.0, 1.0, 0.0, RATE); // test PID controller for throttle
// initialisation of hardware
LED LEDs;
-PC pc(USBTX, USBRX, 57600); //Achtung: Treiber auf PC fuer Serial-mbed installieren. hier herunterladen https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe
+PC pc(USBTX, USBRX, 57600);
L3G4200D Gyro(p28, p27);
ADXL345 Acc(p28, p27);
HMC5883 Comp(p28, p27, GlobalTimer);
BMP085 Alt(p28, p27);
RC_Channel RC[] = {(p10), (p11), (p12), (p13)}; // noooo p19/p20!!!!
-//Servo Motor[] = {(p15), (p16), (p17), (p18)};
+Servo Motor[] = {(p15), (p16), (p17), (p18)};
// variables for loop
unsigned long dt = 0;
unsigned long time_loop = 0;
float angle[3] = {0,0,0}; // angle of calculated situation
-float Gyro_data[3];
-int Acc_data[3];
-float Acc_angle[2] = {0,0};
float Comp_angle = 0;
float tempangle = 0;
+int Motorvalue[3];
+
+float pidtester;
void get_Data()
{
// read data from sensors
- Gyro.read(Gyro_data);
- Acc.read(Acc_data);
+ Gyro.read();
+ Acc.read();
Comp.Update();
Alt.Update();
- // calculate the angles for roll and pitch from accelerometer
- Acc_angle[0] = Rad2Deg * atan2((float)Acc_data[1], (float)Acc_data[2]);
- Acc_angle[1] = 3 -Rad2Deg * atan2((float)Acc_data[0], (float)Acc_data[2]); // TODO Offset accelerometer einstellen
-
//calculate angle for yaw from compass
Comp_angle = Comp.getAngle(Comp.Mag[0], Comp.Mag[1]);
@@ -54,15 +54,20 @@
time_loop = GlobalTimer.read_us(); // set new time for next measurement
// calculate angles for roll, pitch an yaw
- angle[0] += (Acc_angle[0] - angle[0])/50 + Gyro_data[0] *dt/15000000.0;
- angle[1] += (Acc_angle[1] - angle[1])/50 + Gyro_data[1] *dt/15000000.0;
- tempangle += (Comp_angle - tempangle)/50 - Gyro_data[2] *dt/15000000.0;
- angle[2] -= Gyro_data[2] *dt/15000000.0; // gyro only here
+ angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt/15000000.0;
+ angle[1] += (Acc.angle[1]+3 - angle[1])/50 + Gyro.data[1] *dt/15000000.0;// TODO Offset accelerometer einstellen
+ tempangle += (Comp_angle - tempangle)/50 - Gyro.data[2] *dt/15000000.0;
+ angle[2] += Gyro.data[2] *dt/15000000.0; // gyro only here
// Read RC data
- //RC[0].read() // TODO: RC daten lesen und einberechnen!
-
- LEDs.rollnext();
+ controller.setProcessValue(RC[3 -1].read());
+ for (int j = 0; j < 4; j++)
+ Motorvalue[j] = controller.compute(); // throttle
+
+ for (int j = 0; j < 4; j++)
+ Motor[j] = 1000 + 5*abs(angle[1]);//Motorvalue[j]; // set new motorspeeds
+ pid.setProcessValue(angle[0]);
+ pidtester = pid.compute();
}
@@ -72,20 +77,33 @@
pc.printf("Flybed v0.2");
LEDs.roll(2);
+ controller.setInputLimits(1000.0, 2000.0);
+ controller.setOutputLimits(1000.0, 2000.0);
+ controller.setMode(AUTO_MODE);
+
+ pid.setInputLimits(-90.0, 90.0);
+ pid.setOutputLimits(-90.0, 90.0);
+ pid.setMode(AUTO_MODE);
+ pid.setSetPoint(0.0);
+
// Start!
GlobalTimer.start();
- Datagetter.attach(&get_Data, 0.02); // start to get data all 10ms
+ Datagetter.attach(&get_Data, RATE); // start to get data all 10ms
while(1) {
pc.locate(10,5); // PC output
pc.printf("dt:%dms %6.1fm ", dt/1000, Alt.CalcAltitude(Alt.Pressure));
pc.locate(10,8);
pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", angle[0], angle[1], angle[2]);
+ pc.locate(10,9);
+ pc.printf("ACC: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", Acc.angle[0], Acc.angle[1], Acc.angle[2]);
pc.locate(10,10);
pc.printf("Debug_Yaw: Comp:%6.1f tempangle:%6.1f ", Comp_angle, tempangle);
pc.locate(10,12);
pc.printf("Comp_Raw: %6.1f %6.1f %6.1f ", Comp.RawMag[0], Comp.RawMag[1], Comp.RawMag[2]);
pc.locate(10,13);
pc.printf("Comp_Mag: %6.1f %6.1f %6.1f ", Comp.Mag[0], Comp.Mag[1], Comp.Mag[2]);
- //Motor_left = 1000 + 5*abs(angle[1]);
+ pc.locate(10,15);
+ pc.printf("pidtester: %6.1f RC: %d %d %d %d ", pidtester, RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read());
+ LEDs.rollnext();
}
}
Matthias Grob
