SOLVED - Need help with DIY Segway project

Hi,
I'm building a DIY Segway using a:

  1. RoboClaw 2x15A Motor Controller to drive 2 brush DC motors (http://www.orionrobotics.com/RoboClaw-2x15A-Motor-Controller_p_268.html),
  2. “IMU Digital Combo Board - 6 Degrees of Freedom ITG3200/ADXL345” to get gyro & accelerometer inputs (SparkFun 6 Degrees of Freedom IMU Digital Combo Board - ITG3200/ADXL345 - SEN-10121 - SparkFun Electronics),
  3. and a Arduino Uno board.

When I combine both components in a simple test program (see below), the program works for a few loop and then goes into an uncontrolled state in which the Motor Controller still drives the motors (at undefined speeds) and has both status LED on green (data transmission and power), but no red error LED light on the RoboClaw board.

When I just control the IMU board all is fine, same when I just have the motor controller running. Problems start when I trie to operate both at the same time.

I've experimented with different program loop times (50, 100, 200, and 300ms) and the results are pretty much the same.

Please see the program code:

#include "BMSerial.h"
#include "RoboClaw.h"
#include <arduino.h>
#include <FreeSixIMU.h>        // IMU_Digital_Combo_Board_6_Degrees include
#include <FIMU_ADXL345.h>      // IMU_Digital_Combo_Board_6_Degrees include  
#include <FIMU_ITG3200.h>      // IMU_Digital_Combo_Board_6_Degrees include
#include <Wire.h>              // IMU_Digital_Combo_Board_6_Degrees include

#define address 0x80           // Robo Claw address.
     // RoboClaw is set to Mode 7 Packet Serial Mode - Address 0x80 , 9600 baud

bool valid;						// Robo Claw status variable
float imu_q[4]; 				// IMU q values
FreeSixIMU my3IMU=FreeSixIMU();	// Set the FreeIMU object, connected to A0 and A1
RoboClaw roboclaw(5, 6);       // Arduino Pin5 (Receive) to RobClaw S2 (Transmit)
                                // Arduino Pin6 (Transmit) to RobClaw S1 (Receive)
                                // Arduino GND to GND pin on S1
void setup() {
  init();                       // initialize the Arduino Uno
  roboclaw.begin(38400);         // initialize the RoboClaw serial communication
  delay(100);
  Serial.begin(57600);		// initialize the terminal serial communication link
  delay(100);
  Wire.begin();                 // initialize com link to IMU board
  delay(100);
  my3IMU.init();                // initialize the IMU board
}

void loop() {
  uint8_t i, ctr=1;
  unsigned long loop_time=100, execute_time, start_millis;

  while(ctr) {  
    for (i = 27; i < 128; i+=20) {
      start_millis = millis();        // initialize loop timer
      my3IMU.getQ(imu_q);		    // get the IMU sensor data
      roboclaw.ForwardM1(address, i); //start Motor1 forward at speed i
      roboclaw.ForwardM2(address, i); //start Motor2 forward at speed i
      execute_time = millis() - start_millis;
      Serial.print(F("Motor Speed: "));
      Serial.print(i);
      Serial.print(F("\tExec Time: "));
      Serial.print(execute_time);
      Serial.print(F("\tClock: "));
      Serial.print(millis());
      Serial.print(F("\nIMU_Q: "));
      Serial.print(imu_q[0], 5);
      Serial.print(F("\tIMU_Q: "));
      Serial.print(imu_q[1], 5);
      Serial.print(F("\tIMU_Q: "));
      Serial.print(imu_q[2], 5);
      Serial.print(F("\tIMU_Q: "));
      Serial.print(imu_q[3], 5);
      Serial.print(F("\nBattery Power: "));
      Serial.print((float)(roboclaw.ReadMainBatteryVoltage(address, &valid) / 10.0), 1);
      Serial.println(" ");
      execute_time = millis() - start_millis;
      if (loop_time > execute_time) {
        delay(loop_time-execute_time);  // don't process more than once every "loop_time" milli-seconds
      }
    }
    Serial.print(F("\n>>>>>>> End of loop "));
    Serial.print(ctr);
    Serial.print(F(" with loop-time "));
    Serial.print(loop_time);
    Serial.println(F(" <<<<<<<\n"));
    roboclaw.ForwardM1(address, i/2); //slow down Motor1
    roboclaw.ForwardM2(address, i/2); //slow down Motor2
    delay(200);
    roboclaw.ForwardM1(address, i/3); //slow down Motor1
    roboclaw.ForwardM2(address, i/3); //slow down Motor2
    delay(200);
    roboclaw.ForwardM1(address, 0);   //stop Motor1
    roboclaw.ForwardM2(address, 0);   //stop Motor2
    delay(2000);
    ctr++;
  }
}

Please see attached the screenshot from the Serial Monitor.

Your help will be greatly appreciated.

Volker

How is the speed undefined?
You tell it to go from 27 to 127:

for (i = 27; i < 128; i+=20) {
start_millis = millis(); // initialize loop timer
my3IMU.getQ(imu_q); // get the IMU sensor data
roboclaw.ForwardM1(address, i); //start Motor1 forward at speed i
roboclaw.ForwardM2(address, i); //start Motor2 forward at speed i
execute_time = millis() - start_millis;
Serial.print(F("Motor Speed: "));
Serial.print(i);

I don't see how you ever get out of the while loop, as ctr starts at 1, it is only incremented, so the while is always true?

Thanks CrossRoads for your interest / help.

Your're correct, the program should stay forever [or at least until (int) ctr overflows] in the outmost while loop. However, after <30 iterations thru this while loop the program stops to send output to the Serial Terminal Monitor but the motors continue to run at some (random) speed.

It seems as if the Arduino hangs up......
Arduino does not appear to run out of memory, still plenty available.

No idea if there could be a conflict between the I2C communication {that controls the IMU-6DOF board via Arduino pins A4 (to SDA) & A5 (to SCL)} and the serial communication using the wire library {that controls the RoboClaw board via Arduino digital pins 5 & 6}. I've tried pins 10 & 11 with same negative result.

Add some more serial.prints, see what the last thing it actually reports is, go from there.

"No idea if there could be a conflict between the I2C communication {that controls the IMU-6DOF board via Arduino pins A4 (to SDA) & A5 (to SCL)} and the serial communication using the wire library {that controls the RoboClaw board via Arduino digital pins 5 & 6}. I've tried pins 10 & 11 with same negative result."

Uses softwareWire for roboclaw? Instead of the real I2C?

Thanks CrossRoads.
What / where would you suggest additional Serial.print(s) for debugging?

Also, I just found out that if I turn off the main power to the system (powers RoboClaw logic plus the motors) and keep the Arduino connected to the USB (and hence powered up thru the USB), the Arduino program resumes normal operation and loop thru the while loop - strange.....).

Not sure what you meant by "Uses softwareWire for roboclaw? Instead of the real I2C?"
The RoboClaw motor controller is driven using the BMSerial / RoboClaw libraries.
The IMU-6DOF board is driven using the wire I2C-communication library .

Volker

I re-wrote the test program to eliminate the need for the RoboClaw / BMSerial libraries and used the SoftwareSerial (NewSerial) library instead. However, no luck in solving the problem. Everything still the same. But the program is easier to read. Memory status: 17,936 out of 32,256 and 1,242 SRAM remaining during execution (out of 2,048). Here we go:

#include <arduino.h>
#include <inttypes.h>
#include <FreeSixIMU.h>         // IMU_Digital_Combo_Board_6_Degrees include
#include <Wire.h>               // IMU_Digital_Combo_Board_6_Degrees include
#include <SoftwareSerial.h>     // use SoftwareSerial library to communicate with RoboClaw

#define RX 5		        // Arduino data receiving pin
#define TX 6		        // Arduino data transmission pin
#define MAX_BWD -127            // maximum backward speed
#define MAX_FWD +127            // maximum forward speed
#define GETMBATT 24	        // Robo Claw command code to read main battery voltage
#define address 0x80            // Robo Claw address.
     // RoboClaw is set to Mode 7 Packet Serial Mode - Address 0x80 , 9600 baud

bool valid;		        // Robo Claw status variable
float angles[3]; 	        // Euler angles Yaw, Pitch, Roll
FreeSixIMU sixDOF=FreeSixIMU();	// Set the FreeIMU object, connected to A0 and A1
SoftwareSerial roboclaw(RX,TX);	// create SoftwareSerial port with receive / transmit pins
                                // Arduino Pin5 (RX - Receive) to RobClaw S2 (Transmit)
                                // Arduino Pin6 (TX -Transmit) to RobClaw S1 (Receive)
                                // Arduino GND to GND pin on S1
void setup() {
  init();                       // initialize the Arduino Uno
  Serial.begin(57600);		// initialize the terminal serial communication link
  while (!Serial) {
    delay(5);                   // wait for serial port to connect. Needed for Leonardo only
  }
  Wire.begin();                 // initialize com link to IMU board
  delay(100);
  sixDOF.init();                // initialize the IMU board
  delay(100);
  roboclaw.begin(9600);        // set communication speed for the SoftwareSerial link to RoboClaw board
  delay(100);
}

void loop() {
  uint8_t i, ctr=1;
  unsigned long loop_time=200, execute_time, start_millis;

  while(ctr) {  
    for (i = 27; i < 128; i+=20) {
      start_millis = millis();        // initialize loop timer
      sixDOF.getEuler(angles);	      // get the Euler angles from the IMU sensor
      Drive_Motors(0, i);             // set speed for motor 0 (Left)
      Drive_Motors(1, i);             // set speed for motor 1 (Right)
      execute_time = millis() - start_millis;
      Serial.print("Motor Speed: ");
      Serial.print(i);
      Serial.print("\tExec Time: ");
      Serial.print(execute_time);
      Serial.print("\tClock: ");
      Serial.print(millis());
      Serial.print("\tFree Memory: ");
      Serial.print(RAM_status());
      Serial.print("\nYaw: ");
      Serial.print(angles[0], 5);
      Serial.print("\tPitch: ");
      Serial.print(angles[1], 5);
      Serial.print("\tRoll: ");
      Serial.print(angles[2], 5);
      Serial.println(" ");
      execute_time = millis() - start_millis;
      if (loop_time > execute_time) {
        delay(loop_time-execute_time);  // don't process more than once every "loop_time" milli-seconds
      }
    }
    Serial.print(F("\n>>>>>>> End of loop "));
    Serial.print(ctr);
    Serial.print(F(" with loop-time "));
    Serial.print(loop_time);
    Serial.println(F(" <<<<<<<\n"));
    Drive_Motors(0, i/2); //slow down Motor1
    Drive_Motors(1, i/2); //slow down Motor2
    delay(200);
    Drive_Motors(0, i/3); //slow down Motor1
    Drive_Motors(1, i/3); //slow down Motor2
    delay(200);
    Drive_Motors(0, 0); //Motor1 off
    Drive_Motors(1, 0); //Motor2 off
    delay(2000);
    ctr++;
  }
}

/*************************************************************
 * Drive DC Motors using the RoboClaw board
 * adopted from the original RoboClaw library
 *
 * due to hardware mounting both motors are rotating opposite 
 * of each other.  Therefore we reverse the speed of the left motor 
 *
 * Inputs:  motor (0 or 1 for left or right motor)
 *          motor_speed:  -127 (full reverse)
 *                           0 (stop)
 *                        +127 (full forward)
 *
 * Return:  none
 *************************************************************/
void Drive_Motors(uint8_t motor, int motor_speed) {
  uint8_t mspeed, dir, motor_command, crc;

  if (motor_speed >= 0) {
    dir = 0;
    if (motor_speed > MAX_FWD) {
      motor_speed = MAX_FWD;
    }
    mspeed = (uint8_t)motor_speed;
  } 
  else {
    dir = 1;
    if (motor_speed < MAX_BWD) {
      motor_speed = MAX_BWD;
    }
    mspeed = (uint8_t)(-motor_speed);
  }
  if (motor == 0)
    dir = dir ^ 1;          // reverse dir to adjust for hardware config (motor wiring)    
  motor_command = motor * 4 + dir;
  crc = address + motor_command + mspeed;
  roboclaw.write(address);
  roboclaw.write(motor_command);
  roboclaw.write(mspeed);
  roboclaw.write(crc&0x7F);
}

int RAM_status(void) {
  extern int __heap_start, *__brkval; 
  int v; 
  return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval); 
}

I hope there is someone out there that can help me to resolve this issue.

Volker

I guess I was just a little confused over your terminology in the notes.

Looking back again to the original:
#define address 0x80 // Robo Claw address.
// RoboClaw is set to Mode 7 Packet Serial Mode - Address 0x80 , 9600 baud
yet:
roboclaw.begin(38400); // initialize the RoboClaw serial communication

Maybe a mismatch there?

You seem to have changed it here to match:

#define address 0x80 // Robo Claw address.
// RoboClaw is set to Mode 7 Packet Serial Mode - Address 0x80 , 9600 baud
and
roboclaw.begin(9600); // set communication speed for the SoftwareSerial link to RoboClaw board

You have an Uno you said? Probably don't need this then:
while (!Serial) {
delay(5); // wait for serial port to connect. Needed for Leonardo only
}

What is your grounding scheme like? If you're controlling lots of current, that may be causing issues with grounding and creating upsets.

Sorry for the typos, I tried various SoftwareSerial baud speeds. Arduino will only communicate with the RoboClaw if the baud rate is correctly set on the RoboClaw board and in the Arduino software. So sorry, that's not the issue.

In terms of the grounding scheme, please see the attached schematic. As you can see the Arduino and RoboClaw boards share the same power supply GND from Battery II. The Arduino is on 12V (+ terminal of Battery II), the RoboClaw on +24V (+ terminal of Battery I, Battery II and Battery I are in series). Any suggestions on how to follow up on your GND question and potential for upsets?

On another note, I reviewed the verbose Arduino compiler output and found a few warnings in std Arduino library code (see one example warning msg below). Should I worry about that?

C:\Program Files\Arduino\hardware\arduino\cores\arduino\HardwareSerial.cpp:467: warning: comparison between signed and unsigned integer expressions

Thanks again for your help!
Volker

Is there a difference between 24V- and S1 Gnd? Can you confrm they are connected at the motor bridge and not just being connected thru the Arduino?

The RoboClaw motorbridge is powered by 24V.
The Arduino is powered by 12V.
They share a common ground with the RoboClaw and IMU_6DOF logic circuits. The RoboClaw logic circuit is also connected to +5V Arduino Pin. The IMU_6DOF is connected to Arduino +3.3V.

|----- GND Arduino +12V---------|
| |
|---GND Bat 2 +12V Bat 2 ---|--- GND Bat 1 +12V Bat 1 ---|
| |
|------GND RoboClaw +24V---------------------------------------|
|
|------GND S1 RoboClaw logic ciruit
|------GND S2 RoboClaw logic ciruit
|
|------GND IMU-6DOF

I hope this drawing makes sense.

I have never used two serial ports operating at different baud rates, but I did notice that in the SoftwareSerial library examples, the code uses the "listen" function to explicitly select a port. Not sure if this would help, but I also came across another serial library while looking through forums of similar systems:

http://forum.arduino.cc/index.php?topic=54544.0
library link: NewSoftSerial | Arduiniana

May be capable of reading multiple baud rates. Seems to me it's something to do with the multiple serial connections because of this statement:

"Also, I just found out that if I turn off the main power to the system (powers RoboClaw logic plus the motors) and keep the Arduino connected to the USB (and hence powered up thru the USB), the Arduino program resumes normal operation and loop thru the while loop - strange.....)."

my theory: When the power source is disconnected from your roboclaw controller and you lose you serial connection, you no longer are getting conflicting serial messages, and your system works.

My overall suggestion would be to make an message pitch and catch between your serial connections and make sure you aren't crossing your messages within the code (and after loop iterations) before moving onto hardware, components, and wiring.

Thanks for all the help so far.
Made a good step forward this morning. When I replace all motor speed change statements with a speed of 0, the program behaves as designed. Thus, I have a hardware problem with some kind of interference coming into the logic circuit negatively effecting the Arduino operations.
Next step is to further isolate the motor power from the logic power circuit.
I'll keep you updated on my progress.

Spoke with the folks at Orion (RoboClaw manufacturer) and determined that the RoboClaw conflicts with the interupts being used by Serial.print().

Next step for me is to find a way to find a work-around.

After many detours I finally found a solution to the problem. It came down to some hardware interference that I resolved by:

  1. moving the RoboClaw motor controller about 10" away from the Arduino board
  2. installing ferrit cores around both motor cables and the main power cable to the RoboClaw motor controller

After doing this the original software operates stable.

PS: I was never able to identify the interference with an Oscilloscope.

Now I can move on and optimize the PID control algorithm parameters so that the Segway can balance itself.

Arduino: 1.5.7 (Windows 7), Board: "Arduino Uno"

hartway_digital.ino:25:20: fatal error: I2Cdev.h: No such file or directory
compilation terminated.

This report would have more information with
"Show verbose output during compilation"
enabled in File > Preferences.

I have similar "hang up" problems in my segway project.

Wenn you say ferrite core on motor cables and Roboclaw power supply, how do you run them trough the ferrit core?

  • As a pair(+ and - ) straight trough?
  • as a pair looped around? once, twice?

i would appreciate your help, thanks