DIY Segway code using the MPU-6050

[SouthernAtHeart]

I've made a homemade segway using this sensor, but it's rather jerky.  I'm wondering if anyone has some good working code that I could look at.  I'm using Sabertooth 25x2 motor drivers, and and Atmega328P chip.

thanks.

[laptophead]

I wonder if you're using the PID library for Arduino.  Properly adjusted, it dampens the jerks.  I have a hard time implementing with Jeff's library....

 

Keep us posted

[luisrodenas]

Oh oh...  this is not nice.  Any forum moderator???  Jeff??

[SouthernAtHeart]

I wonder if you're using the PID library for Arduino.

I think this code, a modified version from I2C, just gets the angle from a calculation made from the accel/gyro readings, and then maps motor speed to platform angle. If there were a way to have the gyro reading actually check the 'rate of change' and if it's too fast, slow down the motor power accordingly. I can reason this theory, but don't know where to even start, as far as coding it. I'm not a programmer, only what little I've learned working with Arduino.

My code:

boolean debugMode = false; //set to true to execute Serial debug data

#include "Wire.h"

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

MPU6050 mpu;

#define OUTPUT_READABLE_YAWPITCHROLL

// MPU control/status vars

bool dmpReady = false; // set true if DMP init was successful

uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU

uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)

uint16_t packetSize; // expected DMP packet size (default is 42 bytes)

uint16_t fifoCount; // count of all bytes currently in FIFO

uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars

Quaternion q; // [w, x, y, z] quaternion container

VectorInt16 aa; // [x, y, z] accel sensor measurements

VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements

VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements

VectorFloat gravity; // [x, y, z] gravity vector

float euler[3]; // [psi, theta, phi] Euler angle container

float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo

uint8_t teapotPacket[14] = {

'$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

// ================================================================

// === INTERRUPT DETECTION ROUTINE ===

// ================================================================

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high

void dmpDataReady() {

mpuInterrupt = true;

}

// ================================================================

// === SEGWAY SETUP ===

// ================================================================

#include <SoftwareSerial.h>

SoftwareSerial Segway(12,4); //(4, 12); // RX, TX, (D12 is used to send commands to the Sabertooth)

boolean statusOK = false; //will be set to true once the segway is level for startup

float pitch;

int prevPitch;

int motor_speed; //speed value used to give values to each motor

int calibrateButton = A1; //pushbutton connected to A1 pin

int pitchOffset = 0; //in case the IMU is not physically mounted perpedicular

int maxSpeed = 64; //will be changed by standbyMode

boolean standbyMode = true;

// ================================================================

// === INITIAL SETUP ===

// ================================================================

void setup() {

// join I2C bus (I2Cdev library doesn't do this automatically)

Wire.begin();

Serial.begin(115200); //for debugging

Segway.begin(9600); //begin communication with Segway motor driver

// initialize device

Serial.println(F("Initializing I2C devices..."));

mpu.initialize();

// verify connection

Serial.println(F("Testing device connections..."));

Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

// load and configure the DMP

Serial.println(F("Initializing DMP..."));

devStatus = mpu.dmpInitialize();

// make sure it worked (returns 0 if so)

if (devStatus == 0) {

// turn on the DMP, now that it's ready

Serial.println(F("Enabling DMP..."));

mpu.setDMPEnabled(true);

// enable Arduino interrupt detection

Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));

attachInterrupt(0, dmpDataReady, RISING);

mpuIntStatus = mpu.getIntStatus();

// set our DMP Ready flag so the main loop() function knows it's okay to use it

Serial.println(F("DMP ready! Waiting for first interrupt..."));

dmpReady = true;

// get expected DMP packet size for later comparison

packetSize = mpu.dmpGetFIFOPacketSize();

}

else {

// ERROR!

// 1 = initial memory load failed

// 2 = DMP configuration updates failed

// (if it's going to break, usually the code will be 1)

Serial.print(F("DMP Initialization failed (code "));

Serial.print(devStatus);

Serial.println(F(")"));

}

Serial.println(F("Waiting for Segway to be balanced before starting"));

}

// ================================================================

// === MAIN PROGRAM LOOP ===

// ================================================================

void loop() {

// if programming failed, don't try to do anything

if (!dmpReady) return;

// wait for MPU interrupt or extra packet(s) available

while (!mpuInterrupt && fifoCount < packetSize) {

// other program behavior stuff here doesn't work!

}

// reset interrupt flag and get INT_STATUS byte

mpuInterrupt = false;

mpuIntStatus = mpu.getIntStatus();

// get current FIFO count

fifoCount = mpu.getFIFOCount();

// check for overflow (this should never happen unless our code is too inefficient)

if ((mpuIntStatus & 0x10) || fifoCount == 1024) {

// reset so we can continue cleanly

mpu.resetFIFO();

if (debugMode) Serial.println(F("FIFO overflow!"));

// otherwise, check for DMP data ready interrupt (this should happen frequently)

}

else if (mpuIntStatus & 0x02) {

// wait for correct available data length, should be a VERY short wait

while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

// read a packet from FIFO

mpu.getFIFOBytes(fifoBuffer, packetSize);

// track FIFO count here in case there is > 1 packet available

// (this lets us immediately read more without waiting for an interrupt)

fifoCount -= packetSize;

#ifdef OUTPUT_READABLE_YAWPITCHROLL

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

pitch = (float(ypr[1] * 180/M_PI)); //angle of platform

pitch = pitch - pitchOffset; //compensate for platform level point

pitch = pitch * 100;

pitch = int(pitch);

#endif

}

if (statusOK) update_motor();

else startup(); //make sure it's level before the intitial start

leds();

}

//try changing the loop time, the maxPitch to 1300; the fscale factor to 2.5 or 3

void update_motor(){ // Update the motors

static unsigned long prevTime;

int motor1, motor2;

if (millis() - prevTime < 30) return; //only do this every 30 ms

prevTime = millis(); //update the time stamp

//modeCheck(); //update maxPitch for standby mode

int maxPitch = 1500; //value from the MPU-6050, this makes 30 degrees the max tilt for 100% speed

int maxSpeed = 64;

if (pitch < -3000) crash(); //we probably crashed

if (pitch > 3000) crash(); //we probably crashed

//motor_speed = map(pitch, -maxPitch, maxPitch, -standbySpeed, standbySpeed); //standby mode

motor_speed = map(pitch, -maxPitch, maxPitch, -maxSpeed, maxSpeed); //it's now 1 to 64

if (motor_speed > 0) motor_speed = fscale( 1, maxSpeed, 1, maxSpeed, motor_speed, -2); //scale it to change slightly at low numbers

if (motor_speed < 0) motor_speed = fscale( -maxSpeed,-1, -maxSpeed,-1,motor_speed, 2); //scale it to change slightly at low numbers

//if (motor_speed > 45) motor_speed = 64; //that's the maximum speed

Serial.println(motor_speed);

// if (CheckPosNeg(motor_speed)) { //if true, we're oscilating out of control

// int a = 0;

// Segway.write(a);

// statusOK = false;

// return;

// }

motor1 = motor_speed + get_Steering(); //add steering bias to motor 1

motor2 = motor_speed - get_Steering(); //add steering bias to motor 2

// assign final motor output values

motor1 = 65 + motor1; //64 is neutral for motor 1

motor2 = 192 + motor2; //192 is neutral for motor 2

motor1 = constrain(motor1, 1, 127); //constrain the value to it's min/max

motor2 = constrain(motor2, 128, 255);//constrain the value to it's min/max

if (!debugMode) Segway.write(motor1); //Send motor 1 speed over serial

delay(1); //sabertooth can only receive commands at 2000/second

if (!debugMode) Segway.write(motor2); //Send motor 2 speed over serial

if (debugMode) {

Serial.print("Pitch: ");

Serial.print(pitch);

Serial.print("\tMotorSpeed1: ");

Serial.print(motor1);

Serial.print("\tMotorSpeed2: ");

Serial.println(motor2);

}

}

void crash() {

Serial.print("CRASHED!");

int crashed = 0;

while(1){

Segway.write(crashed); //Send motor stop command to inhibit further wounds

delay(50);

}

}

void startup(){

//give the IMU a chance to be up and running

if (millis() < 1000) return;

int tilt = abs(pitch);

if (tilt < 10) { //don't start until it is balanced!

statusOK = true;

Serial.println("Segway is balanced");

}

}

int get_Steering() {

//0 to 1023 is the full reading of POT

int reading = analogRead(A0);

int speedVal = abs(motor_speed);

if (speedVal > 15) reading = map(reading, 0, 1023,-6 , 6); //smoothe turning at higher speeds

else reading = map(reading, 0, 1023,-10 , 10); //this gives a nicer turn at low speeds

return reading;

}

unsigned long prevChangeTime; //last time we changed directions

unsigned long lastRevTime; //last time we went into reverse

const boolean forwardDirection = true;

const boolean reverseDirection = false;

boolean CheckPosNeg(int Val) {

//return false;

const int ControlState = -2;

if (Val == ControlState) return false; //when it's 0, it can be ignored

int timeRange = 2000; //within this time range

const int changeCount = 3; //number of changes to trigger flag

static int lastVal; //the last value passed

static int countArray(changeCount); //I need to keep track of this many changes

static unsigned long timeArray[changeCount + 1];//this will hold a time stamp for each change {1,2,3}

//Serial.println(Val);

if (Val > ControlState) {

if(lastVal < ControlState) { //a change

for (int i=changeCount; i > 0; i--){

timeArray = timeArray[i-1]; //move all the value up the line

}

timeArray[0] = millis(); //put the current one at the beginning

lastVal = Val; //update the lastVal

//now see if it's been longer than [timeRange] for the least recent timestamp

//Serial.println(timeArray[changeCount]);

if (millis() - timeArray[changeCount] < timeRange) {

Serial.println("Oscilate!");

return true; //send up red sparks!

}

else return false; //all is well

}

}

lastVal = Val;

return false;

}

[SouthernAtHeart]

I've made a homemade segway using this sensor, but it's rather jerky.  I'm wondering if anyone has some good working code that I could look at.  I'm using Sabertooth 25x2 motor drivers, and and Atmega328P chip.

thanks.

I'm not familiar with how to implement a PID library.  Currently, all my code does it check the platform angle, and then output the motor speed directly proportionally to that. (0 degrees = 0 motor speed; 20 degrees of so = 100% motor speed).  I think I need PID, but haven't seen any that's used with the 6050.  Here's a video of the problem: http://youtu.be/xU4MFNJ3I5c

[luisrodenas]

Yes, if you put and calibrate a PID it should work much better. Just a PD should be enough.

 

Instead of doing:   motor=P(constant)*(measured angle)

 

Try something like:  motor=P(constant)*(measured angle) - D(constant)*(measured angular velocity)

 

You will have to do some try and error to find the correct values for P and D.

Good luck!

[SouthernAtHeart]

Yes, if you put and calibrate a PID it should work much better. Just a PD should be enough.

 

Instead of doing:   motor=P(constant)*(measured angle)

 

Try something like:  motor=P(constant)*(measured angle) - D(constant)*(measured angular velocity)

 

You will have to do some try and error to find the correct values for P and D.

Good luck!

Thanks.  I am beginning to understand this.  I'm going to try the Arduino PID_V1 library here:

http://playground.arduino.cc/Code/PIDLibrary

 

It has Kp, Ki, & Kd.  So in your expression: motor=P(constant)*(measured angle) - D(constant)*(measured angular velocity)

Kp is the first part, correct?

Then is Kd the second part that I multiply by (measured angular velocity)?  So I don't use Ki?  If I'm not using Ki, what value should it be, 0 or 1?

thanks again.

[luisrodenas]

Yes. I would leave Ki to 0 to start trying. The measured angular velocity is the gyro reading you get from MPU.  Kd usually is smaller than Kp if using same units for angle and angular velocity.