Arduino Uno to control stepper motor using gyro and accelerometer

Projects General Guidance
1

Currently working on a project that at the end of the day needs to use the values from gyro/accelerometer combo to turn a stepper motor.

The gyro/accelerometer are actually on the same board (6DOF) purchased from sparkfun:

I am using a L298N Dual H Bridge DC stepper motor controller

Link to a picture of my setup:

At this point i have tested the motor by running an example code found on arduino:

/*
 Stepper Motor Control - one revolution

 This program drives a unipolar or bipolar stepper motor.
 The motor is attached to digital pins 8 - 11 of the Arduino.

 The motor should revolve one revolution in one direction, then
 one revolution in the other direction.


 Created 11 Mar. 2007
 Modified 30 Nov. 2009
 by Tom Igoe

 */

#include <Stepper.h>

const int stepsPerRevolution = 200;  // change this to fit the number of steps per revolution
// for your motor

// initialize the stepper library on pins 8 through 11:
Stepper myStepper(stepsPerRevolution, 8, 9, 10, 11);

void setup() {
  // set the speed at 60 rpm:
  myStepper.setSpeed(60);
  // initialize the serial port:
  Serial.begin(9600);
}

void loop() {
  // step one revolution  in one direction:
  Serial.println("clockwise");
  myStepper.step(stepsPerRevolution);
  delay(500);

  // step one revolution in the other direction:
  Serial.println("counterclockwise");
  myStepper.step(-stepsPerRevolution);
  delay(500);
}

The gyro/accelerometer are also working by using the FreeSixCube found below:

////////////////////////////////////////////////////////////
// Arduino firmware for use with FreeSixCube processing example
////////////////////////////////////////////////////////////

#include <FreeSixIMU.h>
#include <FIMU_ADXL345.h>
#include <FIMU_ITG3200.h>

#define DEBUG
#ifdef DEBUG
#include "DebugUtils.h"
#endif

#include "CommunicationUtils.h"
#include "FreeSixIMU.h"
#include <Wire.h>


float q[4]; //hold q values

// Set the FreeIMU object
FreeSixIMU my3IMU = FreeSixIMU();

void setup() {
  Serial.begin(115200);
  Wire.begin();

  delay(5);
  my3IMU.init();
  delay(5);
}


void loop() { 
  my3IMU.getQ(q);
  serialPrintFloatArr(q, 4);
  Serial.println(""); //line break
 
  delay(60);

After loading this to arduino i use another program ran in Processing. This generates an on screen image of the gyros position.

import processing.serial.*;

Serial myPort;  // Create object from Serial class

final String serialPort = "COM6"; // replace this with your serial port. On windows you will need something like "COM1".

float [] q = new float [4];
float [] hq = null;
float [] Euler = new float [3]; // psi, theta, phi

int lf = 10; // 10 is '\n' in ASCII
byte[] inBuffer = new byte[22]; // this is the number of chars on each line from the Arduino (including /r/n)

PFont font;
final int VIEW_SIZE_X = 800, VIEW_SIZE_Y = 600;


void setup() 
{
  size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
  myPort = new Serial(this, serialPort, 115200);  

  font = createFont("Courier", 32); 
  

  /*
  float [] axis = new float[3];
   axis[0] = 0.0;
   axis[1] = 0.0;
   axis[2] = 1.0;
   float angle = PI/2.0;
   
   hq = quatAxisAngle(axis, angle);
   
   hq = new float[4];
   hq[0] = 0.0;
   hq[1] = 0.0;
   hq[2] = 0.0;
   hq[3] = 1.0;
   */

  delay(100);
  myPort.clear();
  myPort.write("1");
}


float decodeFloat(String inString) {
  byte [] inData = new byte[4];

  if (inString.length() == 8) {
    inData[0] = (byte) unhex(inString.substring(0, 2));
    inData[1] = (byte) unhex(inString.substring(2, 4));
    inData[2] = (byte) unhex(inString.substring(4, 6));
    inData[3] = (byte) unhex(inString.substring(6, 8));
  }

  int intbits = (inData[3] << 24) | ((inData[2] & 0xff) << 16) | ((inData[1] & 0xff) << 8) | (inData[0] & 0xff);
  return Float.intBitsToFloat(intbits);
}







void readQ() {
  if (myPort.available() >= 18) {
    String inputString = myPort.readStringUntil('\n');
    //print(inputString);
    if (inputString != null && inputString.length() > 0) {
      String [] inputStringArr = split(inputString, ",");
      if (inputStringArr.length >= 5) { // q1,q2,q3,q4,\r\n so we have 5 elements
        q[0] = decodeFloat(inputStringArr[0]);
        q[1] = decodeFloat(inputStringArr[1]);
        q[2] = decodeFloat(inputStringArr[2]);
        q[3] = decodeFloat(inputStringArr[3]);
      }
    }
  }
}


void buildBoxShape() {
  //box(60, 10, 40);
  noStroke();
  beginShape(QUADS);

  //Z+ (to the drawing area)
  fill(#00ff00);
  vertex(-30, -5, 20);
  vertex(30, -5, 20);
  vertex(30, 5, 20);
  vertex(-30, 5, 20);

  //Z-
  fill(#0000ff);
  vertex(-30, -5, -20);
  vertex(30, -5, -20);
  vertex(30, 5, -20);
  vertex(-30, 5, -20);

  //X-
  fill(#ff0000);
  vertex(-30, -5, -20);
  vertex(-30, -5, 20);
  vertex(-30, 5, 20);
  vertex(-30, 5, -20);

  //X+
  fill(#ffff00);
  vertex(30, -5, -20);
  vertex(30, -5, 20);
  vertex(30, 5, 20);
  vertex(30, 5, -20);

  //Y-
  fill(#ff00ff);
  vertex(-30, -5, -20);
  vertex(30, -5, -20);
  vertex(30, -5, 20);
  vertex(-30, -5, 20);

  //Y+
  fill(#00ffff);
  vertex(-30, 5, -20);
  vertex(30, 5, -20);
  vertex(30, 5, 20);
  vertex(-30, 5, 20);

  endShape();
}


void drawCube() {  
  pushMatrix();
  translate(VIEW_SIZE_X/2, VIEW_SIZE_Y/2 + 50, 0);
  scale(5, 5, 5);

  // a demonstration of the following is at 
  // http://www.varesano.net/blog/fabio/ahrs-sensor-fusion-orientation-filter-3d-graphical-rotating-cube
  rotateZ(-Euler[2]);
  rotateX(-Euler[1]);
  rotateY(-Euler[0]);

  buildBoxShape();

  popMatrix();
}


void draw() {
  background(#000000);
  fill(#ffffff);

  readQ();

  if (hq != null) { // use home quaternion
    quaternionToEuler(quatProd(hq, q), Euler);
    text("Disable home position by pressing \"n\"", 20, VIEW_SIZE_Y - 30);
  }
  else {
    quaternionToEuler(q, Euler);
    text("Point FreeIMU's X axis to your monitor then press \"h\"", 20, VIEW_SIZE_Y - 30);
  }

  textFont(font, 20);
  textAlign(LEFT, TOP);
  text("Q:\n" + q[0] + "\n" + q[1] + "\n" + q[2] + "\n" + q[3], 20, 20);
  text("Euler Angles:\nYaw (psi)  : " + degrees(Euler[0]) + "\nPitch (theta): " + degrees(Euler[1]) + "\nRoll (phi)  : " + degrees(Euler[2]), 200, 20);

  drawCube();
}


void keyPressed() {
  if (key == 'h') {
    println("pressed h");

    // set hq the home quaternion as the quatnion conjugate coming from the sensor fusion
    hq = quatConjugate(q);
  }
  else if (key == 'n') {
    println("pressed n");
    hq = null;
  }
}

// See Sebastian O.H. Madwick report 
// "An efficient orientation filter for inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation

void quaternionToEuler(float [] q, float [] euler) {
  euler[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi
  euler[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
  euler[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi
}

float [] quatProd(float [] a, float [] b) {
  float [] q = new float[4];

  q[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
  q[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
  q[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
  q[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];

  return q;
}

// returns a quaternion from an axis angle representation
float [] quatAxisAngle(float [] axis, float angle) {
  float [] q = new float[4];

  float halfAngle = angle / 2.0;
  float sinHalfAngle = sin(halfAngle);
  q[0] = cos(halfAngle);
  q[1] = -axis[0] * sinHalfAngle;
  q[2] = -axis[1] * sinHalfAngle;
  q[3] = -axis[2] * sinHalfAngle;

  return q;
}

// return the quaternion conjugate of quat
float [] quatConjugate(float [] quat) {
  float [] conj = new float[4];

  conj[0] = quat[0];
  conj[1] = -quat[1];
  conj[2] = -quat[2];
  conj[3] = -quat[3];

  return conj;
}

My question is how i would modify the code to move the motor in relation to the gyros position? I am not expecting someone to write anything for me but a pointer or an example that i missed for reference would be nice. I am on a timeline for this project so any help would be appreciated.

Thanks in advance.

2

mjf46:
to move the motor in relation to the gyros position?

You will have to explain what that means.

What range of numbers do you get from the Gyro?
What should the stepper do in response to those numbers ?

...R
Stepper Motor Basics

3

Thank you Robin, The gyro that i have on the board is a ITG-3200, and the Accelerometer is a ADXL345.

This is a screenshot of the values that i am getting: (sorry i am trouble uploading images directly to the forum with the img tags.

I am getting pitch, roll, and yaw values which i believe have the units degrees I am also getting Q values which are thought to be acceleration measured in G's.

I plan on using only one axis (pitch). The initial position of my object will start at 0 degrees. As the object navigates through a course and reaches either a hill or a drop the angle will change to as much as +/- 15 degrees. I want to keep the platform self leveling between these values.

4

mjf46:
I plan on using only one axis (pitch). The initial position of my object will start at 0 degrees. As the object navigates through a course and reaches either a hill or a drop the angle will change to as much as +/- 15 degrees. I want to keep the platform self leveling between these values.

That suggests to me that you might be getting values between (say) 0 and 30.

Suppose the platform is level at 0 (ignore how that might have happened, for the moment) how many motor steps would be needed to move it so it would be level at 30.

Is the gyro attached to the platform or to the chassis ?
In other words are you trying to keep the gyro at a fixed angle or are yo trying to keep the platform at a constant angle even though the gyro angle changes ?

...R

5

My stepper motor has 200 steps total so 1.8 degrees per step. To obtain 30 degrees of rotation it would be close to say 17 steps (rotate one direction 8.5 steps and the opposite direction 8.5 steps). The gyro will be attached to a rolling chassis which the platform is mounted to. I need to keep the platform at say 0 degrees as it navigates through the course. The maximum downhill and uphill angle will be 15 degrees. As it climbs a hill the motor will need to rotate forward to keep the platform level and when the chassis goes downhill it will need to rotate backwards to keep the platform level. Please let me know if a diagram would help you understand more. Thanks for your help.

6

Robin, Sorry also forgot to mention that i am running a 4 wire bipolar stepper motor

7

mjf46:
The gyro will be attached to a rolling chassis which the platform is mounted to. I need to keep the platform at say 0 degrees as it navigates through the course.

It is hard to be sure what will work without some experiments - which I can't do.

My inclination (sorry for the pun) would be to put the device on level ground with the platform level and read the Gyro value. Then for every inclination of the gyro away from level, move the stepper in the appropriate direction.

You will probably need some process to prevent the system from hunting - perhaps a suitable wait period between readings and movements. I say "wait" because you should not use the delay() function. Use millis() for timing as illustrated in several things at a time and planning and implementing a program.

Write some code and do some experiments. The Arduino is a great system for learning-by-doing.

...R

8

Thats kind of what i have been doing. I have the 6DOF combo board that i mentioned above. While browsing i saw old style board that had 6 outputs (3 for the gyro and 3 for the accelerometer) were able to be set up on individual pins. I found an example on how to code if i had the older board. Currently on the newer board the SDA and SCL take the place of those 6 outputs. I need to have the motor and the gyro talking to each other with the same code which i have not been able to find an example yet. Have you heard of an example similar to mine? I have been searching for the last couple of days to find something close to learn from. I cannot figure out how i would take the outputs from a single axis on the gyro to send to the stepper. I did figure out how to limit the stepper to 15 degrees in the clockwise direction and 15 degrees in the counterclockwise direction and i can also control the speed.

9

mjf46:
I need to have the motor and the gyro talking to each other with the same code

I don't think that is a useful way of thinking about the problem.

Think, instead, of having one function that reads the gyro and saves the value in global variable. And a second function that controls the stepper based on that saved value. The code in loop() could be as simple as

void loop() {
   readGyro();
   moveStepper();
}

The saved data is the link between the two components.

You can write short test programs that each have only one of the functions. For the stepper test code you can just have some typical values in an array. When you get the two parts working separately it will be time to join them together.

I'm guessing you will need to do some experimenting to figure out how to get it to work properly so make sure the code in each of the functions is a simple and clear as possible with scope for printing out values for debugging purposes.

...R

10

Robin, This is what i have so far. I took your advice to try and print the values from the gyro. I am only interested in those about one axis. I am getting this program to compile and run, but its not printing the values (at least from what i can see).

  // Include the wire I2C library 
  #include <Wire.h>;
int GYRO_XOUT_H=29;   // assign to register 29
int GYRO_XOUT_L=30;   // assign to register 30


void setup() {
// Start the serial communication at baud rate of 9600
 Serial.begin(9600);
//Create a Wire object
Wire.begin();
}

void loop() 
{
 // Send a request to the slave 
 Wire.beginTransmission(GYRO_XOUT_H);
 Wire.beginTransmission(GYRO_XOUT_L);
// Send a bit to look at register that contains the stored data
Wire.write(29); // This is believed to be the register that contains the data 
Wire.write(30);
// End the Transmission 
Wire.endTransmission();
// Request 1 byte from the specified address
Wire.requestFrom(GYRO_XOUT_H,1);
Wire.requestFrom(GYRO_XOUT_L,1);
// Now we are waiting on a respons from the slave 
while(Wire.available()==0);
//Get the values and read it into a variable 
int X = Wire.read();
int Y = Wire.read();
// Sent the values to serial monitor
Serial.print(X);
Serial.print("X");  //(This is a unit);
Serial.print(Y);
Serial.println("Y");  //(This is a unit);
 delay(300);
}
11 
 Wire.beginTransmission(GYRO_XOUT_H);
 Wire.beginTransmission(GYRO_XOUT_L);

Can you explain, in English, what you think you are doing here? The beginTransmission() method defines the address of the device you want to talk to. Do you really have two gyros?

Wire.requestFrom(GYRO_XOUT_H,1);
Wire.requestFrom(GYRO_XOUT_L,1);
// Now we are waiting on a respons from the slave 
while(Wire.available()==0);
//Get the values and read it into a variable 
int X = Wire.read();
int Y = Wire.read();

You need to ask for one byte, wait for that byte, read the byte, and repeat for the other byte.

12

As @PaulS says, you seem to have your Wire,read()s hopelessly mixed up.

You seem to be asking for a value from GYRO_XOUT_H and then immediately overwriting it with a value from GYRO_XOUT_L

Then you are checking if there is >0 bytes available and trying to read 2 bytes even though there may only be one byte. And where might the second byte have come from ?

I'm not familar with either the Wire library or your Gyro device so please explain what you are trying to do.

...R

13

From what i understand by this page: SparkFun: ITG-3200,Triple-Axis Gyro - eLinux.org

the sensor values are stored in 2 registers for each axis. So for the X axis i used Gyro_Xout_H and Gyro_Xout_L. I used the registers called out in the datasheet (page27).

Datasheet for the ITG-3200:

I am attempting to print values from this axis so that they can be used to control my stepper motor.
Thanks for the continued help.

14

From what i understand by this page...the sensor values are stored in 2 registers for each axis.

But on ONE device. So, you set up to talk to the ONE device ONE time.

15

Thanks Paul,
I think i did what you are asking for:

  // Include the wire I2C library 
  #include <Wire.h>;
int GYRO_XOUT_H=29;   // assign to register 29
int GYRO_XOUT_L=30;   // assign to register 30


void setup() {
// Start the serial communication at baud rate of 9600
 Serial.begin(9600);
//Create a Wire object
Wire.begin();
}

void loop() 
{
 // Send a request to the slave 
Wire.beginTransmission(GYRO_XOUT_H); // Send a bit to look at register that contains the stored data
Wire.write(29); // This is believed to be the register that contains the data 
Wire.endTransmission();// End the Transmission 
Wire.requestFrom(GYRO_XOUT_H,1); // Request 1 byte from the specified address
while(Wire.available()==0); // Now we are waiting on a respons from the slave 
int X = Wire.read(); //Get the values and read it into a variable
Serial.print(X); // Sent the values to serial monitor
Serial.print("X");  


Wire.beginTransmission(GYRO_XOUT_L);
Wire.write(30);
Wire.endTransmission();
Wire.requestFrom(GYRO_XOUT_L,1); 
while(Wire.available()==0);
int Y= Wire.read();
Serial.print(Y);
Serial.print("Y"); 
 delay(300);
}
16

I think i did what you are asking for:

No.

Wire.beginTransmission(GYRO_XOUT_H);
Wire.beginTransmission(GYRO_XOUT_L);

Which ONE device is this?

Have you run the I2C scanner to see what the correct address OF THE DEVICE is? That is NOT the same as the address of the data ON THE DEVICE.

17

You have code in your Original Post that, you say, tests the Gyro successfully.
I can nothing like that in the code in Replies #9 or #14.

On what document have you based the code in those Replies? - post a link to it please

Have you looked at this driver ?
(I just found it, but I know nothing about it)

...R