jetfishteam
My modifications/additions to the code
Dependencies: ADXL345 ADXL345_I2C IMUfilter ITG3200 Servo fishgait mbed-rtos mbed pixy_cam
Fork of
robotic_fish_ver_4_8
by 
jetfishteam
Revision 0:ff9bc5f69c57, committed 2014-01-21
- Comitter:
- sandwich
- Date:
- Tue Jan 21 21:32:05 2014 +0000
- Child:
- 1:80e0af42f876
- Child:
- 2:430c068cf570
- Commit message:
- pololu motor controller works with sine waveform input. IMU is untested
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ADXL345.lib Tue Jan 21 21:32:05 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/ADXL345/#bd8f0f20f433
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ADXL345_I2C.lib Tue Jan 21 21:32:05 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Reiko/code/ADXL345_I2C/#753991a08b2a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.cpp Tue Jan 21 21:32:05 2014 +0000
@@ -0,0 +1,235 @@
+/**
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope using
+ * orientation filter developed by Sebastian Madgwick.
+ *
+ * Find more details about his paper here:
+ *
+ * http://code.google.com/p/imumargalgorithm30042010sohm/
+ */
+
+
+/**
+ * Includes
+ */
+#include "IMU.h"
+
+IMU::IMU(float imuRate,
+ double gyroscopeMeasurementError,
+ float accelerometerRate,
+ float gyroscopeRate) : accelerometer(p9, p10),
+ gyroscope(p9, p10), imuFilter(imuRate, gyroscopeMeasurementError) {
+
+ imuRate_ = imuRate;
+ accelerometerRate_ = accelerometerRate;
+ gyroscopeRate_ = gyroscopeRate;
+
+ a_xAccumulator = 0;
+ a_yAccumulator = 0;
+ a_zAccumulator = 0;
+ w_xAccumulator = 0;
+ w_yAccumulator = 0;
+ w_zAccumulator = 0;
+
+ accelerometerSamples = 0;
+ gyroscopeSamples = 0;
+
+ initializeAccelerometer();
+ calibrateAccelerometer();
+
+ initializeGyroscope();
+ calibrateGyroscope();
+
+ accelerometerTicker.attach(this, &IMU::sampleAccelerometer, accelerometerRate_);
+ gyroscopeTicker.attach(this, &IMU::sampleGyroscope, gyroscopeRate_);
+ filterTicker.attach(this, &IMU::filter, imuRate_);
+
+}
+
+double IMU::getRoll(void) {
+
+ return toDegrees(imuFilter.getRoll());
+
+}
+
+double IMU::getPitch(void) {
+
+ return toDegrees(imuFilter.getPitch());
+
+}
+
+double IMU::getYaw(void) {
+
+ return toDegrees(imuFilter.getYaw());
+
+}
+
+void IMU::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 IMU::sampleAccelerometer(void) {
+
+ if (accelerometerSamples == SAMPLES) {
+
+ 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 {
+
+ accelerometer.getOutput(readings);
+
+ a_xAccumulator += (int16_t) readings[0];
+ a_yAccumulator += (int16_t) readings[1];
+ a_zAccumulator += (int16_t) readings[2];
+
+ accelerometerSamples++;
+
+ }
+
+}
+
+void IMU::calibrateAccelerometer(void) {
+
+ a_xAccumulator = 0;
+ a_yAccumulator = 0;
+ a_zAccumulator = 0;
+
+ 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(accelerometerRate_);
+
+ }
+
+ a_xAccumulator /= CALIBRATION_SAMPLES;
+ a_yAccumulator /= CALIBRATION_SAMPLES;
+ a_zAccumulator /= CALIBRATION_SAMPLES;
+
+ a_xBias = a_xAccumulator;
+ a_yBias = a_yAccumulator;
+ a_zBias = (a_zAccumulator - 250);
+
+ a_xAccumulator = 0;
+ a_yAccumulator = 0;
+ a_zAccumulator = 0;
+
+}
+
+void IMU::initializeGyroscope(void) {
+
+ //Low pass filter bandwidth of 42Hz.
+ gyroscope.setLpBandwidth(LPFBW_42HZ);
+ //Internal sample rate of 200Hz.
+ gyroscope.setSampleRateDivider(4);
+
+}
+
+void IMU::calibrateGyroscope(void) {
+
+ w_xAccumulator = 0;
+ w_yAccumulator = 0;
+ w_zAccumulator = 0;
+
+ for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
+
+ w_xAccumulator += gyroscope.getGyroX();
+ w_yAccumulator += gyroscope.getGyroY();
+ w_zAccumulator += gyroscope.getGyroZ();
+ wait(gyroscopeRate_);
+
+ }
+
+ //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 IMU::sampleGyroscope(void) {
+
+ if (gyroscopeSamples == SAMPLES) {
+
+ 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 {
+
+ w_xAccumulator += gyroscope.getGyroX();
+ w_yAccumulator += gyroscope.getGyroY();
+ w_zAccumulator += gyroscope.getGyroZ();
+
+ gyroscopeSamples++;
+
+ }
+
+}
+
+void IMU::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();
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.h Tue Jan 21 21:32:05 2014 +0000
@@ -0,0 +1,197 @@
+/**
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope using
+ * orientation filter developed by Sebastian Madgwick.
+ *
+ * Find more details about his paper here:
+ *
+ * http://code.google.com/p/imumargalgorithm30042010sohm/
+ */
+
+#ifndef MBED_IMU_H
+#define MBED_IMU_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+#include "ADXL345_I2C.h"
+#include "ITG3200.h"
+#include "IMUfilter.h"
+
+/**
+ * Defines
+ */
+#define IMU_RATE 0.025
+#define ACCELEROMETER_RATE 0.005
+#define GYROSCOPE_RATE 0.005
+#define GYRO_MEAS_ERROR 0.3 //IMUfilter tuning parameter.
+
+//Gravity at Earth's surface in m/s/s
+#define g0 9.812865328
+//Number of samples to average
+#define SAMPLES 4
+#define CALIBRATION_SAMPLES 128
+//Multiply radians to get degrees.
+#define toDegrees(x) (x * 57.2957795)
+//Multiply degrees to get radians.
+#define toRadians(x) (x * 0.01745329252)
+//Full scale resolution on the ADXL345 is 4mg/LSB.
+//Multiply ADC count readings from ADXL345 to get acceleration in m/s/s.
+#define toAcceleration(x) (x * (4 * g0 * 0.001))
+//14.375 LSB/(degrees/sec)
+#define GYROSCOPE_GAIN (1 / 14.375)
+#define ACCELEROMETER_GAIN (0.004 * g0)
+
+/**
+ * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope to calculate
+ * roll, pitch and yaw angles.
+ */
+class IMU {
+
+public:
+
+ /**
+ * Constructor.
+ *
+ * @param imuRate Rate which IMUfilter update and Euler angle calculation
+ * occurs.
+ * @param gyroscopeMeasurementError IMUfilter tuning parameter.
+ * @param accelerometerRate Rate at which accelerometer data is sampled.
+ * @param gyroscopeRate Rate at which gyroscope data is sampled.
+ */
+ IMU(float imuRate,
+ double gyroscopeMeasurementError,
+ float accelerometerRate,
+ float gyroscopeRate);
+
+ /**
+ * Get the current roll angle.
+ *
+ * @return The current roll angle in degrees.
+ */
+ double getRoll(void);
+
+ /**
+ * Get the current pitch angle.
+ *
+ * @return The current pitch angle in degrees.
+ */
+ double getPitch(void);
+
+ /**
+ * Get the current yaw angle.
+ *
+ * @return The current yaw angle in degrees.
+ */
+ double getYaw(void);
+
+private:
+
+ /**
+ * Set up the ADXL345 appropriately.
+ */
+ void initializeAccelerometer(void);
+
+ /**
+ * Calculate the zero g offset.
+ */
+ void calibrateAccelerometer(void);
+
+ /**
+ * Take a set of samples and average them.
+ */
+ void sampleAccelerometer(void);
+
+ /**
+ * Set up the ITG-3200 appropriately.
+ */
+ void initializeGyroscope(void);
+
+ /**
+ * Calculate the bias offset.
+ */
+ 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);
+
+ //ADXL345 accelerometer;
+ ADXL345_I2C accelerometer;
+ ITG3200 gyroscope;
+ IMUfilter imuFilter;
+
+ Ticker accelerometerTicker;
+ Ticker gyroscopeTicker;
+ Ticker filterTicker;
+
+ float accelerometerRate_;
+ float gyroscopeRate_;
+ float imuRate_;
+
+ //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;
+
+ double a_xBias;
+ double a_yBias;
+ double a_zBias;
+
+ 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];
+ int accelerometerSamples;
+ int gyroscopeSamples;
+
+};
+
+#endif /* MBED_IMU_H */
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMUfilter.lib Tue Jan 21 21:32:05 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/IMUfilter/#8a920397b510
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ITG3200.lib Tue Jan 21 21:32:05 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/ITG3200/#b098d99dd81e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Jan 21 21:32:05 2014 +0000
@@ -0,0 +1,23 @@
+#include "mbed.h"
+#include "motor_controller.h"
+#include "IMU.h"
+
+PololuMController mcon(p22, p6, p5);
+IMU imu(0.1, 0.3, 0.005, 0.005);
+
+int main() {
+ Timer t;
+ t.start();
+ while(1) {
+ /* if (speed>=1)
+ {
+ speed=0;
+ mcon.reverse();
+ }
+ mcon.setspeed(speed);
+ */
+ mcon.drive_sinusoidal(t.read(), 1, 0.25*3.14, 0);
+ wait(0.2);
+ }
+ t.stop();
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Jan 21 21:32:05 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/motor_controller.cpp Tue Jan 21 21:32:05 2014 +0000
@@ -0,0 +1,54 @@
+#include "motor_controller.h"
+
+PololuMController::PololuMController(PinName pwmport, PinName A, PinName B)
+{
+ pwm=new PwmOut(pwmport);
+ outA=new DigitalOut(A);
+ outB=new DigitalOut(B);
+ outA->write(0);
+ outB->write(1);
+ timestamp=0;
+}
+
+PololuMController::~PololuMController()
+{
+ delete pwm;
+ delete outA;
+ delete outB;
+}
+
+void PololuMController::setspeed(float speed)
+{
+ pwm->write(speed);
+ return;
+}
+
+void PololuMController::setpolarspeed(float speed)
+{
+ if (speed>=0)
+ {
+ outA->write(0);
+ outB->write(1);
+ pwm->write(abs(speed));
+ }
+ else
+ {
+ outA->write(1);
+ outB->write(0);
+ pwm->write(abs(speed));
+ }
+ return;
+}
+
+void PololuMController::reverse()
+{
+ outA->write(!(outA->read()));
+ outB->write(!(outB->read()));
+ return;
+}
+
+void PololuMController::drive_sinusoidal(float cur_time, float a, float w, float phi)
+{
+ setpolarspeed(a*sin(w*cur_time+phi));
+ return;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/motor_controller.h Tue Jan 21 21:32:05 2014 +0000
@@ -0,0 +1,19 @@
+#pragma once
+#include "mbed.h"
+
+class PololuMController
+{
+ private:
+ PwmOut* pwm;
+ DigitalOut* outA;
+ DigitalOut* outB;
+ float timestamp;
+ public:
+ PololuMController();
+ PololuMController(PinName pwmport, PinName A, PinName B);
+ ~PololuMController();
+ void setspeed(float speed); //0 to 1
+ void setpolarspeed(float speed); //-1 to 1
+ void reverse(); //only works on non-polar speed
+ void drive_sinusoidal(float cur_time, float a, float w, float phi); //a*sin(w*cur_time+phi)
+};
\ No newline at end of file