The Balancing Robot Car

1. Use the NEMA17 Stepper Motor
The bipolar stepper motor we are going to use in this lab was ordered from here:

The datasheet can be downloaded here, The Torque Curve can be downloaded here.

Watch: How a stepper motor works?

In NEMA17's datasheet, the STEP ANGLE parameter tells you the 'steps per revolution' you have to define in your sketch. Every step moves 1.8 degree, there are 360 degrees per revolution so totally 200 steps.

Use the A4988 stepper motor driver

A4988 is an alternative product to the L298N driver.

The A4988 is a microstepping driver for controlling bipolar stepper motors which has built-in translator for easy operation. This means that we can control the stepper motor with just 2 pins from our controller, or one for controlling the rotation direction and the other for controlling the steps.

The Driver provides five different step resolutions: full-step, haft-step, quarter-step, eight-step and sixteenth-step. Also, it has a potentiometer for adjusting the current output, over-temperature thermal shutdown and crossover-current protection. Its logic voltage is from 3 to 5.5 V and the maximum current per phase is 2A if good addition cooling is provided or 1A continuous current per phase without heat sink or cooling.

The next two 2 pins, Step and Direction are the pins that we actually use for controlling the motor movements. The Direction pin controls the rotation direction of the motor and we need to connect it to one of the digital pins on our microcontroller.
With the Step pin we control the mirosteps of the motor and with each pulse sent to this pin the motor moves one step. So that means that we don’t need any complex programming, phase sequence tables, frequency control lines and so on, because the built-in translator of the A4988 Driver takes care of everything. Here we also need to mention that these 2 pins are not pulled to any voltage internally, so we should not leave them floating in our program.

Next is the SLEEP Pin and a logic low puts the board in sleep mode for minimizing power consumption when the motor is not in use. Next, the RESET pin sets the translator to a predefined Home state. This Home state or Home Microstep Position can be seen from these Figures from the A4988 Datasheet. So these are the initial positions from where the motor starts and they are different depending on the microstep resolution. If the input state to this pin is a logic low all the STEP inputs will be ignored. The Reset pin is a floating pin so if we don’t have intention of controlling it with in our program we need to connect it to the SLEEP pin in order to bring it high and enable the board.

The next 3 pins (MS1, MS2 and MS3) are for selecting one of the five step resolutions according to the above truth table. These pins have internal pull-down resistors so if we leave them disconnected, the board will operate in full step mode. The last one, the ENABLE pin is used for turning on or off the FET outputs. So a logic high will keep the outputs disabled.

Datasheet of the A4988 driver IC can be downloaded here. The module's pinout and specs can be found here.

Some notes:
- Every pulse will create one step, which is 1.8 degree (if it is a fll step, MS1 MS2 MS3 is  0 0 0), the pulse width cannot be too small. The example uses 500 us as the pulse width and it works fine. I tried 400 us and it doesn't work. You can also use wider pulses, say 1000 us, the rotation speed will be slower.
- 7.4 V for VMOT also works which means you can use a 2-cell lipo battery.
- The 47 - 100 uF cap shorting VMOT to GND can be absent without causing anything, but it is highly recommended to have it in place.

The hardware connection:

The sketch.

The demonstration video:

Use the A4988 driver and a potentiometer to adjust the motor speed in realtime.

Follow the instructions in section 1.2 and try to repeat the same results using the A4988 driver.

The demonstration video:

2. Test the MPU6050 accelerometer/gyroscope sensor
2.1 Understand acceleration and angular velocity

The pinout of MPU-6050

The schematic of the module:

Table of voltage ratings:

MPU-6050 Axis:

MPU I2C communication:

The key registers:

Binary readout data / LSB Sensitivity = xxx LSB / (LSB / g) = xxx g.

Binary readout data / LSB Sensitivity = xxx LSB / (LSB / (deg/s)) = xxx deg/s. This is the angular velocity but how do you get the rotated angles in realtime? - You must know the time spent for each 'void loop ()' function.

Use the following software and hardware to test your accelerometer and gyroscope before it's being connected to the car.

From the UNO's pinout, you can idenfy the I2C pins are A4 and A5:

Make the following hardware connections:

Use this example to test your acceleration and gyroscope readings respectively. Check out MPU6050's datasheet and register map if you have questions about the example sketch.

By holding the MPU6050 sensor at different gestures, you can understand what the accelerometer and the gyroscope are measuring.

The Gyro measures angular velocity. Therea are three axes X, Y, and Z, the direction of them are indicated on the module's PCB board.

The Z axis in the snippet above points out of your monitor and perpendicular to the PCB board. Let's take the airplane's X, Y, and Z axes as the example.

For airplane control, Pitch, Roll and Yaw are defined as the rotation around X, Y and Z axis. Below as a picture to illustrate the definition.

If the MPU is idle, no movement and no rotation, the Gyro readout should be all zeros.

Look at the following demonstration that I held the MPU module at different angles statically, the readout is always zero.

However, the acceleration readout values reacts to the angular changes. (I have converted the unit of the readings to 'g' for the following tests).

Keep in mind that gravity always exists so if you plug your MPU module on a breadboard vertically as follows:

You are supposed to receive something close to 8192 for Accel_X.

The number '8192' comes from the following configuration according to the register map.

(All these 8500+ values are supposed to be 8192 but you can tell the level of the deviations by holding it by hand and the system errors of itself.
The values are positive so disregard the direction of the arrow printed on the MPU module for the X axis. )

The code starts with the 0x3B register for data reading because the 2-byte X axis data is stored in 0x3B and 0x3C.

When you need both the Gyro and Accel readings, don't forget to skip 0x41 and 0x42 which are temperature data:

2.2 Angle calculation

MPU is able to measure the angles by combining the information from the accelerometer and the gyroscope.

If the MPU module is inserted to the breadboard perpendicularly, the direction of the three axes are as follows: 
Z front/back, X is up/down, and Y is left/right:

If the car leans forward, the wheels should move forward to prevent it from tipping over. If it leans backward, wheels should move backward.
The vertical acceleration is always 1 g, so the angle can be calcualted using the asin() fuction: Theta = asin(Accel_Z/1g).
The result of this equation is in radians, to convert it to angles, you need to multiply it by 180/pi (or 360/2pi), which is 57.29578: Theta = asin(Accel_Z/1g) x 57.29578. To implement it in Arduino:

asin((float)accelerometer_data_raw/8192.0)* 57.29578  // 8192 is the LSB/g configuration for the MPU chip

You could also use atan2() to find the angle from acceleration data. The angle change is small so sin() and tan() will give you very similar results. The following line of code uses atan2(ay,az) because the sensor was placed in paralle with the car. You can use atan2() or asin().

double accelYAngle = atan2(ay, az); // atan2() return a radian within -pi and pi.
double degAccelAngle = accelYAngle*57.29578; //multiply it by 180/pi (or 360/2pi), which is 57.29578 to convert it to degrees from radian

For Gyro's readout data is angular velocity (degree/s), for example the loop time is 4000 us, so the Gyro raw data should be converted in degrees using the following formula:

Gyro Raw Data (delta angle change in LSB) x 0.004 s /(131 LSB/degree/s) = Gyro Raw Data x 0.00031. The following example uses a different loop time so gz was mutiplied by 0.00000075:

double scaledZGyro = ((gz*0.00000075)*57.29578);

The address for the FS_SEL bits:

Now, the Data Fusion is a very common technique to filter out the high-frequency components and keep the low frequency components.

  static double angle = degAccelAngle;
  angle = (0.9934 * (angle + scaledZGyro)) + (0.0066 * degAccelAngle); // complimentary filter

The angle_gyro variable should store the real angle it has travelled.

We have two measurements of the angle from two different sources. The measurement from accelerometer gets affected by sudden horizontal movements and the measurement from gyroscope gradually drifts away from actual value. In other words, the accelerometer reading gets affected by short duration signals and the gyroscope reading by long duration signals. These readings are, in a way, complementary to each other. Combine them both using a Complementary Filter and we get a stable, accurate measurement of the angle. The complementary filter is essentially a high pass filter acting on the gyroscope and a low pass filter acting on the accelerometer to filter out the drift and noise from the measurement.

2.3 The PID Control Part (Reference in CE351)

  float targetAngle = 0+angle_zero;
  float Kp = 30, kpOutput = .03;    // kp = .07
  float Ki = .0, kiOutput = .09;   // ki = .14
  float Kd = 4;
  angleError = (angle - targetAngle); // subtract angle by offset   
  // calculate the proportional component
  pTerm = angleError*Kp;
  // calculate the integral component (summation of past errors * i scalar)
  static double integral = 0;
  integral += angleError;
  integral =  constrain(integral, -100,100);
  iTerm = Ki * integral;
  // calculate the derivative component
  static double lastAngle = 0;
  dTerm = Kd * (angle-lastAngle);
  lastAngle = angle;
  output = (pTerm + iTerm - dTerm); // could we use + dTerm?
  static int count =0;
  count ++;
  if(count >= 2){
    count = 0;
    // angle adjusting
    static double outputFilter = output;
    static double outputFilterOld, outputIntegral = 0;
    outputFilter = outputFilterOld * 0.7 + output * 0.3;
    outputFilterOld = outputFilter;
    outputIntegral += outputFilter;
    outputIntegral += -setting_car_speed;
    outputIntegral = constrain(outputIntegral, -3000, 3000);
    outputControlOutput = -kpOutput * outputFilter - kiOutput * outputIntegral; // Further filtering from the Tumbller's original code
    rotation_control_output = setting_turn_speed + kd_turn * scaledYGyro;
  pwm_left  = output - outputControlOutput - rotation_control_output;
  pwm_right = output - outputControlOutput + rotation_control_output;

  pwm_left = constrain(pwm_left, -255, 255);
  pwm_right = constrain(pwm_right, -255, 255);
    if (pwm_left < 0)
      digitalWrite(AIN1, 1);
      analogWrite(PWMA_LEFT, -pwm_left);
      digitalWrite(AIN1, 0);
      analogWrite(PWMA_LEFT, pwm_left);
    if (pwm_right < 0)
      digitalWrite(BIN1, 1);
      analogWrite(PWMB_RIGHT, -pwm_right);
      digitalWrite(BIN1, 0);
      analogWrite(PWMB_RIGHT, pwm_right);

2.4 The Initialization and Interrupts (References in CE351)

void carInitialize()
  pinMode(AIN1, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(STBY_PIN, OUTPUT);
  Wire.write(0x6B);//Reset the device, very important
// Configure Gyroscope Sensitivity
  Wire.write(0x1B);// Talk to the GYRO_CONFIG register
  Wire.write(0x08);// Set the register bits as 0000 1000 (500 deg/s full scale)
  Wire.write(0x1C);//Talk to the ACCEL_CONFIG register (1C hex)
  Wire.write(0x08);//Set the register bits as 00001000 (+/- 4g full scale range)



1. Use an Arduino UNO board to communicate with an MPU6050 sensor module. Be able to read Acceleration and Gyroscope data in the serial monitor.(10 pionts)
2. Be able to use data fusion and the complementary filter to report the angle of the MPU module. (10 points)
3. Refer to Sections 2.3 and 2.4, develop your own PID controller for the Tumbller robot and be able to balance and remotely control it. (20 points)
4. Build a balancing robot using the NEMA17 stepper motor and acrylic plates available in the lab. You can reuse the car frames built by former students. The robot is remotely controlled by a joystick through the 2.4 GHz RF modules. (20 points).
5. Use an encoder to tune the PID parameters (20 points)
6. Design the PCB for the remote and the car. (20 points)

David Lee
Brady Morrow, code
Calvin Reese, code