The following project is a result of viewing the Cubli video and becoming interested in the control of unstable equilibrium using brushless motors. The simpler inverted pendulum problem was decided on in order to complete the project before loosing interest. Many people provide better descriptions than I can, therefore I will only focus on the build and try to offer useful links when needed.
Step 1: Parts
As the project title hints, the robot uses parts from a brushless gimbal camera stabilizer. This is ideal as the motor controllers and microcontroller are on the same circuit board and already setup to work with the included brushless motors and inertial measurement unit (IMU). The chosen gimbal system is designed to work with GoPro sized cameras with several iterations found cheaply online.
- Gimbal control board BGC 2.0, look closely at the provided picture (2X L6234 brushless motor control ICs, mega328p microcontroller, Silabs CP2102 USB to serial)
Note: other boards may work, it will need to be programmable using the Arduino IDE, therefore must contain a compatible microcontroller and ISP header to burn the bootloader
- gy-521 IMU (MPU-6050 3-axis gyroscope and 3-axis accelerometer)
- 2X 2208 brushless gimbal motors (fasteners to mount motor, they were included with my gimbal)
- LiPo battery 3S (Turnigy nano-tech 460mAh 25-40C 57X31X17mm)
- Colson Performa wheels 2" X 7/8"
- HC-05 Bluetooth module
- 5/8" aluminum plate or bar to CNC robot body from, body made of two halves with finished size 2.50X4.89X0.55" (1/2" plate or bar also works, body will be 0.1" thinner)
- 2X 6-32 X 1" button head screws
- 1/8" X 3/4" dowel pin
- 6" X 1/2" double sided Velcro strap to hold battery in
- Other assembly stuff: double sided foam tape, hot glue, hook-up wire, 0.8mm piano wire to lengthen Bluetooth antenna, sticky rubber bumpers, battery plugs, indicator LEDs and stuff I have not thought of
Step 2: SolidWorks Model
The SolidWorks model is provided for machining and reference. Make sure your electronics, battery, motors and other parts work with the design before starting. Some modification might be needed.
Step 3: Machining
The inside diameter of the wheels need to be opened up on a lathe to allow for the motors to be pressed into the wheels. Measure the outer diameter of the motors and turn the inner diameter of the wheels about 0.001-0.002" smaller for a press fit. The depth should match the dimension of the outrunner motor bell. The wheel core is made of a hard plastic and holds decent tolerance. I held the wheels by the rubber exterior, in a six jaw chuck and indicated in off the plastic face.
Remove the motor bell/shaft assemblies by pulling the snap ring off the shaft on the back side of the motor and drawing the assembly out of the bearings. Press the bells into the wheels by putting pressure on the edge of the bell (not the shaft).
The body consists of two identical halves except for the alignment pin holes and the fasteners (see provided drawing). The two halves are CNC milled from 5/8" (1/2" also works but robot will be 0.1" thinner) aluminum plate. The SolidWorks part is provided to use with your CAM software of choice. I marked the motor mounting holes, alignment pin hole and fastener holes on the CNC using a center drill before the parts were separated from the plate. The parts should now be deburred and cleaned. I then complete the drilling, tapping and countersinking (this can also be accomplished on the mill before the parts are removed from the plate).
Step 4: Assembly
Look at the provided picture for part orientation.
- Check the control board to see if a program can be uploaded using the Arduino IDE. I needed to burn the Arduino bootloader.
- Press the dowel pin into place
- Attach the control board using double sided foam tape on the webs of the aluminum body
- Attach the brushless motors to the body with the wires on top
- Hot glue the IMU into place making sure it sits flush on the aluminum tabs and the IC is located inline with the motor shafts
- Create a full wave antenna (~12.5cm) out of the piano wire for the Bluetooth module and attach so it goes outside the body (I drilled a small hole through the module allowing the antenna to go out the backside and provide more support)
- Attach the Bluetooth module using double sided foam tape
- Reset the default values on the HC-05 using AT commands and these instructions. My Bluetooth module had a pushbutton instead of a "KEY" pin, that needed to be held during power up and programming. The device name and password are reset and the baud rate is set to 230400.
- Connect the Bluetooth TX to RX, Bluetooth RX through a voltage divider to TX to reduce the voltage to 3.3v and supply the needed power
- Attach a battery connector (I added a 2amp fuse inline to offer a little more protection)
- Hookup the IMU and motors to the control board (they came with attached connectors)
- Attach an indicator LED to pin A1 using a 1k ohm resistor (I attached two and pointed them out the bottom of the robot)
- Other optional stuff: Velcro straps on the top to hold the battery in, hot glue the wires for strain relief, rubber bumpers to protect the floor
- Both halves should now fit together and be held in place using the two #6-32 fasteners
Step 5: Arduino Code
-The code is running on a mega328p and was compiled using Arduino 1.6.6
-The code cycle time is a constant ~250Hz
At the heart of the code is a counter that is advanced every time TIMER1 overflows and triggers an interrupt routine. This counter triggers other events at specific intervals allowing the code to run at a known rate and simplify math in the control loop.
The tilt angle of the robot is calculated using a complementary filter with gyrometer and acceleration values from the MPU6050 IC. This is fed into a control loop with three other values: the time derivative of the tilt angle, robot velocity (acceleration integral) and robot displacement (velocity integral). All four values are added with preset gain to get an output term proportional to robot acceleration. A good explanation of this control loop by David P. Anderson and more information on his balancing robot.
To move the robot an offset is added to the displacement value in the control loop. To turn left or right an offset is added/subtracted from the left and right wheel velocities. The wheel velocities are fed into the the motor control portion of the code.
Brushless motors require 3-phase power. Look at this explanation of how angle control of brushless gimbal motors is achieved. The motor control portion of my code is based off of the BruGi brushless gimbal control code. All three timers are set to output 31.25KHZ phase correct PWM (6 PWM outs). A useful article on adjusting Arduino timers.
A lookup table containing space vector PWM values is used to pull three values with 120 degree phase separation. The values pulled from the lookup table are advanced at a rate proportional to the needed left or right robot wheel velocity.
The code handles motor power adjustment to correct for torque falloff at higher speeds and greater needed torque during higher acceleration. The battery voltage is monitored to prevent damage to the LiPo batteries. The code also handles Bluetooth communication between a smartphone.
Step 6: Tuning Control Loop for Balance
The control loop has four gain values that need to be set.
- kp angle gain
- kd angle velocity gain
- kv velocity gain
- kc position gain
This is how I tuned the robot for balance, it is probably not ideal but it worked. Start with all gain values set to zero. Slowly increase kp gain until the robot no longer falls forward, now increase kd gain until the robot stands but slowly oscillates with growing amplitude until the robot falls over. Increase the kv value until the robot no longer oscillates and recovers from a light push on the top. At this point the robot should have the desired performance. It should stand in one place and recover from a disturbance. Play with the three values to get a better feel for how they behave. Now increase kc gain until the robot returns to the same place after a disturbance but not too large resulting in decreased performance.
I reset all values to zero and started over several times until I achieved decent results. If it looks like the loop is completely failing make sure all the calculated values in the loop have the correct sign and behavior and the feedback values have the correct sign.
Step 7: Bluetooth Control
The android app Joystick BT Commander is used to communicate with the Bluetooth device on the robot. The app is setup to display the robot battery voltage, relative motor output power and the code cycle time. The cycle time is important because it must not exceed the time interval between calls (~4ms) in order to maintain proper timing for derivative and integral calculations. The app also has two buttons setup. A power button "ON" turns on the motor control portion of the code when activated. The second button "Fast" increases the robots top speed by a factor of two when pressed.
Step 8: Finished
When the robot is first powered it needs to be set on its front or back within 5 seconds. At this point the indicator LED will flicker for 10 seconds as 1024 gyroscope readings are averaged to obtain a calibration value. If the battery is charged the indicator led should now start to turn on and off every 2 seconds. A connection can now be made between the smartphone Bluetooth application and the robot. Press the "ON" button and place the robot within 1 degree of vertical to start balancing. Use the joystick to drive the robot. Once the battery is depleted the robot will no longer balance and the indicator LED will stay on.