Java ME 8 + Raspberry Pi + Accelerometer + PWM + Motor Driver = JBalancePI Robot (Part 1)

Version 16

    by Jose Cruz

     

    Learn how to build a two-wheel self-balancing Java robot using Raspberry Pi.

     

    In my last four-part series of articles, I explained how to connect electronic sensors or devices to the Raspberry Pi 2 Model B using various interfaces: Part 1 showed how to use general-purpose input/output (GPIO) interfaces, Part 2 used inter-integrated circuit bus (I2C) interfaces, Part 3 used universal asynchronous receiver transmitter circuit (UART) interfaces, and Part 4 used serial peripheral interface (SPI) interfaces.

     

    This article, which is the first in a new series, focuses on using two types of interfaces, GPIO and I2C, to create a prototype two-wheel self-balancing robot using the following modules:

     

    • MPU-6050 sensor, which contains a three-axis gyroscope and an accelerometer. The accelerometer measures linear acceleration and earth gravity vectors and the gyroscope measures angular velocity. The sensor uses an I2C interface at address 0x68h to read a value raw that contains the accelerometer and gyroscope data.
    • Adafruit PCA9685, which is a 16-channel, 12-bit pulse width modulation (PWM) servo driver that produces pulses at a desirable frequency to control motor speed. It uses an I2C interface at address 0x41h to prevent the Raspberry Pi from producing a software pulse that forces a large delay in CPU response.
    • L298N dual H-bridge DC motor driver, which controls the speed and direction of the two robot motors. It uses a GPIO interface to produce the direction signal and connects to the PWM module to activate and control the speed of each motor.

     

    Note: The complete example code for this NetBeans IDE 8.0.2 project can be downloaded here.

     

    The MPU-6050, the PCA9685, and the L298N modules create a feedback control loop, as shown in Figure 1, which balances the robot and automatically corrects its position upon disturbance.

     

    The robot is prevented from falling by giving acceleration to the wheels according to the robot's inclination from the vertical axis. If the robot tilts at an angle, then in the frame of the wheels, the center of mass of the robot will experience a pseudo force that will apply a torque opposite to the direction of tilt.

     

    f1.gif

    Figure 1. Steps involved in feedback control loop for the balancing robot

     

    Circuits We Will Create

     

    Based on the block diagram shown in Figure 2, the electronic components shown Figure 3, and the mechanical parts shown in Figure 4, we will create the circuits shown in Figure 5 and the two-wheeled balanced robot prototype shown in Figure 6.

     

    f2.png

    Figure 2. Block diagram of the circuits we will create

     

      

    f3.1.jpgf3.2.jpgf3.3.jpg
    f3.4.jpgf3.5.jpgf3.6.jpg
    f3.7.jpgf3.8.jpgf3.9.jpg

    Figure 3. Electronic components we will use

     

    List of electronic components:

     

    • 1 x Raspberry Pi 2 Model B
    • 1 x MPU-6050 three-axis gyroscope with accelerometer sensor module
    • 1 x L298N dual H-bridge DC motor driver
    • 1 x Adafruit PCA9685 16-channel 12-bit PWM servo driver
    • 1 x DC/DC power converter LM2596
    • 1 x Adafruit Perma-Proto Pi HAT
    • 1 x 7.4V 6A LiPo rechargeable battery (Note: For good results, it is suggested that the power supply be a 7.4v LiPo battery with a minimum of 2.200 mAh connected to a DC-DC power converter generating 5v final voltage.)
    • 2 x 0.1 uF capacitor
    • 1 x ThePiHut Wi-Fi dongle 802.11N

     

    f4.1.jpgf4.2.jpgf4.3.jpgf4.4.jpg
    f4.5.jpg

    Figure 4. Mechanical parts we will use

     

    List of mechanical parts:

     

    • 2 x wheel
    • 2 x shaft coupling
    • 2 x motor
    • 4 x brass stud M4x40
    • 4 x brass stud M4x30
    • 8 x brass stud M4x10
    • 2 x motor brackets
    • 3 x acrylic board
    • 20 x screw M4
    • 14 x nut M4

     

    f5.gif

    Figure 5. Schematic of the circuits we will create

     

    f6.gif

    Figure 6. Two-wheeled prototype of balanced robot we will create

     

    A Little Mathematics

     

    The Inverted Pendulum Problem

     

    Balancing robots represents the classic inverted pendulum problem, in which a large mass is placed at the end of a pole. The pole is free to rotate around the base, and the base is free to move in the plane perpendicular to the vertical. The goal is to keep the pole vertical by moving the base in response to changes in the angle.

     

    Complementary Filter

     

    Figure 7 shows a two-wheeled robot with a three-axis accelerometer to measure tilt angle θa and a single-axis gyroscope to measure dynamic tilt angle θg. Both angles must be fused by using a filter to obtain a final tilt angle θ. In our case, we will use a complementary filter, as shown in Figure 8, which controls the speed of the motors based on the angular velocity information.

     

    f7.png

    Figure 7. Robot with three-axis accelerometer and a single-axis gyroscope

     

    f8.jpg

    Figure 8. Complementary filter block diagram (source: Mouser)

     

    We obtain the θ angle reading from the accelerometer in a continuous loop by using a low-pass filter, and we sum the results with the gyroscope values by applying a high-pass filter.

     

    We build this filter inside the method Filter in the JBalancePI class, as shown in Listing 1. The variable angle_filtered is the θ angle:

     

         /** 
         * Read data from accelerometer and gyroscope and apply a complementary 
         * filter 
         */ 
         private void Filter() {    
         //Data from MPU-6050 Accelerometer and Gyroscope    
         data = accel.getMotion6();    
         //Calculate angle and convert from radians to degrees    
         float angle_raw = (float) (Math.atan2(data.AccelY, data.AccelZ) * 180.00 / pi + accel_offset);   
         float omega = (float) (data.GyroX / gyro_gain + gyro_offset);    
         // Filter data to get the real value    
         long now = System.currentTimeMillis();    
         float dt = (float) ((now - preTime) / 1000.00);    
         preTime = now;    
         //Calculate error using complementary filter    
         float K = 0.8F;    
         float A = K / (K + dt);    
         angle_filtered = A * (angle_filtered + omega * dt) + (1 - A) * angle_raw;
    

     

    Listing 1. Filter method that implements a complementary filter

     

    For details about the mathematical formulas, please see this information.

     

    It is very important that the MPU-6050 sensor's Y orientation is mounted parallel to the robot wheels, as shown in Figure 9.

     

    f9.png

    Figure 9. MPU-6050 sensor's Y orientation (in the red circle), mounted parallel to the wheels

     

    Proportional, Integral, and Derivative (PID) Control System

     

    The control algorithm that we will use to maintain the balance off the autonomous self-balancing two-wheeled robot is the PID controller. The PID controller is well known as a three-term controller; it is an incredibly powerful and ubiquitous control algorithm. An output signal u can be generated by summing the three components, as shown in Figure 10:

     

    f10.gif

    Figure 10. u(t) output control signal

     

    The input to the controller is the error from the system. The Kp, Ki, and Kd terms are referred to as the proportional, integral, and derivative constants (the three terms get multiplied by these constants respectively), and e(t) is the error from the desired output at time t.

     

    The closed-loop control system is also referred to as a negative feedback system. The basic idea of a negative feedback system is that it measures the process output from a sensor. The measured process output gets subtracted from the reference set-point value to produce an error. The error is then fed into the PID controller, where the error gets managed in three ways.

     

    The error will be used on the PID controller to execute the proportional term, the integral term for the reduction of steady-state errors, and the derivative term to handle overshoots. After the PID algorithm processes the error, the controller produces a control signal u. The PID control signal then gets fed into the process under control.

     

    The process under PID control is the two-wheeled robot, as shown in Figure 11. The PID control signal will try to drive the process to the desired reference set-point value. In the case of the two-wheeled robot, the desired set-point value is the zero degree vertical position.

     

    f11.png

    Figure 11. PID controller and complementary filter working together

     

    PID Tuning

     

    Here are some steps that will help us to get Kp, Ki, and Kd faster:

     

    • Set the Ki and Kd terms to 0, and adjust Kp so that the robot starts to oscillate (move back and forth) about the balance position. Kp should be large enough for the robot to move but not too large; otherwise, the movement will not be smooth.
    • With Kp set, increase Ki so that the robot accelerates faster when it is off balance. With Kp and Ki properly tuned, the robot should be able to self-balance for at least a few seconds.
    • Finally, increase Kd so that the robot will move about its balanced position more gently. There shouldn't be any significant overshoots.

     

    We build this PID controller inside method PID in the JBalancePI class, as shown in Listing 2.

     

    //Set- point values used in PID controller private final float Kp = 17F;
     private final float Kd = 840F;
     private final float Ki = 0.1F;
    
    /*
     * Proportional, Integral, Derivative control.
     */
     private void PID() {
     long now = System.currentTimeMillis();
     int timeChange = (int) (now - lastTime);
     lastTime = now;
     float error = angle_filtered;  // Proportion
     errSum += error * timeChange;  // Integration
     float dErr = (error - lastErr) / timeChange;  // Differentiation
     float output = Kp * error + Ki * errSum + Kd * dErr;
     lastErr = error;
     LOutput = output - Turn_Speed + Run_Speed;
     ROutput = output + Turn_Speed + Run_Speed;
     }
    

    Listing 2. PID Controller method

     

    Summary of the Code We Will Write

     

    Motor Driver Controller with PWM Pulse

     

    The PID controller produces the necessary data to create a PWM pulse to move the motors forward, move them back, or stop them. To do that, we create a class called L298Device that uses class GPIOPin and the class PCA9685Device from my previous article. See Listing 3.

     

    /**
     * Interface to L298N Mmotor Ddriver using GPIO bus
     *
     * @author Jose Cruz
     */
    public class L298Device {
    
          //Enable motor left
          private GPIOPin IN1 = null;
          private GPIOPin IN2 = null;
          //Enable motor right
          private GPIOPin IN3 = null;
          private GPIOPin IN4 = null;
    
          //PWM port 0 for motor left
          private byte ENA = 0;
          //PWM port 1 for motor right
          private byte ENB = 1;
    
          //Define PWM motor object
          PCA9685Device pwm;
    

    Listing 3. Creating the L298Device class

    Next, we define a constructor L298Device that creates a device at the defined GPIO pins and a PWM object, as shown in Listing 4.

     

    /**
     * Initialize GPIO IN1 to IN4 motor input terminal
     *
     * @param in1
     * @param in2
     * @param in3
     * @param in4
     */
     public L298Device(int in1, int in2, int in3, int in4) {
     try {
                // define device for IN1 pin
                IN1 = (GPIOPin) DeviceManager.open(
                        new GPIOPinConfig.Builder()
                        .setControllerNumber(0)
                        .setPinNumber(in1)
                        .setDirection(GPIOPinConfig.DIR_OUTPUT_ONLY)
                        .setDriveMode(GPIOPinConfig.MODE_OUTPUT_OPEN_DRAIN)    
                        .setInitValue(false)
                        .build());
    
                // define device for IN2 pin
                IN2 = (GPIOPin) DeviceManager.open(new GPIOPinConfig.Builder()
                        .setControllerNumber(0)
                        .setPinNumber(in2)
                        .setDirection(GPIOPinConfig.DIR_OUTPUT_ONLY)
                        .setDriveMode(GPIOPinConfig.MODE_OUTPUT_OPEN_DRAIN
                        .setInitValue(false)
                        .build());
                // define device for IN3 pin
                IN3 = (GPIOPin) DeviceManager.open(new GPIOPinConfig.Builder()
                        .setControllerNumber(0)
                        .setPinNumber(in3)
                        .setDirection(GPIOPinConfig.DIR_OUTPUT_ONLY)
                        .setDriveMode(GPIOPinConfig.MODE_OUTPUT_OPEN_DRAIN)
                        .setInitValue(false)
                        .build());
                // define device for IN4 pin
                IN4 = (GPIOPin) DeviceManager.open(new GPIOPinConfig.Builder()
                        .setControllerNumber(0)
                        .setPinNumber(in4)
                        .setDirection(GPIOPinConfig.DIR_OUTPUT_ONLY)
                        .setDriveMode(GPIOPinConfig.MODE_OUTPUT_OPEN_DRAIN)
                        .setInitValue(false)
                        .build());
    
                pwm = new PCA9685Device();
                pwm.setPWMFreq(1000);
          } catch (IOException ex) {
                Logger.getGlobal().log(Level.WARNING, ex.getMessage());
          }
     }
    

     

    Listing 4. Initializing GPIO pins and creating pwm object

     

    Then create methods to move forward (moveL_Forward, moveR_Forward), move back (moveL_Back, moveR_Back), and stop both robot motors (stopL, stopR), as shown in Listing 5.

     

      /**

          * Motor left move forward

          */

          public void moveL_Forward() {

              try {

                  IN1.setValue(false);

                  IN2.setValue(true);

              } catch (IOException ex) {

                    Logger.getGlobal().log(Level.WARNING, ex.getMessage());

          }

    }

     

    /**

    * Motor left move back

    */

    public void moveL_Back() {

          try {

              IN1.setValue(true);

              IN2.setValue(false);

              } catch (IOException ex) {   

                  Logger.getGlobal().log(Level.WARNING, ex.getMessage());

          }

    }

     

    /**

    * Motor right move forward

    */

    public void moveR_Forward() {

              try {

                IN3.setValue(false);

                IN4.setValue(true);

              } catch (IOException ex) {

              Logger.getGlobal().log(Level.WARNING, ex.getMessage());

          }

    }

     

    /**

    * Motor right move back

    */

    public void moveR_Back() {

              try {

                IN3.setValue(true);

                IN4.setValue(false);

              } catch (IOException ex) {

                  Logger.getGlobal().log(Level.WARNING, ex.getMessage());

          }

    }

     

    /**

    * Stop motor left

    */

    public void stopL() {

              try {

                IN1.setValue(true);

                IN2.setValue(true);

                motorL_PWM((short) 0);

              } catch (IOException ex) {

                  Logger.getGlobal().log(Level.WARNING, ex.getMessage());

          }

    }

     

    /**

    * Stop motor right

    */

    public void stopR() {

              try {

                IN3.setValue(true);

                IN4.setValue(true);

                motorR_PWM((short) 0);

              } catch (IOException ex) {

              Logger.getGlobal().log(Level.WARNING, ex.getMessage());

          }

    }

     

     

    Listing 5. Methods to move and stop both motors

     

    To start moving the motors, we need set the PWM pulse to the desired frequency by using the pwm object, as shown in Listing 6.

     

          /**

            * Set PWM pulse for motor left speed

            * @param _pwm

            */

              public void motorL_PWM(short _pwm) {

                  pwm.setPWM(ENA, (short) 0, _pwm);    }

            /**

              * Set PWM pulse for motor right speed

              * @param _pwm

              */

              public void motorR_PWM(short _pwm) {

              pwm.setPWM(ENB, (short) 0, _pwm);

     

    Listing 6. Create PWM pulses at desired frequency for both motors

     

    Complete JBalancePI Class

     

    Using the L298Device class, we create a motor control method PWMControl that uses values calculated inside PID controller methods PID, LOutput,  and ROutput to control the movement, direction, and velocity, as shown in Listing 7.

     

        /*

        * PWM motor control

        */

        private void PWMControl() {

              if (LOutput > 0) {

                  motor.moveL_Forward();

        } else if (LOutput < 0) {

              motor.moveL_Back();

        } else {

              motor.stopL();

        }

            if (ROutput > 0) {

              motor.moveR_Forward();

        } else if (ROutput < 0) {

              motor.moveR_Back();

        } else {

              motor.stopR();

          }

              motor.motorL_PWM((short) (Math.min(4095, Math.abs(LOutput) * 4095 / 256)));

              motor.motorR_PWM((short) (Math.min(4095, Math.abs(ROutput) * 4095 / 256)));

     

    Listing 7. Defining the type of movement and creating a PWM pulse for velocity control

     

    We create a feedback control loop, as shown in Figure 1, with the thread class ControlLoop, as shown in Listing 8.

     

    /*
        * Thread for moving and balancing robot
        */ 
         class ControlLoop extends Thread {    
              @Override    
         public void run() {        
              while (shouldRun) {            
                   Filter();            
                   Logger.getGlobal().log(Level.INFO, "Angle = " + angle_filtered);            
                   // If angle > 45 or < -45 then stop the robot            
                   if (Math.abs(angle_filtered) < 45) {                
                        PID();                
                        PWMControl();            
                   } else {                
                        motor.stopL();                
                        motor.stopR();                
                   // Keep reading accelerometer and gyroscope values after falling down                
                   for (int i = 0; i < 100; i++)                      
                        Filter();              
                   // Empty data and restart the robot automatically                
                   if (Math.abs(angle_filtered) < 45)                
                   {                    
                   for (int i = 0; i <= 500; i++) // Reset the robot and delay 2 seconds                    
                   {                        
                        angle_filtered = 0;                        
                        Filter();                        
                        errSum = Run_Speed = Turn_Speed = 0;                        
                        PID();                    
                    }                
              }              
         }        
    }        
    accel.close();        
    motor.close();    
    } 
    }         
    

    Listing 8. Feedback control loop thread class

     

    Let's now complete the MIDlet class with startApp and destroyApp methods to initialize the complementary filter, the PID controller, the motor controller, the accelerometer, and the gyroscope, and create a task to balance the robot with the feedback control loop., as shown in Listing 9.

     

      /**   
          *    
         */    
         public void startApp() {          
              loggerHandler.start();        
              Logger.getGlobal().setLevel(Level.INFO);          
              Logger.getGlobal().log(Level.INFO, "***** JBalancePi v1.3 Started *****");        
              try {            
                   //Activate MPU-6050 with I2C bus            
                   accel = new MPU6050Device();           
                   //Activate motor control with GPIO bus: In1=27, In2=22, In3=24, In4=25            
                   motor = new L298Device(27, 22, 24, 25);            
                   // Loop 200 times to get the real values when starting            
                   for (int i = 0; i < 200; i++) {                
                        Filter();            
                   }            
                   if (Math.abs(angle_filtered) < 45) // Start the robot after cleaning data            
                   {                
                        angle_filtered = 0;                
                        Filter();                
                        errSum = Run_Speed = Turn_Speed = 0;                
                        PID();            
                   }            
                   //Start move and balance thread            
                   controlLoopTask = new ControlLoop();            
                   controlLoopTask.start();          
                } catch (IOException ex) {            
                        Logger.getGlobal().log(Level.WARNING, ex.getMessage());        
              }    
             }      
      /**      
        *      
        * @param unconditional      
        */    
         @Override    
         public void destroyApp(boolean unconditional) {        
              //Stop thread        
              shouldRun = false;        
              Logger.getGlobal().log(Level.INFO, "***** JBalancePi v1.0 Stopped *****");      
         } 
    }
    

     

    Listing 9. MIDlet class methods startApp and destroyApp

     

    Performing Some Additional Configuration

     

    Before running our test MIDlets using NetBeans IDE 8.0.2, it is important to establish the appropriate API permissions. To do this, in the IDE, select project JavaMEDemos, and then right-click and select Properties to show the Project Properties window.  Select Application Descriptor, and then select the API Permissions tab. Include all the permissions shown in Figure 12.

     

    f12.png

    Figure 12. Establishing API permissions

     

    Conclusion

     

    In this article, we saw how to construct a simple two-wheeled balancing robot using some modules such as the MPU-6050 accelerometer and gyroscope, the PCA9685 PWM servo driver, and the L298Nmotor driver.

     

    It is important to establish some initial values for PID constants Kp, Ki, and Kd and adjust them until the robot is balanced. In the next article in this series, we will connect an OLED display and some buttons to modify the values dynamically.

     

    We are opening a lot of possibilities for remote control of this robot, such as Bluetooth, Wi-Fi, REST, Twitter, web interfaces, and gestures with Kinect. In subsequent articles, we will see how we can implement remote control.

     

    About the Author

     

    Jose Cruzis a software engineer who has been working with Java since 1998. He is a lead developer of Java, Java ME, and Java EE at Ferreteria EPA C.A. in Venezuela. From an early age, his hobby has been electronics. This has led him to combine computing with electronics and develop projects where Java and embedded devices such as Arduino and Raspberry Pi are used.

    Join the Conversation

     

    Join the Java community conversation on Facebook, Twitter, and the Oracle Java Blog!