firmware using madgwick filter
EmaMaker
parent
db359fd230
commit
d23cc7d750
|
|
@ -0,0 +1,249 @@
|
|||
//=============================================================================================
|
||||
// Madgwick.c
|
||||
//=============================================================================================
|
||||
//
|
||||
// Implementation of Madgwick's IMU and AHRS algorithms.
|
||||
// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
|
||||
//
|
||||
// From the x-io website "Open-source resources available on this website are
|
||||
// provided under the GNU General Public Licence unless an alternative licence
|
||||
// is provided in source."
|
||||
//
|
||||
// Date Author Notes
|
||||
// 29/09/2011 SOH Madgwick Initial release
|
||||
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
|
||||
// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised
|
||||
//
|
||||
//=============================================================================================
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Header files
|
||||
|
||||
#include "Madgwick.h"
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <Arduino.h>
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Definitions
|
||||
|
||||
#define sampleFreqDef 512.0f // sample frequency in Hz
|
||||
|
||||
|
||||
//============================================================================================
|
||||
// Functions
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// AHRS algorithm update
|
||||
|
||||
Madgwick::Madgwick() {
|
||||
q0 = 1.0f;
|
||||
q1 = 0.0f;
|
||||
q2 = 0.0f;
|
||||
q3 = 0.0f;
|
||||
now = micros();
|
||||
}
|
||||
|
||||
void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
|
||||
float recipNorm;
|
||||
float s0, s1, s2, s3;
|
||||
float qDot1, qDot2, qDot3, qDot4;
|
||||
float hx, hy;
|
||||
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
|
||||
|
||||
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
|
||||
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
|
||||
updateIMU(gx, gy, gz, ax, ay, az);
|
||||
return;
|
||||
}
|
||||
|
||||
now = micros();
|
||||
// Set integration time by time elapsed since last filter update
|
||||
delta_t = ((now - last_update) / (float)1000000.0f);
|
||||
last_update = now;
|
||||
|
||||
// Convert gyroscope degrees/sec to radians/sec
|
||||
gx *= (float)0.0174533f;
|
||||
gy *= (float)0.0174533f;
|
||||
gz *= (float)0.0174533f;
|
||||
|
||||
// Rate of change of quaternion from gyroscope
|
||||
qDot1 = (float)0.5f * (-q1 * gx - q2 * gy - q3 * gz);
|
||||
qDot2 = (float)0.5f * (q0 * gx + q2 * gz - q3 * gy);
|
||||
qDot3 = (float)0.5f * (q0 * gy - q1 * gz + q3 * gx);
|
||||
qDot4 = (float)0.5f * (q0 * gz + q1 * gy - q2 * gx);
|
||||
|
||||
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
|
||||
if(!((ax == (float)0.0f) && (ay == (float)0.0f) && (az == (float)0.0f))) {
|
||||
|
||||
// Normalise accelerometer measurement
|
||||
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
|
||||
ax *= recipNorm;
|
||||
ay *= recipNorm;
|
||||
az *= recipNorm;
|
||||
|
||||
// Normalise magnetometer measurement
|
||||
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
|
||||
mx *= recipNorm;
|
||||
my *= recipNorm;
|
||||
mz *= recipNorm;
|
||||
|
||||
// Auxiliary variables to avoid repeated arithmetic
|
||||
_2q0mx = (float)2.0f * q0 * mx;
|
||||
_2q0my = (float)2.0f * q0 * my;
|
||||
_2q0mz = (float)2.0f * q0 * mz;
|
||||
_2q1mx = (float)2.0f * q1 * mx;
|
||||
_2q0 = (float)2.0f * q0;
|
||||
_2q1 = (float)2.0f * q1;
|
||||
_2q2 = (float)2.0f * q2;
|
||||
_2q3 = (float)2.0f * q3;
|
||||
_2q0q2 = (float)2.0f * q0 * q2;
|
||||
_2q2q3 = (float)2.0f * q2 * q3;
|
||||
q0q0 = q0 * q0;
|
||||
q0q1 = q0 * q1;
|
||||
q0q2 = q0 * q2;
|
||||
q0q3 = q0 * q3;
|
||||
q1q1 = q1 * q1;
|
||||
q1q2 = q1 * q2;
|
||||
q1q3 = q1 * q3;
|
||||
q2q2 = q2 * q2;
|
||||
q2q3 = q2 * q3;
|
||||
q3q3 = q3 * q3;
|
||||
|
||||
// Reference direction of Earth's magnetic field
|
||||
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
|
||||
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
|
||||
_2bx = sqrtf(hx * hx + hy * hy);
|
||||
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
|
||||
_4bx = (float)2.0f * _2bx;
|
||||
_4bz = (float)2.0f * _2bz;
|
||||
|
||||
// Gradient decent algorithm corrective step
|
||||
s0 = -_2q2 * ((float)2.0f * q1q3 - _2q0q2 - ax) + _2q1 * ((float)2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * ((float)0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * ((float)0.5f - q1q1 - q2q2) - mz);
|
||||
s1 = _2q3 * ((float)2.0f * q1q3 - _2q0q2 - ax) + _2q0 * ((float)2.0f * q0q1 + _2q2q3 - ay) - (float)4.0f * q1 * (1 - (float)2.0f * q1q1 - (float)2.0f * q2q2 - az) + _2bz * q3 * (_2bx * ((float)0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * ((float)0.5f - q1q1 - q2q2) - mz);
|
||||
s2 = -_2q0 * ((float)2.0f * q1q3 - _2q0q2 - ax) + _2q3 * ((float)2.0f * q0q1 + _2q2q3 - ay) - (float)4.0f * q2 * (1 - (float)2.0f * q1q1 - (float)2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * ((float)0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * ((float)0.5f - q1q1 - q2q2) - mz);
|
||||
s3 = _2q1 * ((float)2.0f * q1q3 - _2q0q2 - ax) + _2q2 * ((float)2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * ((float)0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * ((float)0.5f - q1q1 - q2q2) - mz);
|
||||
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
|
||||
s0 *= recipNorm;
|
||||
s1 *= recipNorm;
|
||||
s2 *= recipNorm;
|
||||
s3 *= recipNorm;
|
||||
|
||||
// Apply feedback step
|
||||
qDot1 -= beta * s0;
|
||||
qDot2 -= beta * s1;
|
||||
qDot3 -= beta * s2;
|
||||
qDot4 -= beta * s3;
|
||||
}
|
||||
|
||||
// Integrate rate of change of quaternion to yield quaternion
|
||||
q0 += qDot1 * delta_t;
|
||||
q1 += qDot2 * delta_t;
|
||||
q2 += qDot3 * delta_t;
|
||||
q3 += qDot4 * delta_t;
|
||||
|
||||
// Normalise quaternion
|
||||
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 *= recipNorm;
|
||||
q1 *= recipNorm;
|
||||
q2 *= recipNorm;
|
||||
q3 *= recipNorm;
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// IMU algorithm update
|
||||
|
||||
void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
|
||||
float recipNorm;
|
||||
float s0, s1, s2, s3;
|
||||
float qDot1, qDot2, qDot3, qDot4;
|
||||
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
|
||||
|
||||
// Convert gyroscope degrees/sec to radians/sec
|
||||
gx *= (float)0.0174533f;
|
||||
gy *= (float)0.0174533f;
|
||||
gz *= (float)0.0174533f;
|
||||
|
||||
now = micros();
|
||||
// Set integration time by time elapsed since last filter update
|
||||
delta_t = ((now - last_update) / (float)1000000.0f);
|
||||
last_update = now;
|
||||
|
||||
// Rate of change of quaternion from gyroscope
|
||||
qDot1 = (float)0.5f * (-q1 * gx - q2 * gy - q3 * gz);
|
||||
qDot2 = (float)0.5f * (q0 * gx + q2 * gz - q3 * gy);
|
||||
qDot3 = (float)0.5f * (q0 * gy - q1 * gz + q3 * gx);
|
||||
qDot4 = (float)0.5f * (q0 * gz + q1 * gy - q2 * gx);
|
||||
|
||||
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
|
||||
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
|
||||
|
||||
// Normalise accelerometer measurement
|
||||
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
|
||||
ax *= recipNorm;
|
||||
ay *= recipNorm;
|
||||
az *= recipNorm;
|
||||
|
||||
// Auxiliary variables to avoid repeated arithmetic
|
||||
_2q0 = (float)2.0f * q0;
|
||||
_2q1 = (float)2.0f * q1;
|
||||
_2q2 = (float)2.0f * q2;
|
||||
_2q3 = (float)2.0f * q3;
|
||||
_4q0 = (float)4.0f * q0;
|
||||
_4q1 = (float)4.0f * q1;
|
||||
_4q2 = (float)4.0f * q2;
|
||||
_8q1 = (float)8.0f * q1;
|
||||
_8q2 = (float)8.0f * q2;
|
||||
q0q0 = q0 * q0;
|
||||
q1q1 = q1 * q1;
|
||||
q2q2 = q2 * q2;
|
||||
q3q3 = q3 * q3;
|
||||
|
||||
// Gradient decent algorithm corrective step
|
||||
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
|
||||
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
|
||||
s2 = (float)4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
|
||||
s3 = (float)4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
|
||||
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
|
||||
s0 *= recipNorm;
|
||||
s1 *= recipNorm;
|
||||
s2 *= recipNorm;
|
||||
s3 *= recipNorm;
|
||||
|
||||
// Apply feedback step
|
||||
qDot1 -= beta * s0;
|
||||
qDot2 -= beta * s1;
|
||||
qDot3 -= beta * s2;
|
||||
qDot4 -= beta * s3;
|
||||
}
|
||||
|
||||
// Integrate rate of change of quaternion to yield quaternion
|
||||
q0 += qDot1 * delta_t;
|
||||
q1 += qDot2 * delta_t;
|
||||
q2 += qDot3 * delta_t;
|
||||
q3 += qDot4 * delta_t;
|
||||
|
||||
// Normalise quaternion
|
||||
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 *= recipNorm;
|
||||
q1 *= recipNorm;
|
||||
q2 *= recipNorm;
|
||||
q3 *= recipNorm;
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Fast inverse square-root
|
||||
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
|
||||
|
||||
float Madgwick::invSqrt(float x) {
|
||||
float halfx = 0.5f * x;
|
||||
float y = x;
|
||||
long i = *(long*)&y;
|
||||
i = 0x5f3759df - (i>>1);
|
||||
y = *(float*)&i;
|
||||
y = y * (1.5f - (halfx * y * y));
|
||||
y = y * (1.5f - (halfx * y * y));
|
||||
return y;
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
//=============================================================================================
|
||||
// Madgwick.h
|
||||
//=============================================================================================
|
||||
//
|
||||
// Implementation of Madgwick's IMU and AHRS algorithms.
|
||||
// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
|
||||
//
|
||||
// From the x-io website "Open-source resources available on this website are
|
||||
// provided under the GNU General Public Licence unless an alternative licence
|
||||
// is provided in source."
|
||||
//
|
||||
// Date Author Notes
|
||||
// 29/09/2011 SOH Madgwick Initial release
|
||||
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
|
||||
//
|
||||
//=============================================================================================
|
||||
#ifndef __Madgwick_h__
|
||||
#define __Madgwick_h__
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
//--------------------------------------------------------------------------------------------
|
||||
// Variable declaration
|
||||
class Madgwick {
|
||||
private:
|
||||
float delta_t = 0; // Used to control display output rate
|
||||
uint32_t now = 0; // used to calculate integration interval
|
||||
uint32_t last_update = 0; // used to calculate integration interval
|
||||
static float invSqrt(float x);
|
||||
float beta; // algorithm gain
|
||||
float q0;
|
||||
float q1;
|
||||
float q2;
|
||||
float q3; // quaternion of sensor frame relative to auxiliary frame
|
||||
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Function declarations
|
||||
public:
|
||||
Madgwick(void);
|
||||
void changeBeta(float newBeta) { beta = newBeta; }
|
||||
void begin(float confBeta) { beta = confBeta; }
|
||||
void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
|
||||
void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
|
||||
|
||||
float getQuatW() {
|
||||
return q0;
|
||||
}
|
||||
|
||||
float getQuatX() {
|
||||
return q1;
|
||||
}
|
||||
|
||||
float getQuatY() {
|
||||
return q2;
|
||||
}
|
||||
|
||||
float getQuatZ() {
|
||||
return q3;
|
||||
}
|
||||
|
||||
};
|
||||
#endif
|
||||
|
|
@ -0,0 +1,132 @@
|
|||
#include "FastIMU.h"
|
||||
#include "Madgwick.h"
|
||||
#include <Wire.h>
|
||||
|
||||
//#define PRINT_ANGLES
|
||||
//#define PRINT_QUAT
|
||||
//#define PERFORM_CALIBRATION //Comment to disable startup calibration
|
||||
|
||||
#define IMU_ADDRESS 0x68 //Change to the address of the IMU
|
||||
MPU6050 IMU; //Change to the name of any supported IMU!
|
||||
|
||||
AccelData IMUAccel; //Sensor data
|
||||
GyroData IMUGyro;
|
||||
Madgwick filter;
|
||||
|
||||
void setup_imu(){
|
||||
Wire.begin();
|
||||
Wire.setClock(400000); //400khz clock
|
||||
|
||||
calData calib;
|
||||
calib.accelBias[0]=-0.02;
|
||||
calib.accelBias[1]=-0.01;
|
||||
calib.accelBias[2]=0.16;
|
||||
calib.gyroBias[0] = -2.89;
|
||||
calib.gyroBias[1] = -3.59;
|
||||
calib.gyroBias[2] = -0.66;
|
||||
|
||||
int err = IMU.init(calib, IMU_ADDRESS);
|
||||
if (err != 0) {
|
||||
Serial.print("Error initializing IMU: ");
|
||||
Serial.println(err);
|
||||
while (true) {
|
||||
;
|
||||
}
|
||||
}
|
||||
|
||||
if (err != 0) {
|
||||
Serial.print("Error Setting range: ");
|
||||
Serial.println(err);
|
||||
while (true) {
|
||||
;
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef PERFORM_CALIBRATION
|
||||
Serial.println("Keep IMU level.");
|
||||
delay(5000);
|
||||
IMU.calibrateAccelGyro(&calib);
|
||||
Serial.println("Calibration done!");
|
||||
Serial.println("Accel biases X/Y/Z: ");
|
||||
Serial.print(calib.accelBias[0]);
|
||||
Serial.print(", ");
|
||||
Serial.print(calib.accelBias[1]);
|
||||
Serial.print(", ");
|
||||
Serial.println(calib.accelBias[2]);
|
||||
Serial.println("Gyro biases X/Y/Z: ");
|
||||
Serial.print(calib.gyroBias[0]);
|
||||
Serial.print(", ");
|
||||
Serial.print(calib.gyroBias[1]);
|
||||
Serial.print(", ");
|
||||
Serial.println(calib.gyroBias[2]);
|
||||
delay(5000);
|
||||
|
||||
IMU.init(calib, IMU_ADDRESS);
|
||||
#endif
|
||||
filter.begin(0.2f);
|
||||
Serial.println("IMU up");
|
||||
}
|
||||
|
||||
|
||||
void update_imu(){
|
||||
IMU.update();
|
||||
IMU.getAccel(&IMUAccel);
|
||||
IMU.getGyro(&IMUGyro);
|
||||
// MPU6050 has no magnetometer
|
||||
filter.updateIMU(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ);
|
||||
|
||||
float qw = filter.getQuatW();
|
||||
float qx = filter.getQuatX();
|
||||
float qy = filter.getQuatY();
|
||||
float qz = filter.getQuatZ();
|
||||
|
||||
double heading{0}, bank{0}, attitude{0};
|
||||
|
||||
double sqw = qw*qw;
|
||||
double sqx = qx*qx;
|
||||
double sqy = qy*qy;
|
||||
double sqz = qz*qz;
|
||||
double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
|
||||
double test = qx*qy + qz*qw;
|
||||
if (test > 0.499*unit) { // singularity at north pole
|
||||
heading = 2 * atan2(qx,qw);
|
||||
attitude = M_PI/2;
|
||||
bank = 0;
|
||||
return;
|
||||
}
|
||||
if (test < -0.499*unit) { // singularity at south pole
|
||||
heading = -2 * atan2(qx,qw);
|
||||
attitude = M_PI/2;
|
||||
bank = 0;
|
||||
return;
|
||||
}
|
||||
heading = atan2(2*qy*qw-2*qx*qz , sqx - sqy - sqz + sqw);
|
||||
attitude = asin(2*test/unit);
|
||||
bank = atan2(2*qx*qw-2*qy*qz , -sqx + sqy - sqz + sqw);
|
||||
|
||||
pitch = heading;
|
||||
roll = bank;
|
||||
yaw = attitude;
|
||||
|
||||
|
||||
#ifdef PRINT_ANGLES
|
||||
Serial.print("Roll: ");
|
||||
Serial.print(roll, 5);
|
||||
Serial.print("\tPitch: ");
|
||||
Serial.print(pitch, 5);
|
||||
Serial.print("\tYaw: ");
|
||||
Serial.println(yaw, 5);
|
||||
#endif // PRINT_ANGLES
|
||||
|
||||
#ifdef PRINT_QUAT
|
||||
Serial.print("QW: ");
|
||||
Serial.print(qw, 5);
|
||||
Serial.print("\tQX: ");
|
||||
Serial.print(qx, 5);
|
||||
Serial.print("\tQY: ");
|
||||
Serial.print(qy, 5);
|
||||
Serial.print("\tQZ: ");
|
||||
Serial.println(qz, 5);
|
||||
#endif // PRINT_QUAT
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,133 @@
|
|||
|
||||
int mot_pins[][3] = {
|
||||
{17, 16, 20},
|
||||
{19, 18, 21}
|
||||
};
|
||||
|
||||
|
||||
// PWM value at which the motors start moving (empirical)
|
||||
constexpr int base_pwm = 100 ; // Actually starts moving at 70
|
||||
// Max PWM value
|
||||
constexpr int max_pwm = 255;
|
||||
|
||||
void setup_motors(){
|
||||
pinMode(mot_pins[0][0], OUTPUT);
|
||||
pinMode(mot_pins[0][1], OUTPUT);
|
||||
pinMode(mot_pins[1][0], OUTPUT);
|
||||
pinMode(mot_pins[1][1], OUTPUT);
|
||||
|
||||
Serial.println("Motors up");
|
||||
}
|
||||
|
||||
double torque_to_pwm_sx(double t){
|
||||
return 547.323 * t + 50.874;
|
||||
}
|
||||
|
||||
double torque_to_pwm_dx(double t){
|
||||
return 684.284 * t + 44.29;
|
||||
}
|
||||
|
||||
double speed_to_pwm(double speed){
|
||||
return map(speed, 0, 100, 0, 255);
|
||||
}
|
||||
|
||||
int sign(auto b){
|
||||
return b == 0 ? 0 : b > 0 ? 1 : -1;
|
||||
}
|
||||
|
||||
void move(int mot, double speed){
|
||||
double pwm = speed_to_pwm(speed);
|
||||
move_pwm(mot, pwm*sign(speed));
|
||||
}
|
||||
|
||||
void move_pwm(int mot, int pwm){
|
||||
if(pwm == 0){
|
||||
digitalWrite(mot_pins[mot][0], LOW);
|
||||
digitalWrite(mot_pins[mot][1], LOW);
|
||||
}else if(pwm < 0){
|
||||
digitalWrite(mot_pins[mot][0], HIGH);
|
||||
digitalWrite(mot_pins[mot][1], LOW);
|
||||
}else{
|
||||
digitalWrite(mot_pins[mot][1], HIGH);
|
||||
digitalWrite(mot_pins[mot][0], LOW);
|
||||
}
|
||||
|
||||
analogWrite(mot_pins[mot][2], abs(pwm));
|
||||
}
|
||||
|
||||
void test_motors(){
|
||||
while(!Serial) delay(10);
|
||||
|
||||
int m = 0;
|
||||
while(true){
|
||||
digitalWrite(mot_pins[m][0], LOW);
|
||||
digitalWrite(mot_pins[m][1], HIGH);
|
||||
|
||||
for(int i = 0; i < 255; i++){
|
||||
Serial.print("PWM: ");
|
||||
Serial.println(i);
|
||||
analogWrite(mot_pins[m][2], i);
|
||||
delay(100);
|
||||
}
|
||||
delay(1000);
|
||||
digitalWrite(mot_pins[m][0], LOW);
|
||||
digitalWrite(mot_pins[m][1], LOW);
|
||||
m = 1-m;
|
||||
}
|
||||
}
|
||||
|
||||
void test_motors_nodeadzone(){
|
||||
int m = 0;
|
||||
while(true){
|
||||
for(int i = 0; i < 100; i++){
|
||||
Serial.print("Speed: ");
|
||||
Serial.println(i);
|
||||
move(m, i);
|
||||
delay(100);
|
||||
}
|
||||
delay(1000);
|
||||
move(m, 0);
|
||||
m = 1-m;
|
||||
}
|
||||
}
|
||||
|
||||
void test_motors_rpm(){
|
||||
while(!Serial) delay(10);
|
||||
|
||||
unsigned long tstart = millis();
|
||||
while(millis() - tstart < 200) {
|
||||
move(0, 20);
|
||||
}
|
||||
move(0,0);
|
||||
}
|
||||
|
||||
void test_motors_diff(){
|
||||
move(MOT_SX, 50*MOT_SX_MULT);
|
||||
move(MOT_DX, 50*MOT_DX_MULT);
|
||||
}
|
||||
|
||||
|
||||
void test_motors_torque(int motor, int dir){
|
||||
while(!Serial){;}
|
||||
|
||||
while(1){
|
||||
delay(1000);
|
||||
Serial.println("Setup, waiting 8 secs");
|
||||
delay(8000);
|
||||
|
||||
for(int i = 0; i <= 255; i+= 15){
|
||||
digitalWrite(mot_pins[motor][1], dir > 0 ? LOW : HIGH);
|
||||
digitalWrite(mot_pins[motor][0], dir > 0 ? HIGH : LOW);
|
||||
Serial.print("PWM: ");
|
||||
Serial.println(i);
|
||||
|
||||
analogWrite(mot_pins[motor][2], i);
|
||||
delay(8000);
|
||||
|
||||
digitalWrite(mot_pins[motor][0], LOW);
|
||||
digitalWrite(mot_pins[motor][1], LOW);
|
||||
delay(2000);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,79 @@
|
|||
#include <ArduPID.h>
|
||||
|
||||
#define MOT_DX 0
|
||||
#define MOT_SX 1
|
||||
|
||||
constexpr float MOT_SX_MULT = 1.0;
|
||||
constexpr float MOT_DX_MULT = 1.0;
|
||||
|
||||
ArduPID myController;
|
||||
|
||||
// Calculated with matlab, then adjusted by hand
|
||||
// Decent values?
|
||||
/*constexpr double KP = 2.5;
|
||||
constexpr double KI = 0.04;
|
||||
constexpr double KD = 0.03;*/
|
||||
|
||||
// Event better
|
||||
constexpr double KP = 4;
|
||||
constexpr double KI = 5;
|
||||
constexpr double KD = 0.1;
|
||||
|
||||
//double setpoint = -0.015;
|
||||
double setpoint = 0.0;
|
||||
double output = 0;
|
||||
double input = 0;
|
||||
|
||||
double roll{ 0 }, pitch{ 0 }, yaw{ 0 };
|
||||
|
||||
void setup(void) {
|
||||
Serial.begin(9600);
|
||||
delay(500);
|
||||
|
||||
setup_imu();
|
||||
setup_motors();
|
||||
|
||||
pinMode(LED_BUILTIN, OUTPUT);
|
||||
digitalWrite(LED_BUILTIN, HIGH);
|
||||
|
||||
// Let the initial error from madgwick filter discharge without affecting the integral term of the PID
|
||||
unsigned long t = millis();
|
||||
while (millis() - t < 2000) {
|
||||
update_imu();
|
||||
}
|
||||
|
||||
myController.begin(&input, &output, &setpoint, KP, KI, KD, P_ON_E, FORWARD);
|
||||
myController.setOutputLimits(-0.72, 0.72); // double of max torque motors can exhert
|
||||
//myController.setWindUpLimits(-0.00001, 0.0001);
|
||||
myController.setSampleTime(1);
|
||||
myController.start();
|
||||
|
||||
digitalWrite(LED_BUILTIN, LOW);
|
||||
}
|
||||
|
||||
double map_double(double x, double in_min, double in_max, double out_min, double out_max) {
|
||||
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
||||
}
|
||||
|
||||
unsigned long t = 0;
|
||||
double oldpwm = 0;
|
||||
|
||||
void loop() {
|
||||
update_imu();
|
||||
|
||||
/*static double oldPitch = 0;
|
||||
input = 0.5*oldPitch + 0.5*pitch;
|
||||
oldPitch = pitch;*/
|
||||
input = pitch;
|
||||
|
||||
myController.compute();
|
||||
|
||||
double torque = abs(output) * 0.5;
|
||||
|
||||
double s = sign(output);
|
||||
double pwm_dx = torque <= 0.005 ? 30*s : torque_to_pwm_dx(torque)*s;
|
||||
double pwm_sx = torque <= 0.005 ? 30*s : torque_to_pwm_sx(torque)*s;
|
||||
|
||||
move_pwm(MOT_SX, pwm_sx);
|
||||
move_pwm(MOT_DX, pwm_dx);
|
||||
}
|
||||
Loading…
Reference in New Issue