Commit b590e9d2 authored by Wouter Klop's avatar Wouter Klop

Merge branch 'devel' into 'devel'

improved lots of things

See merge request kloppertje/balancingrobot!2
parents 9c76d5c4 e4559e0d
# BalancingRobot
This repository contains all resource files needed to replicate the high speed, two wheeled balancing robot.
For some videos, see my [youtube channel](https://www.youtube.com/watch?v=D7hvI_Tb0o4).
For more info, and future updates, see [my website](http://elexperiment.nl/2018/11/high-speed-balancing-robot-introduction/)
Please be aware that this code is very experimental / far from complete. So, you'll probably have to implement some stuff yourself. Also, use at your own risk.
\ No newline at end of file
For Software Setup see [Software/README.md](Software/README.md)
Please be aware that this code is very experimental / far from complete. So, you'll probably have to implement some stuff yourself. Also, use at your own risk.
### Branches
- master: old and stable version
- devel: new version with lots of new features (recommended)
This diff is collapsed.
......@@ -45,7 +45,7 @@ THE SOFTWARE.
#include "I2Cdev.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE
#ifdef I2CDEV_IMPLEMENTATION_WARNINGS
#if ARDUINO < 100
......@@ -87,6 +87,11 @@ THE SOFTWARE.
#endif
#ifndef BUFFER_LENGTH
// band-aid fix for platforms without Wire-defined BUFFER_LENGTH (removed from some official implementations)
#define BUFFER_LENGTH 32
#endif
/** Default constructor.
*/
I2Cdev::I2Cdev() {
......@@ -217,7 +222,7 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
int8_t count = 0;
uint32_t t1 = millis();
#if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE)
#if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE)
#if (ARDUINO < 100)
// Arduino v00xx (before v1.0), Wire library
......@@ -225,7 +230,7 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
// I2C/TWI subsystem uses internal buffer that breaks with large data requests
// so if user requests more than BUFFER_LENGTH bytes, we have to do it in
// smaller chunks instead of all at once
for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) {
Wire.beginTransmission(devAddr);
Wire.send(regAddr);
Wire.endTransmission();
......@@ -249,7 +254,7 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
// I2C/TWI subsystem uses internal buffer that breaks with large data requests
// so if user requests more than BUFFER_LENGTH bytes, we have to do it in
// smaller chunks instead of all at once
for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) {
Wire.beginTransmission(devAddr);
Wire.write(regAddr);
Wire.endTransmission();
......@@ -273,7 +278,7 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
// I2C/TWI subsystem uses internal buffer that breaks with large data requests
// so if user requests more than BUFFER_LENGTH bytes, we have to do it in
// smaller chunks instead of all at once
for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) {
Wire.beginTransmission(devAddr);
Wire.write(regAddr);
Wire.endTransmission();
......@@ -337,7 +342,7 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
int8_t count = 0;
uint32_t t1 = millis();
#if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE
#if (ARDUINO < 100)
// Arduino v00xx (before v1.0), Wire library
......@@ -594,7 +599,8 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.beginTransmission(devAddr);
Wire.send((uint8_t) regAddr); // send address
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \
|| I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100)
Wire.beginTransmission(devAddr);
Wire.write((uint8_t) regAddr); // send address
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
......@@ -608,16 +614,18 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_
#endif
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.send((uint8_t) data[i]);
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
Wire.write((uint8_t) data[i]);
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \
|| I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100)
Wire.write((uint8_t) data[i]);
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
Fastwire::write((uint8_t) data[i]);
#endif
}
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.endTransmission();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
status = Wire.endTransmission();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \
|| I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100)
status = Wire.endTransmission();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
Fastwire::stop();
//status = Fastwire::endTransmission();
......@@ -649,8 +657,9 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.beginTransmission(devAddr);
Wire.send(regAddr); // send address
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
Wire.beginTransmission(devAddr);
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \
|| I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100)
Wire.beginTransmission(devAddr);
Wire.write(regAddr); // send address
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
Fastwire::beginTransmission(devAddr);
......@@ -664,8 +673,9 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.send((uint8_t)(data[i] >> 8)); // send MSB
Wire.send((uint8_t)data[i++]); // send LSB
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
Wire.write((uint8_t)(data[i] >> 8)); // send MSB
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \
|| I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100)
Wire.write((uint8_t)(data[i] >> 8)); // send MSB
Wire.write((uint8_t)data[i++]); // send LSB
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
Fastwire::write((uint8_t)(data[i] >> 8)); // send MSB
......@@ -675,8 +685,9 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16
}
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.endTransmission();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
status = Wire.endTransmission();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \
|| I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100)
status = Wire.endTransmission();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
Fastwire::stop();
//status = Fastwire::endTransmission();
......
......@@ -47,13 +47,12 @@ THE SOFTWARE.
#ifndef _I2CDEV_H_
#define _I2CDEV_H_
#define BUFFER_LENGTH 32
// -----------------------------------------------------------------------------
// I2C interface implementation setting
// -----------------------------------------------------------------------------
#ifndef I2CDEV_IMPLEMENTATION
#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE
//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_SBWIRE
//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE
#endif // I2CDEV_IMPLEMENTATION
......@@ -69,6 +68,7 @@ THE SOFTWARE.
// ^^^ NBWire implementation is still buggy w/some interrupts!
#define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project
#define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library
#define I2CDEV_BUILTIN_SBWIRE 5 // I2C object from Shuning (Steve) Bian's SBWire Library at https://github.com/freespace/SBWire
// -----------------------------------------------------------------------------
// Arduino-style "Serial.print" debug constant (uncomment to enable)
......@@ -87,6 +87,9 @@ THE SOFTWARE.
#if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY
#include <I2C.h>
#endif
#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE
#include "SBWire.h"
#endif
#endif
#ifdef SPARK
......
......@@ -36,21 +36,13 @@ THE SOFTWARE.
#include "MPU6050.h"
/** Default constructor, uses default I2C address.
* @see MPU6050_DEFAULT_ADDRESS
*/
MPU6050::MPU6050() {
devAddr = MPU6050_DEFAULT_ADDRESS;
}
/** Specific address constructor.
* @param address I2C address
* @param address I2C address, uses default I2C address if none is specified
* @see MPU6050_DEFAULT_ADDRESS
* @see MPU6050_ADDRESS_AD0_LOW
* @see MPU6050_ADDRESS_AD0_HIGH
*/
MPU6050::MPU6050(uint8_t address) {
devAddr = address;
MPU6050::MPU6050(uint8_t address):devAddr(address) {
}
/** Power on and prepare for general usage.
......
......@@ -435,8 +435,7 @@ THE SOFTWARE.
class MPU6050 {
public:
MPU6050();
MPU6050(uint8_t address);
MPU6050(uint8_t address=MPU6050_DEFAULT_ADDRESS);
void initialize();
bool testConnection();
......@@ -459,15 +458,15 @@ class MPU6050 {
uint8_t getFullScaleGyroRange();
void setFullScaleGyroRange(uint8_t range);
// SELF_TEST registers
uint8_t getAccelXSelfTestFactoryTrim();
uint8_t getAccelYSelfTestFactoryTrim();
uint8_t getAccelZSelfTestFactoryTrim();
// SELF_TEST registers
uint8_t getAccelXSelfTestFactoryTrim();
uint8_t getAccelYSelfTestFactoryTrim();
uint8_t getAccelZSelfTestFactoryTrim();
uint8_t getGyroXSelfTestFactoryTrim();
uint8_t getGyroYSelfTestFactoryTrim();
uint8_t getGyroZSelfTestFactoryTrim();
uint8_t getGyroXSelfTestFactoryTrim();
uint8_t getGyroYSelfTestFactoryTrim();
uint8_t getGyroZSelfTestFactoryTrim();
// ACCEL_CONFIG register
bool getAccelXSelfTest();
void setAccelXSelfTest(bool enabled);
......@@ -823,8 +822,6 @@ class MPU6050 {
// special methods for MotionApps 2.0 implementation
#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20
uint8_t *dmpPacketBuffer;
uint16_t dmpPacketSize;
uint8_t dmpInitialize();
bool dmpPacketAvailable();
......@@ -924,8 +921,6 @@ class MPU6050 {
// special methods for MotionApps 4.1 implementation
#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41
uint8_t *dmpPacketBuffer;
uint16_t dmpPacketSize;
uint8_t dmpInitialize();
bool dmpPacketAvailable();
......@@ -1027,6 +1022,10 @@ class MPU6050 {
private:
uint8_t devAddr;
uint8_t buffer[14];
#if defined(MPU6050_INCLUDE_DMP_MOTIONAPPS20) or defined(MPU6050_INCLUDE_DMP_MOTIONAPPS41)
uint8_t *dmpPacketBuffer;
uint16_t dmpPacketSize;
#endif
};
#endif /* _MPU6050_H_ */
......@@ -269,6 +269,10 @@ const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF
};
#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR
#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01
#endif
// thanks to Noah Zerkin for piecing this stuff together!
const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
// BANK OFFSET LENGTH [DATA]
......@@ -302,7 +306,7 @@ const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro
0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo
0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo
0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate
0x02, 0x16, 0x02, 0x00, MPU6050_DMP_FIFO_RATE_DIVISOR // D_0_22 inv_set_fifo_rate
// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
......@@ -395,7 +399,7 @@ uint8_t MPU6050::dmpInitialize() {
setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
setIntEnabled(0x12);
setIntEnabled(1<<MPU6050_INTERRUPT_FIFO_OFLOW_BIT|1<<MPU6050_INTERRUPT_DMP_INT_BIT);
DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
setRate(4); // 1khz / (1 + 4) = 200 Hz
......@@ -703,6 +707,8 @@ uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi
return 0;
}
#ifdef USE_OLD_DMPGETYAWPITCHROLL
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
// yaw: (about Z axis)
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
......@@ -712,6 +718,24 @@ uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gra
data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
return 0;
}
#else
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
// yaw: (about Z axis)
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
// pitch: (nose up/down, about Y axis)
data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
// roll: (tilt left/right, about X axis)
data[2] = atan2(gravity -> y , gravity -> z);
if (gravity -> z < 0) {
if(data[1] > 0) {
data[1] = PI - data[1];
} else {
data[1] = -PI - data[1];
}
}
return 0;
}
#endif
// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
......
......@@ -262,6 +262,10 @@ const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF
};
#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR
#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x03
#endif
const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
// BANK OFFSET LENGTH [DATA]
0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ?
......@@ -302,9 +306,9 @@ const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
0x07, 0x67, 0x01, 0x9A, // ?
0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo
0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo
0x02, 0x16, 0x02, 0x00, 0x03 // D_0_22 inv_set_fifo_rate
0x02, 0x16, 0x02, 0x00, MPU6050_DMP_FIFO_RATE_DIVISOR // D_0_22 inv_set_fifo_rate
// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
// This very last 0x03 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
// DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))
......@@ -432,7 +436,7 @@ uint8_t MPU6050::dmpInitialize() {
DEBUG_PRINTLN(F("Success! DMP configuration written and verified."));
DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
setIntEnabled(0x12);
setIntEnabled(1<<MPU6050_INTERRUPT_FIFO_OFLOW_BIT|1<<MPU6050_INTERRUPT_DMP_INT_BIT);
DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
setRate(4); // 1khz / (1 + 4) = 200 Hz
......@@ -808,6 +812,8 @@ uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi
return 0;
}
#ifdef USE_OLD_DMPGETYAWPITCHROLL
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
// yaw: (about Z axis)
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
......@@ -817,6 +823,24 @@ uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gra
data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
return 0;
}
#else
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
// yaw: (about Z axis)
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
// pitch: (nose up/down, about Y axis)
data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
// roll: (tilt left/right, about X axis)
data[2] = atan2(gravity -> y , gravity -> z);
if(gravity->z<0) {
if(data[1]>0) {
data[1] = PI - data[1];
} else {
data[1] = -PI - data[1];
}
}
return 0;
}
#endif
// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
......
......@@ -3,6 +3,7 @@
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 2016-04-18 - Eliminated a potential infinite loop
// 2013-05-08 - added seamless Fastwire support
// - added note about gyro calibration
// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
......@@ -212,7 +213,9 @@ void setup() {
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
......@@ -248,6 +251,10 @@ void loop() {
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
if (mpuInterrupt && fifoCount < packetSize) {
// try to get out of the infinite loop
fifoCount = mpu.getFIFOCount();
}
// other program behavior stuff here
// .
// .
......@@ -268,13 +275,14 @@ void loop() {
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
fifoCount = mpu.getFIFOCount();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
} else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
......
......@@ -4,8 +4,9 @@
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 2016-04-18 - Eliminated a potential infinite loop
// 2016-02-28 - Cleaned up code to be in line with other example codes
// - Added Ethernet outputs for Quaternion, Euler, RealAccel, WorldAccel
// - Added Ethernet outputs for Quaternion, Euler, RealAccel, WorldAccel
// 2016-02-27 - Initial working code compiled
// Bugs:
// - There is still a hangup after some time, though it only occurs when you are reading data from the website.
......@@ -218,7 +219,9 @@ void setup() {
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
......@@ -236,7 +239,6 @@ void setup() {
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
return;
}
// configure LED for output
......@@ -252,9 +254,14 @@ void setup() {
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
wdt_reset();//Resets the watchdog timer. If the timer is not reset, and the timer expires, a watchdog-initiated device reset will occur.
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
if (mpuInterrupt && fifoCount < packetSize) {
// try to get out of the infinite loop
fifoCount = mpu.getFIFOCount();
}
// other program behavior stuff here
// .
// .
......@@ -274,13 +281,14 @@ void loop() {
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
fifoCount = mpu.getFIFOCount();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
} else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
......
......@@ -22,7 +22,6 @@ Also, you have to publish all modifications.
#include <ArduinoOTA.h>
#include <Streaming.h>
#include <MPU6050.h>
#include <EEPROM.h>
#include <PID.h>
#include <AsyncTCP.h>
#include <ESPAsyncWebServer.h>
......@@ -32,6 +31,7 @@ Also, you have to publish all modifications.
#include <SPIFFSEditor.h>
#include <fastStepper.h>
#include <par.h>
#include <Preferences.h> // for storing settings
// ----- Type definitions
typedef union {
......@@ -72,13 +72,7 @@ AsyncWebServer httpServer(80);
WebSocketsServer wsServer = WebSocketsServer(81);
// -- EEPROM
#define EEPROM_SIZE 1024
#define EEPROM_ADR_INIT 0
#define EEPROM_ADR_GYRO_OFFSET 50
#define EEPROM_ADR_ANGLE_OFFSET 60
#define EEPROM_ADR_WIFI_SSID 70
#define EEPROM_ADR_WIFI_KEY 100
#define EEPROM_ADR_WIFI_MODE 69
Preferences preferences;
// -- Stepper motors
#define motEnablePin 27
......@@ -193,7 +187,8 @@ void setup() {
Serial.begin(115200);
IBus.begin(Serial2);
EEPROM.begin(EEPROM_SIZE);
preferences.begin("settings", false); // false = RW-mode
// preferences.clear(); // Remove all preferences under the opened namespace
pinMode(motEnablePin, OUTPUT);
pinMode(motUStepPin1, OUTPUT);
......@@ -227,36 +222,38 @@ void setup() {
delay(50);
// Init EEPROM, if not done before
if (EEPROM.read(EEPROM_ADR_INIT) != 123) {
EEPROM.write(EEPROM_ADR_INIT, 123);
for (uint16_t i=1; i<EEPROM_SIZE; i++) {
EEPROM.write(i, 0);
}
Serial.println("EEPROM init complete");
#define PREF_VERSION 1 // if setting structure has been changed, count this number up to delete all settings
if (preferences.getUInt("pref_version", 0) != PREF_VERSION) {
preferences.clear(); // Remove all preferences under the opened namespace
preferences.putUInt("pref_version", PREF_VERSION);
Serial << "EEPROM init complete, all preferences deleted, new pref_version: " << PREF_VERSION << "\n";
}
// Read gyro offsets
Serial << "Gyro calibration values: ";
for (uint8_t i=0; i<3; i++) {
gyroOffset[i] = EEPROM.readShort(EEPROM_ADR_GYRO_OFFSET + i*2);
char buf[16];
sprintf(buf, "gyro_offset_%u", i);
gyroOffset[i] = preferences.getShort(buf, 0);
Serial << gyroOffset[i] << "\t";
}
Serial << endl;
// Read angle offset
angleOffset = EEPROM.readFloat(EEPROM_ADR_ANGLE_OFFSET);
angleOffset = preferences.getFloat("angle_offset", 0.0);
// Perform initial gyro measurements
initSensor(50);
// Connect to Wifi and setup OTA if known Wifi network cannot be found
boolean wifiConnected = 0;
if (EEPROM.read(EEPROM_ADR_WIFI_MODE)==1) {
char ssid[30];
char key[30];
EEPROM.readString(EEPROM_ADR_WIFI_SSID, ssid, 30);
EEPROM.readString(EEPROM_ADR_WIFI_KEY, key, 30);
Serial << "Connecting to " << ssid << endl;
if (preferences.getUInt("wifi_mode", 0)==1) {
char ssid[63];
char key[63];
preferences.getBytes("wifi_ssid", ssid, 63);
preferences.getBytes("wifi_key", key, 63);
Serial << "Connecting to '" << ssid << "'" << endl;
// Serial << "Connecting to '" << ssid << "', '" << key << "'" << endl;
WiFi.mode(WIFI_STA);
WiFi.begin(ssid, key);
if (!(WiFi.waitForConnectResult() != WL_CONNECTED)) {
......@@ -530,7 +527,7 @@ void loop() {
// }
// Serial << endl;
Serial << speedInput << "\t" << steerInput << endl;
// Serial << speedInput << "\t" << steerInput << endl;
// Serial << microStep << "\t" << absSpeed << "\t" << endl;
......@@ -634,13 +631,13 @@ void parseCommand(char* data, uint8_t length) {
break;
case 'k': {
uint8_t cmd2 = atoi(data+1);
if (cmd2==1) {
if (cmd2==1) { // calibrate gyro
calculateGyroOffset(100);
} else if (cmd2==2) {
} else if (cmd2==2) { // calibrate acc
Serial << "Updating angle offset from " << angleOffset;
angleOffset = filterAngle;
Serial << " to " << angleOffset << endl;
EEPROM.writeFloat(EEPROM_ADR_ANGLE_OFFSET, angleOffset);
preferences.putFloat("angle_offset", angleOffset);
}
break;}
case 'l':
......@@ -651,7 +648,7 @@ void parseCommand(char* data, uint8_t length) {
break;
case 'w': {
char cmd2 = data[1];
char buf[30];
char buf[63];
uint8_t len;
switch (cmd2) {
......@@ -665,23 +662,19 @@ void parseCommand(char* data, uint8_t length) {
break;
case 's': // Update WiFi SSID
len = length-3;
// EEPROM.write(EEPROM_ADR_WIFI_SSID, len);
memcpy(buf, &data[2], len);
buf[len] = 0;
EEPROM.writeString(EEPROM_ADR_WIFI_SSID, buf);
EEPROM.commit();
preferences.putBytes("wifi_ssid", buf, 63);
break;
case 'k': // Update WiFi key
len = length-3;
memcpy(buf, &data[2], len);
buf[len] = 0;
EEPROM.writeString(EEPROM_ADR_WIFI_KEY, buf);
EEPROM.commit();
preferences.putBytes("wifi_key", buf, 63);
break;
case 'm': // WiFi mode (0=AP, 1=use SSID)
Serial.println(atoi(&data[2]));
EEPROM.write(EEPROM_ADR_WIFI_MODE, atoi(&data[2]));
EEPROM.commit();
preferences.putUInt("wifi_mode", atoi(&data[2]));
}
break;}
}
......@@ -727,11 +720,11 @@ void calculateGyroOffset(uint8_t nSample) {
gyroOffset[1] = sumY/nSample;
gyroOffset[2] = sumZ/nSample;
for (uint8_t i=0; i<3; i++) {
EEPROM.writeShort(EEPROM_ADR_GYRO_OFFSET + i*2, gyroOffset[i]);
char buf[16];
sprintf(buf, "gyro_offset_%u", i);
preferences.putShort(buf, gyroOffset[i]);
}
EEPROM.commit();
Serial << "New gyro calibration values: " << gyroOffset[0] << "\t" << gyroOffset[1] << "\t" << gyroOffset[2] << endl;
}
......@@ -785,7 +778,8 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length
Serial.printf("[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload);
// send message to client
char wBuf[40];
char wBuf[63];
char buf[63];
sprintf(wBuf, "c%dp%5.2f", 1, pidAngle.K);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "c%di%5.2f", 1, pidAngle.Ti);
......@@ -834,11 +828,11 @@ void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t length
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "l%5.0f", maxStepSpeed);
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "wm%d", EEPROM.read(EEPROM_ADR_WIFI_MODE));
sprintf(wBuf, "wm%d", preferences.getUInt("wifi_mode", 0)); // 0=AP, 1=Client
wsServer.sendTXT(num, wBuf);
sprintf(wBuf, "ws%s", EEPROM.readString(EEPROM_ADR_WIFI_SSID).c_str());
preferences.getBytes("wifi_ssid", buf, 63);
sprintf(wBuf, "ws%s", buf);
wsServer.sendTXT(num, wBuf);
}
break;
case WStype_TEXT:
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment