The IMU is a small PCB that contains a gyroscope and accelerometer, each measuring a different part of the angle.

With the angle measurement from the IMU, the Arduino can determine how fast and in which direction to turn the two motors. The easiest angle approximation (and Arduino coding) is done using a −90 degree to +90 degree scale, where 0 degrees is considered "level" (see Figure 11-2). If the angle measured from the IMU is 0 degrees, the motors stops; above 0 degrees and the motors go proportionally forward; and below 0 degrees and the motors go proportionally reverse. This behavior keeps the Seg-bot level and enables you to control its speed and direction by leaning front to back.

Figure 11.2. The IMU board measuring 0 degrees (flat on table)

To steer this vehicle, a potentiometer mounted on the handlebar is turned from left to right while moving. If the dial is centered, the motors receive equal power and drive straight; if turned left or right, the max speeds of each motor is oppositely affected, causing the Seg-bot to turn.

There is also a gain potentiometer near the handlebar to adjust the responsiveness or sensitivity of the motors. A higher gain yields faster motor response to angle changes making the Seg-bot much more sensitive (a tighter feel, when riding), whereas a lower gain can result in a slower motor response to angle change (a mushy feel when riding, but more forgiving).

Also installed is a button switch that must be pressed to operate the bot. This switch is a large, red button, handily placed where your left thumb sits so that if you fall off the Seg-bot or for any other reason, let go of the handlebar, it immediately stops the motors. There is also an angle limit in the code that kills the motors if tilted too far in either direction. These failsafes keep the Seg-bot rider relatively safe, as long as the rider understands how to operate the bot.

Now that you know the main features of the Seg-bot, take a look at the parts list.

As component parts get smaller, 3.3v devices are becoming more popular. Luckily, the FTDI USB interface chip on the Arduino has a small 3.3v power supply of around 50mA, and the guys who designed the Arduino provided a breakout pin for us to use this 3.3v source. The Razor 6 DOF needs a clean 3.3v power source, but it consumes only a few milliamps of current, so you can power it from the Arduino 3.3v power supply pin. Because the maximum voltage in the chip is 3.3v, you would be right to assume that the analog output voltage for each sensor is also be between 0–3.3v (rather than the usual 0–5v).

The normal range for the Arduinos 10-bit Analog to Digital Converters (pins A0-A5) is 0–1023, where a 0v input yields a value of 0, and a 5v input yields a value of 1023. If you simply plug a 3.3v sensor into an Arduino analog input, the maximum value that the Arduino can read from the sensor is approximately 675 or so (3.3v / 5v * 1023). To get the full range using a 3.3v device, you must connect the desired reference voltage (3.3v) to the Analog reference (Aref) pin on the Arduino and add a line of code to the end of the setup() function to tell the Arduino to use an EXTERNAL analog reference. (That is, use whatever voltage is connected to the Aref pin.) This changes only the reference voltage of the Analog input pins (A0-A5); it does not change any of the other input or output pins, which still produce 5v signals.

To change the analog reference voltage, you need to introduce a new Arduino command;

analogReference(EXTERNAL); // add this line to the bottom of the setup() function of your
sketch to use an external analog reference voltage.

After you tell the Arduino to look at the Aref pin for the analog voltage reference, don't forget to then connect the 3.3v pin to the Aref pin! For more information about the analogReference() command, visit the following Arduino reference page: http://www.arduino.cc/en/Reference/AnalogReference.

With the board powered and the Arduino reading the proper voltages from the analog inputs, now take a look at the individual sensors that you must interface from the IMU.

The accelerometer measures the gravitational force of the IMU relative to the horizon. The accelerometer on the Razor 6DOF can measure the tilt of the IMU to approximately 90 degrees in either direction. To equal 0 degrees (or "level"), the IMU must be parallel to the horizon (see Figure 11-2). If the IMU board is tilted left or right (see Figures 11-4 and 11-5), the angle measurement yields a proportional value in either direction.

Although the accelerometer is capable of measuring tilt of approximately 90 degrees in either direction, you measure up to approximately only 25–30 degrees in either direction for the Seg-bot. Any more tilt than this would result in a hazardous riding condition, so the code is set to shut down the motors in the event that the IMU measures an angle above the limit.

Figure 11.4. The IMU rotating along its X-axis at −25 degrees (reverse)

Figure 11.5. The IMU rotating along its X-axis at 45 degrees (forward)

Ideally, this sensor can read a steady value proportional to the angle of the board. Using the 10-bit value range of the Analog inputs on the Arduino (0–1023), a "level" IMU can read a center value of 511. If you tilt the IMU board 90-degrees forward (along its X-axis), the value should increase up to 1023, whereas tilting it 90-degrees backward yields a 0 value. The notable feature of the accelerometer is that it can hold its value when tilted; you can test your accelerometer with the code in Listing 11-1.

// Arduino - Analog 3.3v Accelerometer
// Connect accelerometer output into Arduino analog pin 0
// Upload, then open Serial monitor at 9600bps to see accelerometer value

int angle = 0; // variable used to hold the "rough angle" approximation

void setup(){

  Serial.begin(9600); // start Serial monitor to print values

  analogReference(EXTERNAL); // tell Arduino to use the voltage connected to the Aref pin for
analog reference (3.3v) - remove this command if you are using a 5v sensor.

}

void loop(){

  angle = analogRead(0);  // read the accelerometer from Analog pin 0

  Serial.print("Accelerometer:  ");  // print the word "Accelerometer: " first
  Serial.println(angle);  // then print the value of the variable "angle"

  delay(50);    // update 20 times per second (1000 mS / 50 mS = 20)

}

At first glance, it seems like you could use an accelerometer alone to detect the angle of the IMU board. These sensors are used (by themselves) to detect changes in angular movement for cell phones, laptops, gaming consoles, and many other applications. The reason we cannot use an accelerometer only to detect the angle for the Seg-bot is that it's severely affected by gravity. That is, any sudden change in gravity (even vibrations) can affect the output angle of the accelerometer, even if the angle has not changed.

This is most apparent if you hold the accelerometer at a specific angle and gently vibrate your hand up and down. This can drastically change the output readings, so much so that the signal becomes useless without some sort of filtering to weed out the false readings.

Unfortunately, vibrations and bumps are unavoidable when riding the Seg-bot (or any other moving vehicle) and must be dealt with. This is where the gyroscope sensor comes in handy.

The gyroscope measures how fast, or more specifically, how many degrees per second the IMU move at a given moment. This value is also represented as a 0–3.3v analog signal from the IMU, so you must convert the 10-bit analog value into an approximate angle. Unfortunately, the gyroscope is not quite as easy to obtain an angular value from because it can measure only the rate of angle change.

The gyroscope shows a change in voltage while the sensor is moved along its measured axis. Unlike the accelerometer, when you stop moving the gyroscope, the voltage descends back to its center (zero rate) level. To determine how far the gyroscope has rotated knowing only the rate of change, you must also know the time period of the change. To do this, determine the time of each loop cycle and calculate the distance (in degrees) that the gyroscope has rotated given the speed output.

From my testing of these devices individually, I learned that while the accelerometer is excellent for referencing an actual angle by itself without any calculations (except mapping), it is highly prone to erroneous readings in an unstable environment (such as the Seg-bot). When I tried to use the accelerometer only, the test bot would quickly fall over due to spikes in the angular readings causing absurd speed values to be written to the motors.

When trying to use the gyroscope by itself, the angle approximation was much more stable from cycle to cycle with no discernible spikes in the angular value. But this "stable" value would drift away from its resting point such that the bot would stay balanced for a few seconds before slowly falling over. This was because the gyro drift was adding up after each cycle and causing the angle reading to deviate from its actual position. (That is, it thinks it is at 0 degrees, level, when it is actually at 5 degrees and counting...TIMBER!)

To keep the angle calculation from experiencing the negative effects from either sensor, you need to combine the best parts of both signals to create a filtered angle, free from both noise and error.

With both semi-erroneous sensor readings available, you need to combine them to get a stable angle reading. This is commonly done using a Kalman filter, but I found this to be too complicated for my taste-my angle filtering will be done using a type of complementary filter, more commonly referred to as a weighted average. The weighted average is as stable as the gyroscope angle calculation without the drift error and as precise as the accelerometer reading without the spikes from bumps and vibrations.

Many power-chair motors are equipped with electric solenoid brakes that clamp to the rear of the motor shaft and keep it from moving when the power chair is turned off. Although this safety feature helps protect the power-chair operator from rolling down a hill in the event of a low battery, it is not necessary for our Seg-bot and should be removed.

You can usually determine if your motors have an electric brake by the number of wires in the harness (see Figure 11.9). If your motor has a wiring harness with four wires, it probably has an electric brake. The two smaller wires are used to control the brake solenoid, which is not released until the terminals are powered, usually by 12vdc.

Figure 11.9. The wiring harness connector of a motor with an electric solenoid brake installed

To remove the electric brake, you must first locate it under the dust cap on the rear of the motor housing. Remove the two or three screws that hold the dust cap on to the end of the motor (see Figure 11.10).

Figure 11.10. The dust cap on the end of the motor

When the brake is visible, you should see 3–5 bolts holding the brake solenoid into the motor housing. These bolts need to be removed, and the wires leading to the solenoid can be snipped (see Figure 11-11). With the brake solenoid removed, the top of the motor assembly should look similar to the top motor in Figure 11-11, whereas the lower motor is pictured with the brake solenoid still installed.

Figure 11.11. The two motors with dust caps removed-the brake has been removed from the upper motor, while still in place on the lower motor.

In Figure 11-12, you can again see the difference between the two motors; the brake has been removed from the left motor while still installed on the right motor. After both solenoid brakes are removed, you can reinstall the dust covers onto the end of the motor assemblies. The new (modified) motors should weigh a few pounds less after the mechanical surgery. Now the small center contacts from the harness in Figure 11-9 will no longer be active; you need only the two large outside contacts to operate each motor.

Figure 11.12. The two motors standing next to each other, the brake has been removed from the left motor, while still in place on the right motor. The dust caps are on the ground next to the motors.

Alternatively, you could leave the solenoid brakes in place, but you would need another relay interface used to supply 12v to the solenoid brakes of each motor when you turn the main power on. This of course, would draw an extra 500mA – 1A continuously from the main battery supply just to keep the motor brakes disengaged while using—so I recommend just removing them.

Although it is ideal to have the weight of the motors perfectly balanced, so the bot will balance by itself, it is not necessary. The mounting holes on my motors would have made it extremely difficult to position the motors vertically (which would have provided better balance), so I mounted them pointing toward the front of the bot. Because you need to hold the Engage button to activate the motors, I found no need to make the bot balance without me being on it. When you stand on the bot, it is easy to keep it balanced because your weight is mostly at the rear of the frame. You can still make the Seg-bot balance by itself if you like, by adding a small counterweight to the rear of the frame until it balances by itself without moving forward or backward.

Now that the motors are prepared for use, select a motor controller to supply them power.

The Arduino SoftwareSerial library enables you to create a separate (virtual) Serial communication line on any Arduino I/O pin. With a max speed of 9600bps, SoftwareSerial is suitable for sending commands from the Arduino to the Sabertooth motor controller. The Sabertooth does not send any information back to the Arduino, so you only need a Serial transmit pin (Tx); although you set up both (Listing 11-2).

#include <SoftwareSerial.h> // Tell Arduino to use the SoftwareSerial library

// define the transmit and receive pins to use
#define rxPin 2
#define txPin 3

// set up a new serial port to use pins 2 and 3 above
SoftwareSerial mySerial =  SoftwareSerial(rxPin, txPin);

void setup()  {

  Serial.begin(9600); // start the standard Serial monitor

  // define pin modes for SoftwareSerial tx, rx pins:
  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);

  // set the data rate for the new SoftwareSerial port
  mySerial.begin(9600);
}

void loop() {

mySerial.print(0, BYTE); // this will print through pin 3 to the Sabertooth
Serial.print("Hello"); // this will print through pin 1 to your computer Serial monitor

delay(100);


}

For more information regarding the uses and limitations of the SoftwareSerial library, please visit the Arduino reference page at www.arduino.cc/en/Reference/SoftwareSerial.

Now that you have the Arduino talking to the Sabertooth, consider what it needs to say to the Sabertooth to make the motors move.

The Sabertooth 2×25 motor controller is programmed to accept a simple Serial protocol that enables forward and reverse speed control of both motors. The Sabertooth looks for a Serial value between 0 and 255 in the Byte value range. Two motors are controlled by the Sabertooth and each has to go two directions, so the 255 number range must be broken up into four parts (2 motor × 2 directions = 4). The byte "0" is a full Stop command and can be used to simultaneously command both motors to the stop position.

Bytes 1–127 are used to control Motor1 and bytes 128–255 are used to control Motor2. These two value ranges are each broken up into forward and reverse, with the center positions (64 and 192) being neutral for each motor (see Figure 11-15). There are 64 speed control steps in either direction for either motor, providing adequate resolution and smooth acceleration.

Figure 11.15. I made this graph to better illustrate the Simplified Serial input method of the Sabertooth motor controller.

The code to control the motors focus on converting the angle value into a speed value centered around the stop points for each motor. To do this, use the Arduino map() command, which takes one value range and stretches it across another.

Now select some batteries that have enough power to drive the Seg-bot to the mailbox and back...or maybe to the coffee shop.

Lead-acid batteries are the most efficient for the price where weight is not a primary concern. However, if you plan to make a portable or lightweight scooter, you might consider using NiMH or LiPo batteries (at a higher price). Using two 12v, 12AH SLA batteries (see Figure 11-16), the Seg-bot can drive for an hour or more continuously on flat terrain. Remember that driving on hilly terrain draws more current from the batteries, which can drain them quicker.

Figure 11.16. These two 12v 12AH Werker brand batteries provide plenty of run time for the Seg-bot.

I chose to use two 12v SLA batteries rated at 12AH each, wired in series to produce 24v and 12AH. These batteries are readily available online and in battery stores, measuring 6-inch L × 4-inch W × 3.8-inch H and weighing 10lbs each. You can use a higher capacity battery if you like; my original thought was to use 18AH batteries, but they would not fit in my frame design. As long as the batteries fit on your frame and produce 24v, they should work fine.

I charge these batteries with a standard 12v automotive battery charger using the low-power 2-amp charge level. I also disconnect the batteries from their 24v series configuration and use two home-made cables to connect the two batteries in parallel for charging, effectively decreasing the charging rate by doubling the Amp Hour rating from 12AH to 24AH. Charging a 24AH load at 2-amps equals a C/12 charging rate instead of a C/6 rate. I typically charge my batteries at the lowest rate possible to prolong the life of the cells. If you use rapid chargers (that is, high-amperage charge rate) on lead-acid (SLA), NiMH, or NiCad batteries, you notice the total life of the battery decrease significantly.

As you may recall, the Arduino boards voltage input is limited to a recommended maximum of 12v (and an absolute limit of 20v), so to avoid damaging the +5v regulator on the Arduino, you need to power it with 12v. In this two-battery series connection, it is easy to get 12v, by simply tapping into the series wire connecting the two batteries (See Figure 11-17).

Figure 11.17. Here you can see the basic wiring diagram between the Arduino, Sabertooth, and batteries. The Sabertooth requires a 24v supply whereas the Arduino requires a 12v supply.

With the batteries selected, it is time to build the frame. Use the battery dimensions to help determine the required size of the frame base, and after the frame is completed, mount the batteries to the underside of the frame between the motors.

The upper frame is a 72-inch piece of 1inch square steel tubing, cut into two pieces. The longer, 52inch piece serves as the main handlebar riser assembly and must be semi-cut and bent into a slightly obtuse L shape (see Figure 11-19).

The shorter 20-inch piece is used as the top part of the T handlebar and houses both steering and gain potentiometers, and the Engage button. It is bolted to the top of the frame riser bar using a large bolt.

The base consists of (three) 18-inch pieces of ¾-inch steel square tubing, each bolted to the motors and the upper frame piece (see Figure 11-20). To keep the weight centered between the wheels, the batteries are housed under the frame base using a small cage formed from ½-inch steel flat bar (see Figure 11.21). Though it could be avoided, I chose to use my wire-feed welder to strengthen a few spots and keep from adding extra support braces and bolts.

The battery cage must be larger enough to house the two SLA batteries which measure 6-inch L × 4-inch W × 3.8-inch H each. I made the battery cage large enough to hold both batteries and contain some of the wiring using (three) 20-inch long pieces of ½-inch wide flat steel bar (⅛-inch thick). The flat bars are each bent 90 degrees, 5 inches from each end—it should bend easily using a vise or pliers.

The entire frame assembly connects to each motor gear box using ten 8mm bolts. There are three ½-inch bolts used to connect the frame; one connects the upper T handle together, and the other two bolts connect the base frame to the lower part of the handlebar. You may also need four small bolts to hold the battery cage to the frame base if you do not have access to a welder.

With the basic design out of the way, it is time to get out your metal cutting saw, a drill, and a few other standard hand tools to begin building the frame.

To build the frame, first cut several pieces from the metal tubing to make the various frame sections. After cutting each piece, drill a few holes where needed and bolt everything together. The following eight steps guide you through the frame building process:

Figure 11.21. The underside of the base frame showing the battery cage

When assembled, the completed frame should look similar to Figure 11-22. I painted my entire frame black to look nice and protect the metal from rust. Painting is not required, but if you do plan to paint your frame, it is easier to do so before anything is mounted to it.

Figure 11.22. The finished frame after painting black

Your frame should be complete and ready for the motors, batteries, and electronics to be mounted. Start by wiring the frame with some input controls near the handlebar to allow steering and sensitivity (gain) inputs, and an Engage button to make sure there is an operator on board before engaging the motors.

The Arduino code reads the input from the X-axis on the IMU to determine the forward/reverse speed of both motors. Without any further additions, the hardware enables only the Seg-bot to travel either forward or reverse because the two motors are driven at the same speed.

To steer the Seg-bot, the motors need to be sent different speed values. To turn left, the left motor needs a lower speed value than the right motor. For smooth and responsive control, the changes made to each motor should be inversely proportional to each other. That is, as you decrease the left by 1 unit, you also increase the right by 1, causing a total speed difference of 2 units. By creating an imbalance in the values sent to each motor, you can turn the Seg-bot with great precision.

You can implement the steering sensor in several ways, the simplest is with a single 5k potentiometer. The code is set to read the potentiometer value of 0–1023 and convert the value to a range of 10 speed units in either direction. This increases or decreases the normal motor output values by up to a difference of 20 speed units from the left motor to the right, depending on which direction the potentiometer is turned. To drive the Seg-bot straight, the potentiometer must be in the center position.

Depending on the battery voltage, charge of the batteries, and gearing of the motors, the maximum speed of the Seg-bot might need to be adjusted for comfortable riding. To make this easy to do without reprogramming, I added another potentiometer to change the maximum speed value. The 0–1023 value from this potentiometer will be converted (mapped) into the maximum speed value for both motors, ranging from 50% to 100%. This enables the user to change the balancing sensitivity of the Seg-bot while in use.

The final input for the Seg-bot is the addition of the engage switch, which tells the Seg-bot that you are ready to ride. The engage switch is nothing more than a large Red momentary button-switch mounted to the handlebar and must be pressed for the Seg-bot to move. The Seg-bot stops as soon as the Engage button is released, so you must be sure to keep a good grip on it with your thumb when riding.

I added a small block of code to not only check the status of the Engage button but also the IMU angle. While the Engage button is open (not pressed), the Seg-bot cannot move; when the Engage button is closed (pressed), the code begins to check the IMU angle to make sure it is at 0 degrees before engaging the motors. When the Seg-bot is leveled to 0 degrees, it smoothly engages the motors and again begins processing the motor speed values. This keeps the Seg-bot from jerking quickly to correct its angle, in case you accidentally push the button while it is not level.... Safety first!

To connect each of these inputs to the Arduino, run some wires from the top of the frame where each input is mounted down to the base of the frame where the Arduino and IMU are mounted. I used a 4-conductor intercom wire from Radio Shack (item# 278-858) and a separate GND signal wire to run the input signals through the square tubing frame riser, thus concealing and protecting each wire. First, mount each input control to the handlebar and frame riser.

To mount the input controls to the frame, you need a few drill bits and a power drill. Start by taking a look at the finished handlebar in Figure 11-23, showing the Engage button, steering potentiometer, and gain potentiometer mounted into the 1-inch square tube frame.

Figure 11.23. The front view of the handlebar with all three input devices visible.

The following six steps guide you through installing the input controls:

Figure 11.28. Here you can see the painted Input wires going into the frame riser bar through a hole that I cut. By routing the wires through the steel tubing, they stay protected from being cut or pinched.

The Seg-bot should now be ready to install the electronics and make the final connections.

With the inputs installed, you need to solder the wires to the button and potentiometer terminals. You should run common +3.3v and GND power supply signals to the handlebar, and then share them between the Input devices, as shown in Figure 11.32.

Figure 11.32. The wiring diagram for the steering and gain potentiometers and the Engage button

Make sure you connect the outer terminals of each potentiometer with the power signals (+3.3v and GND) and the center terminal of each potentiometer to the Arduino input pin. The Engage button switch has only two terminals and should be connected to the Arduino input and GND.

The Seg-bot should now be ready for wiring.

The Seg-bot now needs to have each wire connected before you can upload the code and start testing. There is one wire connection between the Arduino and Sabertooth (other than GND), and three input wires from the engage button and potentiometers (and +3.3v and GND) that need to be connected to the Arduino.

Table 11-3 shows how each connection should be made from the inputs to the Arduino, the Arduino to the Sabertooth, and from the Sabertooth to the motors, where an x = no connection to that device.

As always, double-check your connections to make sure everything is wired properly before uploading the code and testing.

The Accelerometer is the first thing you need to read to get the approximate angle. Listing 11-4 shows the variables used.

int accel_pin = 5;
int accel_reading;
int accel_raw;
int accel_offset = 511;
float accel_angle;
float accel_scale = 0.01;

To determine the accel_offset, place the IMU board flat on a table (0 degrees) and read the accelerometer X-axis from an Arduino Analog input pin:

accel_reading = analogRead(accel_pin);

Whatever value displays on the serial monitor when the IMU board is level will be the accel_offset value, which should ideally be 511. his value will be subtracted from each accel_reading to yield the accel_raw variable. The accel_raw variable should read 0 when the IMU board is level:

accel_raw = accel_reading - accel_offset;

The original plan was to turn the IMU board 90 degrees in both directions and record the lowest/highest values displayed on the Serial monitor. I would then use the Arduino map() function to scale this recorded value range to be between −90 and +90.

But after turning the IMU in both directions, the lowest and highest values turned out to be −90 and +90, so there was no need to map() them. I did want to make sure that this value did not get out of range, so I added a line of code to set a minimum and maximum allowable value for the variable, using the constrain() function. See the Arduino reference pages for more information.

???Okay to use map() as a verb? DD

accel_raw = constrain(accel_raw, −90, 90);

Finally, it is easier to convert this value into a float variable (decimal number) so it can later be weighted as an average. I set the accel_scale variable equal to 0.01, effectively dividing the angle by 100 and setting the angle range to be from −0.90 to 0.90 (90 / 100); because it is a float variable, 0 degrees is actually represented as 0.00 degrees.

accel_angle = (float)(accel_raw) * accel_scale;

After multiplying the accel_raw reading by the accel_scale (0.01), the accel_angle variable is ready to be weighted into the filtered angle. Next, calculate the gyro angle.

The gyroscope reading filters out unwanted spikes from the accelerometer caused by bumps, vibrations, or sudden changes in gravity that have not affected the angle. Because the gyroscope is measuring only how much the angle has changed since the last reading, you must use the previous filtered angle reading as your reference to measure from. Several more variables are required to read the gyroscope, which are shown in Listing 11-5.

int gyro_pin = 1;
float angle = 0.00;
int gyro_reading;
int gyro_raw;
int gyro_offset = 391;

float gyro_rate;
float gyro_scale = 0.01;
float gyro_angle;
float loop_time = 0.05;

Using the base angle calculation from the accelerometer to reference the gyroscope angle to, you can use the gyroscope reading on a cycle-by-cycle basis, discarding each previous reading. This is how you eliminate the accumulated "drift" error from your filtered angle.

First, read the angular rate value from the gyroscope on one of the Arduino Analog input pins.

gyro_reading = analogRead(gyro_pin);

As with the accelerometer, calculate the gyro_offset by recording the gyro_reading when the IMU board is at rest. Although this value would instinctively be approximately 511, my IMU reads approximately 391 on the serial monitor for each of the 4x amplified gyro outputs (on a 10-bit scale of 0–1023). I thought this was unusual, but the datasheet for the gyro confirmed that it has a Zero-rate voltage level of 1.23 V, which is less than half of 3.33 V and would yield a value lower than 511 when at rest. If using a Razor 6 DOF IMU, you should leave the gyro_offset = 391.

gyro_raw = gyro_reading - gyro_offset;

This reading is slightly confusing; because the value is centered at 391, it can go up 632 units to 1023, but it can go down only 391 units to 0. To try and round this value out, I decided to constrain the value to +/−391.

gyro_raw = constrain(gyro_raw, −391, 391);

Now calculate the gyro_rate variable, which is the gyro_raw input multiplied by the gyro_scale, multiplied by the loop_time (0.05 seconds).

To make sure the angle readings are in unison, open the serial monitor after loading the code and move the Seg-bot back and forth to verify that the angles are measured properly.

With a USB cable connected to the Arduino, serial monitor open, and the main Seg-bot power off, start from 0.00 degrees and tilt the Seg-bot forward a few inches and then stop. You should see both the accel_angle and gyro_angle move in unison on the serial monitor. If not, you may need to adjust the gyro_scale variable.

The gyro_scale is a variable that can be used to tune the sensitivity of the gyro readings. Ideally the angle calculated by the gyro should be close to the accel_angle readings on the serial monitor. By default, the gyro_scale is set to 0.01, just like the accel_scale, but if the gyro_angle is less responsive than the accel_angle after testing, you might try increasing the gyro_scale to 0.02 or so.

gyro_rate = (float)(gyro_raw  * gyro_scale) * -loop_time;

Now to get an angle value for the gyro, add its current rate to the previous angle value:

gyro_angle = angle + gyro_rate;

The resulting gyro_angle value should remain close to the accel_angle value, which are both factored into the filtered angle value. With the angle readings from the IMU recorded, you now need to combine them together to determine the filtered value.

With the accel_angle and the gyro_angle calculated, you can now come up with a weighted average of the two, by multiplying each angle calculation by a weighted percentage and then adding them together. The variables used to calculate the filtered angle are shown in Listing 11-6. Because the final angle is calculated as a decimal number, these variables need to be declared as float types instead of integers.

float angle = 0.00;
float gyro_weight = 0.98;
float accel_weight = 0.02;
float gyro_angle;
float accel_angle;

Remember that the accelerometer is greatly affected by vibrations and bumps, so its weight is low in the average, at 2% (0.02). This means that the gyro_angle, which is an updated average loosely based on the previous filtered angle, is weighted heavily at 98% (0.98). This is because the gyro has a much more stable view of the current angle than the accelerometer. The small 2% weight from the accelerometer however is enough to keep the gyro from drifting.

To get the current angle, set the angle variable equal to the sum of the two weighted sensors ((.98)gyro + (.02)accelerometer). You can change the weights of these two variables from 98% gyro and 2% accelerometer to something else; although, going below 95% gyro and 5% accelerometer produced unstable results during my testing.

Following is the formula to calculate the filtered angle:

angle = (float)(gyro_weight * gyro_angle) + (accel_weight * accel_angle);

With the filtered angle value calculated, the Seg-bot should balance itself. To achieve steering on the Seg-bot, you still need to create an imbalance between the outputs of each motor. To do this, read the steering potentiometer with the Arduino and set each motor's maximum output based on the position of the potentiometer.

The read_pots() function is called to read both the steering and gain potentiometers. The steering potentiometer does nothing more than spin one motor faster than the other, which makes the bot turn. If the potentiometer is biased to either the left or right, the Arduino causes the Seg-bot to turn in that direction. The gain potentiometer determines how quickly the motors respond to changes in the filtered angle, by changing the maximum motor output speed (for both motors) from between 50% and 100%, depending on the position of the gain potentiometer. The variables used to read the steering and gain potentiometers are shown in Listing 11-7.

int steeringPot = 3;
int steer_reading;
float steer_val;
int steer_range = 7;

int gainPot = 2;
int gain_reading;
int gain_val;

The value of the steering potentiometer is read from the steeringPot analog input (pin A3) and stored in the variable steer_reading.

steer_reading = analogRead(steeringPot);

The steer_reading value is then mapped from 0–1023, to range between −7 and +7. You can change the "steer_range variable if needed-a lower value yields less responsive steering whereas a higher value yields more responsive steering. Values above 15 resulted in far too responsive steering whereas a 0 value would not respond at all.

The steer_range variable needs to be assigned only one number, and the value range will be mapped from −(that number) to +(that number), where zero is straight ahead, (Both motors get equal speed.) If your potentiometer responds opposite as it should, try changing the negative sign of the mapped steer_range variables.

steer_val = map(steer_reading, 0, 1023, steer_range, -steer_range);

The steer_reading can cause the motors to be turned even if the Seg-bot is not tilted forward or backward, so I added an if statement that checks to see if the Seg-bot is at 0 degrees. If so, the gain_reading will also be set to 0, so the bot will not be affected by the steering potentiometer.

if (angle == 0.00){
    gain_reading = 0;
 }

Now read the value of the GAIN potentiometer to determine the top speed of the Seg-bot. The gain changes the maximum top speed from 50% up to 100%. This is useful to fine-tune the responsiveness of the Seg-bot to fit different riding styles.

gain_reading = analogRead(gainPot);

After reading the input, map the value to between 32 and 64. This value is your gain_reading and sets the maximum speed in either direction. If the potentiometer is turned down completely, the max speed is between −32 and 32 (or a max of 50% speed). If the potentiometer is turned up completely, the max speed is increased to a range of −64 to 64, allowing up to a 100% max speed.

Why 64? Remember that the Sabertooth serial interface enables 64 speed steps in either direction, where 64 steps above or below neutral can yield full speed in that direction. By allowing the top speed to be 64, it can go 100%. If you allow it to go up to only 32, it can achieve only 50% of its full speed, therefore making the bot less responsive to small angle changes.

speed_val = map(gain_reading, 0, 1023, 32, 64);

Now that you have processed the input control signals from each potentiometer, check the engage button to see if it is pressed.

The auto_level() function is used to complement the engage_switch, to make sure the Seg-bot is level before engaging the motors. When the engage_switch is released, the Arduino removes power to the motors until the button is pressed again, notifying the Arduino that there is an operator on board.

At first, I just held the bot approximately level (at 0 degrees) and then pressed the button, but if it were not level, it quickly corrected itself to be level. I began to think this could be slightly dangerous when I had the Seg-bot tilted forward and accidentally pressed the button. The handlebar immediately came flying back toward me and almost hit me in the head!

To fix this bug, I decided to add a small bit of code that checks to see if the engage_switch has been released. If so, it makes sure that when it is pressed again, the angle variable must be close to 0 before it re-engages the motors. The motors now engage smoothly and without any sudden movements, as soon as the Seg-bot is level. The variables used for the auto_level() function are shown in Listing 11-8.

float angle = 0.00;
int engage_switch = 4;
int engage_state;
int engage = false;

First, read the state of the engage_switch and store it in the engage_state variable:

engage_state = digitalRead(engage_switch);

Now check to see if the switch is open or closed. To avoid using pull-down resistors on the engage_switch, I used the built-in Arduino pull-up resistors accessible from the code and connected the engage_switch to GND when closed. This means that when the button is not pressed, the switch reads HIGH or 1 and when the button is pressed, the switch will read LOW or 0. I know this is counter-intuitive, but it makes wiring switches much more simple by connecting the Arduino input wire (D4) to one terminal of the button switch and a GND wire to the other.

Use an if statement to see if the engage_state variable is HIGH. (That is the switch is OPEN.) If so, set the engage variable to be false. (That is, disengage the motors.)

if (engage_state == 1){
  engage = false;
}

Otherwise (else), if the engage_state is LOW (that is, the button is pressed), use another if statement to check and see if the engage variable is currently set to false. If the button is pressed, but the engage variable is false, this means the switch was OPEN but someone just CLOSED it.

At this point, the Arduino is ready to start commanding the motors, but it must first make sure the bot is level. To do this, use yet another if statement to check the current angle value of the Seg-bot. If the angle is below 0.02 and above −0.02 (that is, it is almost perfectly level), re-engage the motors by setting the engage variable equal to true. Otherwise, if the angle is not close to 0 degrees, keep the motors disengaged until the angle is corrected.

If the engage variable is already true when it enters this else{} function, it remains true until the engage_switch is released.

else {
  if (engage == false){
    if (angle < 0.02 && angle > −0.02)
      engage = true;
    else {
      engage = false;
    }
  }
  else {
    engage = true;
  }
}

Now update the motors with the new speed values. Before you can send the values to the Sabertooth motor controller, you first need to convert the angle value into a corresponding motor speed value and then send that value to the Sabertooth as a serial byte.

This is by far the longest function in the code, responsible for changing the angle value into a motor speed value, checking that value to make sure it is within range, and then sending it to the Sabertooth motor controller. Listing 11-9 shows the variables used for the update_motor_speed() function.

int engage;
float angle = 0.00;

int motor_out = 0;
int output;
float steer_val;
int gain_val;

int motor_1_out = 0;
int motor_2_out = 0;
int m1_speed = 0;
int m2_speed = 0;

Before attempting to write a value to the motors, first check to see if the engage variable is set to equal true (that is, if the engage button is pressed). If so, continue to the next if statement. If engage is set to false, skip down to the else statement that stops both motors by setting their outputs equal to 0 (disengage).

if (engage == true){

Next, check the angle variable to make sure it is within tilting limits. I recorded the angle of the Seg-bot when tilting it forward until the motors almost touched the ground; this angle was 0.4 (40 degrees) on my Seg-bot, and I have not yet exceeded it. If the angle while riding exceeds the limits set here, the motor_out variable will be set to 0, which stops both motors.

if (angle < −0.4 || angle > 0.4){
  motor_out = 0;
}

Otherwise, if the angle is within operating limits, set the output variable equal to the angle * 1000. Multiply the angle (which is a decimal number between −0.9 and 0.9) by 1000 to convert it back to a usable integer from a float type variable that you needed to weight the average.

I wanted the full speed to be achieved when the Seg-bot reached a 25-degree deviation from 0 degrees in either direction, so I mapped the motor_out variable from −250 to 250. The number 250 comes from the angle variable where 25 degrees is represented as 0.25. When I multiplied the angle by 1000 to get the output variable, a 25 degree angle now represented as 250.

We are mapping the motor_out variable from −/+250 to −/+ gain_val, which is read from the gainPot potentiometer and can range from −/+32 to −/+64 depending on its position. In any case, when the Seg-bot is at 0 degrees, it yields a motor_out value equal to 0.

else {
  output = (angle * −1000);
  motor_out = map(output, −250, 250, -gain_val, gain_val);
}

This ends the first else{} statement in the update_motor_speed() function. Now manipulate the motor_out variable into separate variables for each motor called motor_1_out and motor_2_out respectively.

At this point, add the steering bias into the individual motor speeds. To do this, add the mapped variable steer_val (−7 to +7) to motor1 while subtracting it from motor2. This causes the wheels to move at different speeds depending on the position of the steering potentiometer. When the steer_val goes negative, it will actually be subtracting from motor1 and adding to motor2.

motor_1_out = motor_out + (steer_val);
motor_2_out = motor_out − (steer_val);

With the steer_val added to each motor output value, check motor_1_out and motor_2_out to make sure that neither is above 63 or below −63. If either of these variables were to be higher than 63 (in either direction), it could unintentionally write a command to the wrong motor!

if(motor_1_out > 63){
  motor_1_out = 63;
}
if(motor_1_out < −63){
  motor_1_out = −63;
}

if(motor_2_out > 63){
  motor_2_out = 63;
}
if(motor_2_out < −63){
  motor_2_out = −63;
}

Now convert these two values into separate Sabertooth simplified serial commands. Remember from the Sabertooth simplified serial chart (Figure 11-15) the neutral command for Motor1 = 64 and the neutral command for Motor2 = 192. Because the motor_1_out and motor_2_out variables are already mapped between a maximum of −64 to +64, you can simply add them to the neutral points of each motor to get the actual BYTE value needed to send to the Sabertooth. If the byte has a negative value, it subtracts from the neutral points to command the motors to reverse.

m1_speed = 64 + motor_1_out;
 m2_speed = 192 + motor_2_out;
}

This is the end of the first if statement in the motor_speed_update() function [if (engage == true)].

The else statement covers what happens if engage == false when the Engage button is released, which is that the motor speed values are both set to 0, stopping the motors (or disengaging them).

else{
  m1_speed = 0;
  m2_speed = 0;
}

After all of the if/else statements are completed, use the SoftwareSerial library to write the new BYTE values for each motor to the Sabertooth using Arduino pin 3 (the SoftwareSerial tx pin)—even if the value written is 0 (that is, stop both motors).

mySerial.print(m1_speed, BYTE);
mySerial.print(m2_speed, BYTE);

After the motor values have been written to the Sabertooth, record the timestamp for this loop() cycle to use when calculating the next gyroscope reading.

This function checks the time of each loop cycle, and add a delay() until the time equals 50 milliseconds. You need each loop cycle to equal 50 milliseconds (or some other known time) so that you correctly calculate the gyroscope rate in degrees per second, rather than degrees per loop cycle (which is not useful). I used 50 millisecond update intervals to provide an angle reading that is updated by the Arduino 20 times each second! This provides seamless angle correction from the Seg-bot.

The while() statement continuously runs until the condition is met. By subtracting the last_cycle variable from the current millis() value, you can get the exact time (in milliseconds) since the last update. Furthermore, if the current millis() value minus the last_cycle timestamp is less than 50 (milliseconds), add a 1 millisecond delay over and over again until the last_cycle variable is greater than or equal to 50 mS. This effectively forces each loop cycle time to equal 50 mS, so you know that every cycle is the same length.

while((millis() - last_cycle) < 50){
  delay(1);
}

When the last_cycle variable is equal to 50, the while() statement is exited, and the last two variables are recorded. First, the actual recorded cycle_time variable is calculated for viewing on the Serial monitor, which should always equal the desired cycle time above (50 in this case):

cycle_time = millis() - last_cycle;

Next, it is time to write the new timestamp value by recording the current millisecond value from the system timer. The timestamp function is now ready to check itself again with the new time value in the next loop cycle.

last_cycle = millis();

Finally, print all the important values to the serial monitor for debugging.

This is the last function and is used only to print values to the serial monitor. You can edit which variables you want to view on the serial monitor, depending on what part of the system you test.

Be warned, trying to view every variable at once using the Serial.print() function causes the loop to take longer than 50 milliseconds, which means that the timing will be off for the gyro calculation, and the angle calculation will be wrong. To avoid this, try to limit the number of variables that you send to the serial monitor to about 3 or 4.

If you are unsure, try viewing the cycle_time variable on the serial monitor because it shows you the calculated loop cycle time in milliseconds. If this number is above 50, you might need to stop printing a few variables in this function.

I have the four most important variables printing by default; although, you can change them or add more (Listing 11-10).

Serial.print("Accel: ");
Serial.print(accel_angle);
Serial.print("  ");

Serial.print("Gyro: ");
Serial.print(gyro_angle);
Serial.print("  ");

Serial.print("Filtered Angle: ");
Serial.print(angle);
Serial.print("  ");

Serial.print(" Time: ");
Serial.print(cycle_time);
Serial.println("    ");

/*       // from here down are commented out unless testing

Serial.print("o/m: ");
Serial.print(output);
Serial.print("/");
Serial.print(motor_out);
Serial.println("  ");

Serial.print("steer_val: ");
Serial.print(steer_val);
Serial.print("  ");

Serial.print("speed_val: ");
Serial.print(speed_val);
Serial.print("  ");

Serial.print("m1/m2: ");
Serial.print(m1_speed);
Serial.print("/");
Serial.println(m2_speed);
*/

These values are used only for debugging purposes and can be changed or omitted.

Now that you know how the code works, Listing 11-11 presents it in its full form, ready to upload to the Arduino. When uploaded, remember to carefully test the Seg-bot for proper motor direction and correct steering. To safely test the Seg-bot, the base must be propped up so that the wheels do not touch the ground. Do this before you turn on the main power!

// Chapter 11: The Seg-bot
// JD Warren 2010 (special thanks to Josh Adams for help during testing and coding)
// Arduino Duemilanove (tested)
// Sparkfun Razor 6 DOF IMU - only using X axis from accelerometer and gyroscope 4x
// Steering potentiometer used to steer bot
// Gain potentiometer used to set max speed (sensitivity)
// Engage switch (button) used to enable motors
//
// By loading this code, you are taking full responsibility for what you may do it!
// Use at your own risk!!!
// If you are concerned with the safety of this project, it may not be for you.
// Test thoroughly with wheels off the ground before attempting to ride - Wear a helmet!


// use SoftwareSerial library to communicate with the Sabertooth motor controller
#include <SoftwareSerial.h>
// define pins used for SoftwareSerial communication

#define rxPin 2
#define txPin 3
// set up a new SoftwareSerial port, named "mySerial" or whatever you want to call it.
SoftwareSerial mySerial =  SoftwareSerial(rxPin, txPin);


// Name Analog input pins
int gyro_pin = 1;  // connect the gyro X axis (4x output) to Analog input 1
int accel_pin = 5; // connect the accelerometer X axis to Analog input 5
int steeringPot = 3; // connect the steering potentiometer to Analog input 3
int gainPot = 2; // connect the gain potentiometer to Analog input 2

// Name Digital I/O pins
int engage_switch = 4; // connect engage button to digital pin 4
int ledPin = 13;

// value to hold the final angle
float angle = 0.00;
// the following 2 values should add together to equal 1.0
float gyro_weight = 0.98;
float accel_weight = 0.02;

// accelerometer values
int accel_reading;
int accel_raw;
int accel_offset = 511;
float accel_angle;
float accel_scale = 0.01;

// gyroscope values
int gyro_offset = 391;
int gyro_raw;
int gyro_reading;
float gyro_rate;
float gyro_scale = 0.025; // 0.01 by default
float gyro_angle;
float loop_time = 0.05;

// engage button variables
int engage = false;
int engage_state = 1;

// timer variables
int last_update;
int cycle_time;
long last_cycle = 0;

// motor speed variables
int motor_out = 0;
int motor_1_out = 0;
int motor_2_out = 0;
int m1_speed = 0;

int m2_speed = 0;
int output;

// potentiometer variables
int steer_val;
int steer_range = 7;
int steer_reading;
int gain_reading;
int gain_val;

// end of Variables


void setup(){

  // Start the Serial monitor at 9600bps
  Serial.begin(9600);
  // define pinModes for tx and rx:
  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);
  // set the data rate for the SoftwareSerial port
  mySerial.begin(9600);

  // set the engage_switch pin as an Input
  pinMode(engage_switch, INPUT);

  // enable the Arduino internal pull-up resistor on the engage_switch pin.
  digitalWrite(engage_switch, HIGH);
  // Tell Arduino to use the Aref pin for the Analog voltage, don't forget to connect 3.3v to
Aref!
  analogReference(EXTERNAL);

}

void loop(){
  // Start the loop by getting a reading from the Accelerometer and converting it to an angle
  sample_accel();
  // now read the gyroscope to estimate the angle change
  sample_gyro();
  // combine the accel and gyro readings to come up with a "filtered" angle reading
  calculate_angle();
  // read the values of each potentiometer
  read_pots();
  // make sure bot is level before activating the motors
  auto_level();
  // update the motors with the new values
  update_motor_speed();
  // check the loop cycle time and add a delay as necessary
  time_stamp();
  // Debug with the Serial monitor
  serial_print_stuff();

}

void sample_accel(){
// Read and convert accelerometer value

  accel_reading = analogRead(accel_pin);
  accel_raw = accel_reading - accel_offset;
  accel_raw = constrain(accel_raw, −90, 90);
  accel_angle = (float)(accel_raw * accel_scale);
}

void sample_gyro(){
// Read and convert gyro value

  gyro_reading = analogRead(gyro_pin);
  gyro_raw = gyro_reading - gyro_offset;
  gyro_raw = constrain(gyro_raw, −391, 391);
  gyro_rate = (float)(gyro_raw * gyro_scale) * -loop_time;
  gyro_angle = angle + gyro_rate;
}

void calculate_angle(){
  angle = (float)(gyro_weight * gyro_angle) + (accel_weight * accel_angle);
}

void read_pots(){
// Read and convert potentiometer values
// Steering potentiometer
  steer_reading = analogRead(steeringPot); // We want to map this into a range between −1 and
1, and set that to steer_val
  steer_val = map(steer_reading, 0, 1023, steer_range, -steer_range);
  if (angle == 0.00){
    gain_reading = 0;
  }
// Gain potentiometer
  gain_reading = analogRead(gainPot);
  gain_val = map(gain_reading, 0, 1023, 32, 64);
}

void auto_level(){
 // enable auto-level turn On
  engage_state = digitalRead(engage_switch);

  if (engage_state == 1){
    engage = false;
  }
  else {
    if (engage == false){
      if (angle < 0.02 && angle > −0.02)
        engage = true;
      else {
        engage = false;

}
    }
    else {
      engage = true;
    }
  }

}

void update_motor_speed(){
// Update the motors

  if (engage == true){
    if (angle < −0.4 || angle > 0.4){
      motor_out = 0;
    }
    else {
      output = (angle * −1000); // convert float angle back to integer format
      motor_out = map(output, −250, 250, -gain_val, gain_val); // map the angle
    }

    // assign steering bias
    motor_1_out = motor_out + (steer_val);
    motor_2_out = motor_out − (steer_val);

    // test for and correct invalid values
    if(motor_1_out > 64){
      motor_1_out = 64;
    }
    if(motor_1_out < −64){
      motor_1_out = −64;
    }
    if(motor_2_out > 64){
      motor_2_out = 64;
    }
    if(motor_2_out < −64){
      motor_2_out = −64;
    }

    // assign final motor output values
    m1_speed = 64 + motor_1_out;
    m2_speed = 192 + motor_2_out;
  }

  else{
    m1_speed = 0;
    m2_speed = 0;
  }

  // write the final output values to the Sabertooth via SoftwareSerial
  mySerial.print(m1_speed, BYTE);

mySerial.print(m2_speed, BYTE);
}

void time_stamp(){
  // check to make sure it has been exactly 50 milliseconds since the last recorded time-stamp
  while((millis() - last_cycle) < 50){
    delay(1);
  }
  // once the loop cycle reaches 50 mS, reset timer value and proceed
  cycle_time = millis() - last_cycle;
  last_cycle = millis();

}

void serial_print_stuff(){
  // Debug with the Serial monitor

  Serial.print("Accel: ");
  Serial.print(accel_angle);  // print the accelerometer angle
  Serial.print("  ");

  Serial.print("Gyro: ");
  Serial.print(gyro_angle);   // print the gyro angle
  Serial.print("  ");

  Serial.print(  "Filtered: ");
  Serial.print(angle);   // print the filtered angle
  Serial.print("  ");

  Serial.print(" time: ");
  Serial.print(cycle_time); // print the loop cycle time
  Serial.println("    ");

  /*  these values are commented out, unless testing
  Serial.print("o/m: ");
  Serial.print(output);
  Serial.print("/");
  Serial.print(motor_out);
  Serial.println("  ");

  Serial.print("steer_val: ");
  Serial.print(steer_val);
  Serial.print("  ");

  Serial.print("steer_reading: ");
  Serial.print(steer_reading);
  Serial.print("  ");

  Serial.print("m1/m2: ");
  Serial.print(m1_speed);
  Serial.print("/");

Serial.println(m2_speed);
  */
  }

//End Code

With the code loaded to the Arduino, it is time to begin testing.