#include <AFMotor.h>
// Tuning Parameters
float TARGET_ANGLE = -3.1; // forward tilt in degrees
float SMOOTHING_FACTOR = 200;
float KE = 15;
float KD = KE/20;
float KI = KD/5;
const int MPU_ADDR = 0x68;
const int WHO_AM_I = 0x75;
const int REG_PWR_MGMT_1 = 0x6b;
const int REG_SIGNAL_PATH_RESET = 0x68;
const int REG_USER_CTRL = 0x6a;
const int REG_ACCEL_XOUT_H = 0x3b;
const int REG_ACCEL_YOUT_H = 0x3d;
const int REG_ACCEL_ZOUT_H = 0x3f;
const int REG_GYRO_XOUT_H = 0x43;
const int REG_GYRO_YOUT_H = 0x45;
const int REG_GYRO_ZOUT_H = 0x47;
const int REG_CONFIG = 0x1a;
const int REG_GYRO_CONFIG = 0x1b;
const int REG_ACCEL_CONFIG = 0x1c;
const int REG_ACCEL_CONFIG2 = 0x1d;
const int XG_OFFSET_H = 0x13;
const int YG_OFFSET_H = 0x15;
const int ZG_OFFSET_H = 0x17;
const int XA_OFFSET_H = 0x77;
const int YA_OFFSET_H = 0x7A;
const int ZA_OFFSET_H = 0x7D;
bool mpu_read(int reg, byte *data) {
Wire.beginTransmission(MPU_ADDR);
Wire.write(reg);
Wire.endTransmission(false);
int n = Wire.requestFrom(MPU_ADDR, 1);
if (n != 1) {
return false;
}
*data = Wire.read();
return true;
}
bool mpu_read_int16(int reg, short *data) {
Wire.beginTransmission(MPU_ADDR);
Wire.write(reg);
Wire.endTransmission(false);
int n = Wire.requestFrom(MPU_ADDR, 2);
if (n != 2) {
return false;
}
int h = Wire.read();
int l = Wire.read();
*data = (h << 8) | l;
return true;
}
void mpu_write(int reg, int data) {
Wire.beginTransmission(MPU_ADDR);
Wire.write(reg);
Wire.write(data & 0xff);
Wire.endTransmission();
}
void mpu_write_int16(int reg, int data) {
Wire.beginTransmission(MPU_ADDR);
Wire.write(reg);
Wire.write((data >> 8) & 0xff);
Wire.write(data & 0xff);
Wire.endTransmission();
}
/**
* Reset the MPU and configure the gyroscope to +- 2000 degrees/second and the accelerometer to +-16g.
* The low pass filter settings control how sensitive the sensors are to quick changes.
* In order of increasing sensitivity: 6, 5, 4, 3, 2, 1, 0, 7
* Info from MPU-9250 register map document.
* @param gyro_lpf Gyroscope low pass filter setting, eg: 0
* 7: 8kHz sampling rate, 3600Hz bandwidth, 0.17ms delay.
* 0: 8kHz sampling rate, 250Hz bandwidth, 0.97ms delay.
* 1: 1kHz sampling rate, 184Hz bandwidth, 2.9ms delay.
* 2: 1kHz sampling rate, 92Hz bandwidth, 3.9ms delay.
* 3: 1kHz sampling rate, 41Hz bandwidth, 5.9ms delay.
* 4: 1kHz sampling rate, 20Hz bandwidth, 9.9ms delay.
* 5: 1kHz sampling rate, 10Hz bandwidth, 17.85ms delay.
* 6: 1kHz sampling rate, 5Hz bandwidth, 33.48ms delay.
* @param accel_lpf Accelerometer low pass filter setting, eg: 0
* 7: 1kHz sampling rate, 420Hz 3dB bandwidth, 1.38ms delay.
* 0: 1kHz sampling rate, 218.1Hz 3dB bandwidth, 1.88ms delay.
* 1: 1kHz sampling rate, 218.1Hz 3dB bandwidth, 1.88ms delay.
* 2: 1kHz sampling rate, 99Hz 3dB bandwidth, 2.88ms delay.
* 3: 1kHz sampling rate, 44.8Hz 3dB bandwidth, 4.88ms delay.
* 4: 1kHz sampling rate, 21.2Hz 3dB bandwidth, 8.87ms delay.
* 5: 1kHz sampling rate, 10.2Hz 3dB bandwidth, 16.83ms delay.
* 6: 1kHz sampling rate, 5.05Hz 3dB bandwidth, 32.48ms delay.
*/
bool reset_mpu(int gyro_lpf = 0, int accel_lpf = 0) {
mpu_write(REG_PWR_MGMT_1, 0x80); // H_RESET, internal 20MHz clock
delay(200); // must delay after reset
mpu_write(REG_SIGNAL_PATH_RESET, 0x7); // GYRO_RST | ACCEL_RST | TEMP_RST
mpu_write(REG_USER_CTRL, 0x1); // SIG_COND_RST
mpu_write(REG_CONFIG, gyro_lpf);
mpu_write(REG_GYRO_CONFIG, 0x10); // GYRO_FS_SEL = 3, +-1000 dps, DLPF on
mpu_write(REG_ACCEL_CONFIG, 0x18); // ACCEL_FS_SEL = 3, +-16g, DLPF on
mpu_write(REG_ACCEL_CONFIG2, accel_lpf);
delay(200);
byte x = 255;
if (!mpu_read(REG_GYRO_CONFIG, &x)) {
return false;
}
if (x != 0x10) {
return false;
}
return true;
}
bool compute_biases() {
mpu_write_int16(XG_OFFSET_H, 0);
mpu_write_int16(YG_OFFSET_H, 0);
mpu_write_int16(ZG_OFFSET_H, 0);
const int N = 100;
const int MAX_VAR = 40;
float sum_x = 0, sum_y = 0, sum_z = 0;
float sum_ax = 0, sum_ay = 0, sum_az = 0;
float xs[N];
float ys[N];
float zs[N];
for (int i = 0; i < N; i++) {
short x;
short y;
short z;
if (!mpu_read_int16(REG_GYRO_XOUT_H, &x)) {
return false;
}
if (!mpu_read_int16(REG_GYRO_YOUT_H, &y)) {
return false;
}
if (!mpu_read_int16(REG_GYRO_ZOUT_H, &z)) {
return false;
}
sum_x += x;
sum_y += y;
sum_z += z;
xs[i] = x;
ys[i] = y;
zs[i] = z;
short ax;
short ay;
short az;
if (!mpu_read_int16(REG_ACCEL_XOUT_H, &ax)) {
return false;
}
if (!mpu_read_int16(REG_ACCEL_YOUT_H, &ay)) {
return false;
}
if (!mpu_read_int16(REG_ACCEL_ZOUT_H, &az)) {
return false;
}
sum_ax += ax;
sum_ay += ay;
sum_az += az;
delay(10);
}
float mean_x = sum_x / N;
float mean_y = sum_y / N;
float mean_z = sum_z / N;
float var_x = 0, var_y = 0, var_z = 0;
for (int i = 0; i < N; i++) {
float dx = xs[i] - mean_x;
var_x = var_x + dx * dx;
float dy = ys[i] - mean_y;
var_y = var_y + dy * dy;
float dz = zs[i] - mean_z;
var_z = var_z + dz * dz;
}
var_x = var_x / N;
var_y = var_y / N;
var_z = var_z / N;
float mean_ax = sum_ax / N;
float mean_ay = sum_ay / N;
float mean_az = sum_az / N;
Serial.print("Gyro: mean ");
Serial.print(mean_x);
Serial.print(", ");
Serial.print(mean_y);
Serial.print(", ");
Serial.print(mean_z);
Serial.print(" var ");
Serial.print(var_x);
Serial.print(", ");
Serial.print(var_y);
Serial.print(", ");
Serial.print(var_z);
Serial.print(" Accel: mean ");
Serial.print(mean_ax);
Serial.print(", ");
Serial.print(mean_ay);
Serial.print(", ");
Serial.println(mean_az);
set_gyro_bias(mean_x, mean_y, mean_z);
set_accel_bias(mean_ax, 0, mean_az);
return true;
}
void set_gyro_bias(int x_bias, int y_bias, int z_bias) {
mpu_write_int16(XG_OFFSET_H, -x_bias);
mpu_write_int16(YG_OFFSET_H, -y_bias);
mpu_write_int16(ZG_OFFSET_H, -z_bias);
}
bool set_accel_bias(short x_bias, short y_bias, short z_bias) {
short cur_x;
short cur_y;
short cur_z;
if (!mpu_read_int16(XA_OFFSET_H, &cur_x)) {
return false;
}
if (!mpu_read_int16(YA_OFFSET_H, &cur_y)) {
return false;
}
if (!mpu_read_int16(ZA_OFFSET_H, &cur_z)) {
return false;
}
cur_x -= x_bias;
cur_y -= y_bias;
cur_z -= z_bias;
cur_x &= ~1;
cur_y &= ~1;
cur_z &= ~1;
mpu_write_int16(XA_OFFSET_H, cur_x);
mpu_write_int16(YA_OFFSET_H, cur_y);
mpu_write_int16(ZA_OFFSET_H, cur_z);
return true;
}
//-----------------------------
AF_DCMotor *motor_left;
AF_DCMotor *motor_right;
void motor_move(float v /* -100 to 100 */) {
if (v > 0) {
motor_left->run(BACKWARD);
motor_right->run(FORWARD);
} else {
motor_left->run(FORWARD);
motor_right->run(BACKWARD);
v = -v;
}
if (v > 100) {
v = 100;
}
v = v * 0.7 + 185;
if (v < 195) {
v = 0;
}
motor_left->setSpeed(v);
motor_right->setSpeed(v);
}
void motor_coast() {
motor_left->run(RELEASE);
motor_right->run(RELEASE);
}
//-----------------------------
long last_time = 0;
float motor_out = 0;
int vertical_count = 0;
bool motor_on = false;
float est_angle = 0;
float last_err = 0;
float i_err = 0;
void setup() {
pinMode(LED_BUILTIN, OUTPUT);
for (int i = 0; i < 20; i++) {
digitalWrite(LED_BUILTIN, HIGH);
delay(100);
digitalWrite(LED_BUILTIN, LOW);
delay(100);
}
digitalWrite(SDA, HIGH);
digitalWrite(SCL, HIGH);
Wire.begin();
Wire.setClock(400000);
while (1) {
byte id;
if (mpu_read(WHO_AM_I, &id)) {
if (id == 0x73) {
if (reset_mpu(5, 5)) {
break;
}
}
}
delay(3000);
}
// add 32768/16g = 2048 to the accel axis pointing to ground
//compute_biases();
set_gyro_bias(27, -40, -43);
set_accel_bias(40, 58, 417);
motor_left = new AF_DCMotor(3);
motor_right = new AF_DCMotor(2);
motor_coast();
last_time = millis();
motor_out = 0;
vertical_count = 0;
motor_on = false;
est_angle = 0;
last_err = 0;
i_err = 0;
digitalWrite(LED_BUILTIN, HIGH);
delay(500);
}
bool read_accel_tilt_angle(float *angle) {
short accel_x;
short accel_z;
if (!mpu_read_int16(REG_ACCEL_XOUT_H, &accel_x)) {
return false;
}
if (!mpu_read_int16(REG_ACCEL_ZOUT_H, &accel_z)) {
return false;
}
*angle = atan2(accel_z, -accel_x) * RAD_TO_DEG;
return true;
}
void loop() {
long current_time = millis();
long delta_t = current_time - last_time;
if (delta_t < 5) {
return;
}
short gyro_y;
if (!mpu_read_int16(REG_GYRO_YOUT_H, &gyro_y)) {
return;
}
float accel_angle;
if (!read_accel_tilt_angle(&accel_angle)) {
return;
}
float gyro_angle_rate = float(gyro_y) * -1000.0 / 32768.0; // degrees/s
float gyro_angle_change = gyro_angle_rate * delta_t / 1000; // degrees
est_angle = ((SMOOTHING_FACTOR-1) * (est_angle + gyro_angle_change) + accel_angle) / SMOOTHING_FACTOR;
if (motor_on && (est_angle > 30 || est_angle < -30)) {
last_err = 0;
i_err = 0;
motor_coast();
motor_on = false;
}
float err = est_angle - TARGET_ANGLE;
if (motor_on) {
float d_err = (err - last_err) * 1000 / delta_t;
last_err = err;
i_err = i_err + err * delta_t;
float u = err * KE + d_err * KD + i_err * KI;
motor_out = u;
motor_move(motor_out);
} else if (err <= 5 && err >= -5) {
vertical_count = vertical_count + 1;
if (vertical_count > 100) {
vertical_count = 0;
last_err = 0;
i_err = 0;
motor_on = true;
}
} else if (err <= 5 && err >= -5) {
motor_on = true;
}
last_time = current_time;
}
