- Arduino Mega2560
- Motor Shield V1
- 3v-to-5v level converter
- MPU-9255
- DC motors
- 9V battery holder for Arduino
- 4x AA battery holder for the motors
The Motor Shield V1 is really obsolete. The I2C pins are not passed through. That makes using it with the Arduino UNO impossible. So I switched to the Mega2560 that has the I2C pins duplicated at a different location. The 3.3v pin is still not passed through. That got fixed with a Dremel. The L293D chip on the motor shield is really inefficient and the voltage drop is significant.
There is a newer Motor Shield V2 without any of these problems. But I didn't want to spend $20. In addition, the new version uses I2C and I didn't feel like troubleshooting multiple devices on the I2C bus with a level converter.
Actually, no.
Wire wrapping to the rescue! Those tiny white wires are 30 AWG Kynar wire wrapping wires. You can wire wrap any rectangular post that is small enough to fit the wire wrapping tool. You can wire wrap DuPont pins! For female headers, wire wrap one side of a double male header and plug it in.
The Arduino UNO R3 and to some extent the Mega2560 are obsolete too. The new Arduino UNO R4 has a far more efficient switching regulator, LED matrix display, and optional bluetooth and wifi support.
The MPU-9255 has been out of production for years.
The 3v-to-5v level converter is needed because the Arduino is a 5v device and the MPU-9255 is a 3v device. You really want to respect the voltages when it comes to I2C communications.
Connecting it all up is easy, right? Just add a few more power and ground pins on the motor shield and use wires with DuPont connectors:
Actually, no.
DuPont connectors do not connect securely. Whenever the bot falls, some wires risk coming loose. Cheap pre-made wires with DuPont connectors also fail at an alarmingly high rate.
Well, I found an alternative:
Wire wrapping to the rescue! Those tiny white wires are 30 AWG Kynar wire wrapping wires. You can wire wrap any rectangular post that is small enough to fit the wire wrapping tool. You can wire wrap DuPont pins! For female headers, wire wrap one side of a double male header and plug it in.
Wire wrapping is actually more secure than soldering. It is also easier to wrap or unwrap than to solder or de-solder. The Cray 1 was wire wrapped. That's good enough for my toy robot. It has gone out of fashion many decades ago though. Fortunately, you can still buy proper Kynar wire wrapping wires and a wire wrapping tool off your favorite online store. Get the tool that does modified wraps.
As for the code, I just ported the Javascript Microbit code to Arduino C.
The Motor Shield V1 can be controlled using the Adafruit Motor Shield library v1.0.1. That seems to work fine.
However, I couldn't find a MPU-9255 library that suited my needs. I ported my MPU-9255 code and even added accelerometer bias adjustments.
Tuning the PID parameters took a few hours. The PID scaling constants seem to depend on the mounting height of the sensor. The bot to stayed up for 2 hours before I shut it down.
Here's the code:
#include <Wire.h>#include <AFMotor.h>// Tuning Parametersfloat TARGET_ANGLE = -3.1; // forward tilt in degreesfloat 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 clockdelay(200); // must delay after resetmpu_write(REG_SIGNAL_PATH_RESET, 0x7); // GYRO_RST | ACCEL_RST | TEMP_RSTmpu_write(REG_USER_CTRL, 0x1); // SIG_COND_RSTmpu_write(REG_CONFIG, gyro_lpf);mpu_write(REG_GYRO_CONFIG, 0x10); // GYRO_FS_SEL = 3, +-1000 dps, DLPF onmpu_write(REG_ACCEL_CONFIG, 0x18); // ACCEL_FS_SEL = 3, +-16g, DLPF onmpu_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/sfloat gyro_angle_change = gyro_angle_rate * delta_t / 1000; // degreesest_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;}
