Arnaud Suire
ajout module_mouvement
Dependencies: mbed xbee_lib ADXL345_I2C IMUfilter ITG3200 Motor RangeFinder Servo mbos PID
Fork of
Labo_TRSE_Drone
by 
HERBERT Nicolas
Revision 37:e1ad11fe3fe4, committed 2014-02-26
- Comitter:
- arnaudsuire
- Date:
- Wed Feb 26 08:46:22 2014 +0000
- Parent:
- 36:660be809a49d
- Child:
- 38:7ab036d94678
- Commit message:
- arnaud pid acc giro
Changed in this revision
--- a/Librairies/ADXL345.lib Wed Nov 20 10:47:38 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/elrafapadron/code/ADXL345/#baba3e2f977d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Librairies/ADXL345_I2C.lib Wed Feb 26 08:46:22 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/SED9008/code/ADXL345_I2C/#a973ef498a1d
--- a/Librairies/Camera_LS_Y201.lib Wed Nov 20 10:47:38 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/shintamainjp/code/Camera_LS_Y201/#43358d40f879
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Librairies/IMUfilter.lib Wed Feb 26 08:46:22 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/SED9008/code/IMUfilter/#4e7171d6f97e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Librairies/PID.lib Wed Feb 26 08:46:22 2014 +0000 @@ -0,0 +1,1 @@ +Librairies/PID#d58c1b8d63d9
--- a/Librairies/xbee_lib.lib Wed Nov 20 10:47:38 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/IngesupMbed01/code/xbee_lib/#945170b9c451
--- a/Module_Asservissement/Sous_Module_Mouvement/Module_Mouvement.cpp Wed Nov 20 10:47:38 2013 +0000
+++ b/Module_Asservissement/Sous_Module_Mouvement/Module_Mouvement.cpp Wed Feb 26 08:46:22 2014 +0000
@@ -155,6 +155,7 @@
break;
default :
+ break;
/*erreur pas de commande*/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Service/Acc_Giro.cpp Wed Feb 26 08:46:22 2014 +0000
@@ -0,0 +1,201 @@
+#include "Acc_Giro.h"
+
+//extern Serial pc;
+
+Acc_Giro::Acc_Giro()
+{
+ imuFilter = new IMUfilter(FILTER_RATE, 0.3);
+ accelerometer = new ADXL345_I2C(p9, p10);
+ //i2c = new I2C(p9, p10); // sda, scl
+ gyroscope = new ITG3200(p9, p10);
+
+
+ calibrate = 0;
+ calibrated = 0;
+ start = 0;
+ started = 0;
+ alg_enable = 0;
+
+ a_xAccumulator = 0;
+ a_yAccumulator = 0;
+ a_zAccumulator = 0;
+ w_xAccumulator = 0;
+ w_yAccumulator = 0;
+ w_zAccumulator = 0;
+
+ accelerometerSamples = 0;
+ gyroscopeSamples = 0;
+}
+
+void Acc_Giro::initializeAccelerometer(void) {
+
+ //Go into standby mode to configure the device.
+ accelerometer->setPowerControl(0x00);
+ //Full resolution, +/-16g, 4mg/LSB.
+ accelerometer->setDataFormatControl(0x0B);
+ //200Hz data rate.
+ accelerometer->setDataRate(ADXL345_200HZ);
+ //Measurement mode.
+ accelerometer->setPowerControl(0x08);
+ //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
+ wait_ms(22);
+
+}
+
+void Acc_Giro::sampleAccelerometer(void) {
+//pc.printf("S");
+ //Have we taken enough samples?
+ if (accelerometerSamples == SAMPLES) {
+
+ //Average the samples, remove the bias, and calculate the acceleration
+ //in m/s/s.
+ a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
+ a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
+ a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
+
+ a_xAccumulator = 0;
+ a_yAccumulator = 0;
+ a_zAccumulator = 0;
+ accelerometerSamples = 0;
+
+ } else {
+ //Take another sample.
+ accelerometer->getOutput(readings);
+
+ a_xAccumulator += (int16_t) readings[0];
+ a_yAccumulator += (int16_t) readings[1];
+ a_zAccumulator += (int16_t) readings[2];
+
+ accelerometerSamples++;
+ }
+}
+
+void Acc_Giro::initializeGyroscope(void) {
+
+ //Low pass filter bandwidth of 42Hz.
+ gyroscope->setLpBandwidth(LPFBW_42HZ);
+ //Internal sample rate of 200Hz. (1kHz / 5).
+ gyroscope->setSampleRateDivider(4);
+
+}
+
+void Acc_Giro::calibrateGyroscope(void) {
+
+ w_xAccumulator = 0;
+ w_yAccumulator = 0;
+ w_zAccumulator = 0;
+
+ //Take a number of readings and average them
+ //to calculate the gyroscope bias offset.
+ for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
+
+ w_xAccumulator += gyroscope->getGyroX();
+ w_yAccumulator += gyroscope->getGyroY();
+ w_zAccumulator += gyroscope->getGyroZ();
+ wait(GYRO_RATE);
+
+ }
+
+ //Average the samples.
+ w_xAccumulator /= CALIBRATION_SAMPLES;
+ w_yAccumulator /= CALIBRATION_SAMPLES;
+ w_zAccumulator /= CALIBRATION_SAMPLES;
+
+ w_xBias = w_xAccumulator;
+ w_yBias = w_yAccumulator;
+ w_zBias = w_zAccumulator;
+
+ w_xAccumulator = 0;
+ w_yAccumulator = 0;
+ w_zAccumulator = 0;
+
+}
+
+void Acc_Giro::calibrateAccelerometer(void) {
+
+ a_xAccumulator = 0;
+ a_yAccumulator = 0;
+ a_zAccumulator = 0;
+
+ //Take a number of readings and average them
+ //to calculate the zero g offset.
+ for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
+
+ accelerometer->getOutput(readings);
+
+ a_xAccumulator += (int16_t) readings[0];
+ a_yAccumulator += (int16_t) readings[1];
+ a_zAccumulator += (int16_t) readings[2];
+
+ wait(ACC_RATE);
+
+ }
+
+ a_xAccumulator /= CALIBRATION_SAMPLES;
+ a_yAccumulator /= CALIBRATION_SAMPLES;
+ a_zAccumulator /= CALIBRATION_SAMPLES;
+
+ //At 4mg/LSB, 250 LSBs is 1g.
+ a_xBias = a_xAccumulator;
+ a_yBias = a_yAccumulator;
+ a_zBias = (a_zAccumulator - 250);
+
+ a_xAccumulator = 0;
+ a_yAccumulator = 0;
+ a_zAccumulator = 0;
+}
+
+void Acc_Giro::sampleGyroscope(void) {
+
+ //Have we taken enough samples?
+ if (gyroscopeSamples == SAMPLES) {
+
+ //Average the samples, remove the bias, and calculate the angular
+ //velocity in rad/s.
+ w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
+ w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
+ w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
+
+ w_xAccumulator = 0;
+ w_yAccumulator = 0;
+ w_zAccumulator = 0;
+ gyroscopeSamples = 0;
+
+ } else {
+ //Take another sample.
+ w_xAccumulator += gyroscope->getGyroX();
+ w_yAccumulator += gyroscope->getGyroY();
+ w_zAccumulator += gyroscope->getGyroZ();
+
+ gyroscopeSamples++;
+
+ }
+
+}
+
+void Acc_Giro::filter(void) {
+ //Update the filter variables.
+ imuFilter->updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
+ //Calculate the new Euler angles.
+ imuFilter->computeEuler();
+ alg_enable = 1;
+}
+
+void Acc_Giro::GetI2CAddress()
+{
+ int count = 1;
+ for (int address=0; address<256; address+=2) {
+ if (!i2c->write(address, NULL, 0)) { // 0 returned is ok
+ char buffer [128];
+ sprintf (buffer, "%i: - %i\n",count, address);
+ // tcp_send(buffer);
+ count++;
+ }
+ }
+}
+
+void Acc_Giro::dataSender(void) {
+ char buffer [128];
+ //sprintf (buffer, "x:%f | y:%f | am1:%f | am3:%f\r\n",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),a_motor_1, a_motor_3);
+ //tcp_send(buffer);
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Service/Acc_Giro.h Wed Feb 26 08:46:22 2014 +0000
@@ -0,0 +1,126 @@
+#ifndef Acc_Giro_H
+#define Acc_Giro_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+#include "ADXL345_I2C.h"
+#include "ITG3200.h"
+#include "IMUfilter.h"
+
+/**
+ * Defines
+ */
+//Gravity at Earth's surface in m/s/s
+#define g0 9.812865328
+//Number of samples to average.
+#define SAMPLES 4
+//Number of samples to be averaged for a null bias calculation
+//during calibration.
+#define CALIBRATION_SAMPLES 128
+//Convert from radians to degrees.
+#define toDegrees(x) (x * 57.2957795)
+//Convert from degrees to radians.
+#define toRadians(x) (x * 0.01745329252)
+//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
+#define GYROSCOPE_GAIN (1 / 14.375)
+//Full scale resolution on the ADXL345 is 4mg/LSB.
+#define ACCELEROMETER_GAIN (0.004 * g0)
+//Sampling gyroscope at 200Hz.
+#define GYRO_RATE 0.002
+//Sampling accelerometer at 200Hz.
+#define ACC_RATE 0.002
+//Updating filter at 100Hz.
+#define FILTER_RATE 0.01
+#define CORRECTION_MAGNITUDE 50
+
+class Acc_Giro {
+
+public:
+
+ //Constructor.
+ Acc_Giro();
+ //destructeur
+ // ~Acc_Giro();
+
+ //Set up the ADXL345 appropriately.
+ void initializeAccelerometer(void);
+ //Calculate the null bias.
+ void calibrateAccelerometer(void);
+ //Take a set of samples and average them.
+ void sampleAccelerometer(void);
+ //Set up the ITG3200 appropriately.
+ void initializeGyroscope(void);
+ //Calculate the null bias.
+ void calibrateGyroscope(void);
+ //Take a set of samples and average them.
+ void sampleGyroscope(void);
+ //Update the filter and calculate the Euler angles.
+ void filter(void);
+ void GetI2CAddress();
+ void dataSender(void);
+
+ //membre classe
+ IMUfilter *imuFilter;
+ ADXL345_I2C *accelerometer;
+ I2C *i2c;
+ ITG3200 *gyroscope;
+ Ticker accelerometerTicker;
+ Ticker gyroscopeTicker;
+ Ticker filterTicker;
+ Ticker dataTicker;
+ Ticker algorithmTicker;
+
+
+
+ int calibrate;
+ int calibrated;
+ int start;
+ int started;
+ int alg_enable;
+
+ char data[128];
+ //Offsets for the gyroscope.
+ //The readings we take when the gyroscope is stationary won't be 0, so we'll
+ //average a set of readings we do get when the gyroscope is stationary and
+ //take those away from subsequent readings to ensure the gyroscope is offset
+ //or "biased" to 0.
+ double w_xBias;
+ double w_yBias;
+ double w_zBias;
+
+ //Offsets for the accelerometer.
+ //Same as with the gyroscope.
+ double a_xBias;
+ double a_yBias;
+ double a_zBias;
+
+ //Accumulators used for oversampling and then averaging.
+ volatile double a_xAccumulator;
+ volatile double a_yAccumulator;
+ volatile double a_zAccumulator;
+ volatile double w_xAccumulator;
+ volatile double w_yAccumulator;
+ volatile double w_zAccumulator;
+
+ //Accelerometer and gyroscope readings for x, y, z axes.
+ volatile double a_x;
+ volatile double a_y;
+ volatile double a_z;
+ volatile double w_x;
+ volatile double w_y;
+ volatile double w_z;
+
+ //Buffer for accelerometer readings.
+ int readings[3];
+ //Number of accelerometer samples we're on.
+ int accelerometerSamples;
+ //Number of gyroscope samples we're on.
+ int gyroscopeSamples;
+
+ };
+
+
+
+ #endif /* PID_H */
\ No newline at end of file
--- a/main.cpp Wed Nov 20 10:47:38 2013 +0000
+++ b/main.cpp Wed Feb 26 08:46:22 2014 +0000
@@ -17,25 +17,11 @@
* Output
*/
-/*#include "mbed.h"
-#include "mbos.h"
-
-DigitalOut myled(LED1);
-
-int main() {
- while(1) {
- myled = 1;
- wait(0.2);
- myled = 0;
- wait(0.2);
-
- //modification
- }
-}*/
#include "mbed.h"
#include "mbos.h"
#include "Module_Communication.h"
#include "Module_Mouvement.h"
+#include "Acc_Giro.h"
#define TASK1_ID 1 // Id for task 1 (idle task is 0)
#define TASK1_PRIO 50 // priority for task 1
@@ -54,23 +40,57 @@
DigitalOut led1(LED1);
DigitalOut led2(LED2);
mbos os(2, 1); // Instantiate mbos with 2 tasks & 1 timer
-ModuleMouvement test;
+Acc_Giro Acc_Giro_test;
+Serial pc(USBTX, USBRX);
int main(void)
{
-/*
- // Configure tasks and timers
- os.CreateTask(TASK1_ID, TASK1_PRIO, TASK1_STACK_SZ, task1);
- os.CreateTask(TASK2_ID, TASK2_PRIO, TASK2_STACK_SZ, task2);
- os.CreateTimer(TIMER0_ID, TIMER0_EVENT, TASK1_ID);
- // Start mbos
- os.Start();
- // never return!
- */
- test.TestMotor();
- C_ModuleCommunication com = C_ModuleCommunication();
- com.receptionDeTrame();
+ //Initialize inertial sensors.
+ Acc_Giro_test.initializeAccelerometer();
+ Acc_Giro_test.initializeGyroscope();
+
+ Timer tmr;
+ tmr.start();
+
+ pc.printf("test started");
+ Acc_Giro_test.calibrate=1;
+ while(true)
+ {
+ //Net::poll();
+ if(tmr.read() > 0.2){
+ // led4=!led4;
+ tmr.reset();
+ wait(0.5);
+ pc.printf("Ax:%f Ay:%f Az:%f || Gx:%f Gy:%f Gz:%f\n", Acc_Giro_test.a_x, Acc_Giro_test.a_y, Acc_Giro_test.a_z, toDegrees(Acc_Giro_test.imuFilter->getRoll()), toDegrees(Acc_Giro_test.imuFilter->getPitch()), toDegrees(Acc_Giro_test.imuFilter->getYaw()));
+
+ }
+
+ if(Acc_Giro_test.calibrate && !Acc_Giro_test.calibrated){
+ Acc_Giro_test.calibrateAccelerometer();
+ Acc_Giro_test.calibrateGyroscope();
+ led2 = 1;
+ Acc_Giro_test.calibrated = 1;
+ Acc_Giro_test.start = 1;
+ pc.printf("Calibrated\n");
+ }
+
+
+ if(Acc_Giro_test.calibrated && Acc_Giro_test.start && !Acc_Giro_test.started){
+
+ //Accelerometer data rate is 500Hz, so we'll sample at this speed.
+ Acc_Giro_test.accelerometerTicker.attach(&Acc_Giro_test, &Acc_Giro::sampleAccelerometer, 0.002);
+ //Gyroscope data rate is 500Hz, so we'll sample at this speed.
+ Acc_Giro_test.gyroscopeTicker.attach(&Acc_Giro_test, &Acc_Giro::sampleGyroscope, 0.002);
+ //Update the filter variables at the correct rate.
+ Acc_Giro_test.filterTicker.attach(&Acc_Giro_test, &Acc_Giro::filter, FILTER_RATE);
+ //Acc_Giro_test.dataTicker.attach(&Acc_Giro_test, &Acc_Giro::dataSender, 0.2);
+ // Acc_Giro_test.algorithmTicker.attach(&algorithm, 0.001);
+ Acc_Giro_test.started = 1;
+ }
+
+
+ }
}
void task1(void)
--- a/mbed.bld Wed Nov 20 10:47:38 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/0954ebd79f59 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.lib Wed Feb 26 08:46:22 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/jalp89/code/mbed/#7ec3cb6bbcc4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/xbee_lib.lib Wed Feb 26 08:46:22 2014 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/IngesupMbed01/code/xbee_lib/#945170b9c451