upload code. commented. working
EmaMaker
commit
557504f87b
|
|
@ -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,133 @@
|
|||
#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(){
|
||||
// TODO: write my own kalman filter
|
||||
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,92 @@
|
|||
#define MOT_DX_STEP 19
|
||||
#define MOT_DX_DIR 18
|
||||
#define MOT_SX_STEP 17
|
||||
#define MOT_SX_DIR 16
|
||||
|
||||
void setup_motors(){
|
||||
pinMode(MOT_DX_STEP, OUTPUT);
|
||||
pinMode(MOT_DX_DIR, OUTPUT);
|
||||
pinMode(MOT_SX_STEP, OUTPUT);
|
||||
pinMode(MOT_SX_DIR, OUTPUT);
|
||||
|
||||
Serial.println("Motors up");
|
||||
}
|
||||
|
||||
/*
|
||||
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,144 @@
|
|||
#include <ArduPID.h>
|
||||
|
||||
ArduPID myController;
|
||||
|
||||
#define MOT_DX_STEP 19
|
||||
#define MOT_DX_DIR 18
|
||||
#define MOT_SX_STEP 17
|
||||
#define MOT_SX_DIR 16
|
||||
|
||||
// Nema 17 make 1.8° per step. Using A4988 drivers, and 1/16th microstepping, it results in 0.1125° per step
|
||||
constexpr double ANGLE_PER_STEP = 0.1125;
|
||||
// Just used a kitchen scale, good enough
|
||||
constexpr double WEIGHT = 0.961;
|
||||
constexpr double WHEEL_RADIUS = 0.0475;
|
||||
// Experimentally, the lowest pulse my steppers can handle without stalling + some leeway
|
||||
constexpr double MAX_HALF_PERIOD = 75; // in microseconds
|
||||
// Which means there is a maximum velocity achievable
|
||||
constexpr double MAX_VELOCITY = 1000000 * WHEEL_RADIUS / (2 * MAX_HALF_PERIOD * ANGLE_PER_STEP) * PI / 180;
|
||||
|
||||
// Derived and analytical model, linearized it and simulated in MATLAB.
|
||||
// PID values are then calculated and verified by simulation in Simulink. I ain't calibrating a PID by hand on this robot
|
||||
// I modified the ArduPID library to make it accept negative values for the parameters
|
||||
constexpr double KP = -50;
|
||||
constexpr double KI = -600;
|
||||
constexpr double KD = 0.05;
|
||||
|
||||
// IMU little bit tilted
|
||||
// TODO: Implement an outer control loop for angular velocity. But that requires encoders on the motors
|
||||
// TODO: try to achieve it crudely by just using a PI controller on the velocity given by the PID balance controller
|
||||
double setpoint = -0.06;
|
||||
double output = 0;
|
||||
double input = 0;
|
||||
|
||||
double yaw{ 0 }, pitch{ 0 }, roll{ 0 };
|
||||
|
||||
void setup() {
|
||||
Serial.begin(9600);
|
||||
|
||||
delay(1000);
|
||||
|
||||
setup_imu();
|
||||
|
||||
pinMode(MOT_DX_DIR, OUTPUT);
|
||||
pinMode(MOT_SX_DIR, OUTPUT);
|
||||
|
||||
// Just to signal it is working
|
||||
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(-MAX_VELOCITY, MAX_VELOCITY); // double of max torque motors can exhert
|
||||
//myController.setWindUpLimits(-10, 10);
|
||||
myController.setSampleTime(2);
|
||||
myController.start();
|
||||
}
|
||||
|
||||
void setup1(){
|
||||
pinMode(MOT_DX_STEP, OUTPUT);
|
||||
pinMode(MOT_SX_STEP, OUTPUT);
|
||||
}
|
||||
|
||||
unsigned long last_time_motors = micros(), current_time_motors = micros();
|
||||
bool b = true;
|
||||
// Such a long halfperiod means that the motors are not moving
|
||||
uint32_t halfperiod1 = INT_MAX;
|
||||
void loop1(){
|
||||
// TODO: handle the steppers using interrupt timers. The second core could be used for IMU processing, while the first one handles the different control loops
|
||||
|
||||
// Retrieve the half period value from core0. Non blocking call
|
||||
if(rp2040.fifo.available()) rp2040.fifo.pop_nb(&halfperiod1);
|
||||
|
||||
current_time_motors = micros();
|
||||
|
||||
if(current_time_motors - last_time_motors > halfperiod1){
|
||||
// Half a pulse. Next cycle will be the rest
|
||||
digitalWriteFast(MOT_DX_STEP, b);
|
||||
digitalWriteFast(MOT_SX_STEP, b);
|
||||
b = !b;
|
||||
last_time_motors = current_time_motors;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned long last_time = millis(), current_time = millis(), time_diff;
|
||||
double frequency = 0;
|
||||
unsigned long half_period0 = 0;
|
||||
double velocity = 0;
|
||||
|
||||
void loop() {
|
||||
current_time = millis();
|
||||
time_diff = current_time - last_time;
|
||||
|
||||
update_imu();
|
||||
// I also modified the ArduPID library to use compute as a boolean. If calculations were done, it returns true. If not enough time has elapsed, it returns false
|
||||
if(myController.compute()){
|
||||
input = pitch;
|
||||
|
||||
// Keeping it here
|
||||
/*
|
||||
double force_per_motor = output / WHEEL_RADIUS;
|
||||
double accel = force_per_motor / (WEIGHT);
|
||||
velocity += accel * time_diff * 0.001;
|
||||
// anti-windup
|
||||
velocity = constrain(velocity, -MAX_VELOCITY, MAX_VELOCITY);*/
|
||||
|
||||
double tvelocity = output;
|
||||
if(tvelocity < 0){
|
||||
digitalWriteFast(MOT_DX_DIR, LOW);
|
||||
digitalWriteFast(MOT_SX_DIR, LOW);
|
||||
tvelocity = -tvelocity;
|
||||
}else{
|
||||
digitalWriteFast(MOT_DX_DIR, HIGH);
|
||||
digitalWriteFast(MOT_SX_DIR, HIGH);
|
||||
}
|
||||
|
||||
tvelocity = tvelocity * 180 / PI;
|
||||
frequency = tvelocity * 0.1125 / WHEEL_RADIUS;
|
||||
half_period0 = 1000000 / (2*frequency);
|
||||
|
||||
// Send the half period to core 1. Non blocking
|
||||
rp2040.fifo.push_nb(half_period0);
|
||||
|
||||
// Some ugly logging I used in debugging, keeping in around
|
||||
/*Serial.println(input);
|
||||
Serial.print(" | ");
|
||||
Serial.print(output);
|
||||
Serial.print(" | ");
|
||||
Serial.print(accel);
|
||||
Serial.print(" | ");
|
||||
Serial.print(velocity);
|
||||
Serial.print(" | ");
|
||||
Serial.print(frequency);
|
||||
Serial.print(" | ");
|
||||
Serial.println(half_period0);*/
|
||||
|
||||
last_time = current_time;
|
||||
}
|
||||
|
||||
}
|
||||
Loading…
Reference in New Issue