def _balance(self):
        """Make the robot balance."""
        while True and not self.stop_balance.is_set():

            # Reset the motors
            self.motor_left.reset()  # Reset the encoder
            self.motor_right.reset()
            self.motor_left.run_direct()  # Set to run direct mode
            self.motor_right.run_direct()

            # Initialize variables representing physical signals
            # (more info on these in the docs)

            # The angle of "the motor", measured in raw units,
            # degrees for the EV3).
            # We will take the average of both motor positions as
            # "the motor" angle, which is essentially how far the middle
            # of the robot has travelled.
            motor_angle_raw = 0

            # The angle of the motor, converted to RAD (2*pi RAD
            # equals 360 degrees).
            motor_angle = 0

            # The reference angle of the motor. The robot will attempt to
            # drive forward or backward, such that its measured position
            motor_angle_ref = 0
            # equals this reference (or close enough).

            # The error: the deviation of the measured motor angle from the
            # reference. The robot attempts to make this zero, by driving
            # toward the reference.
            motor_angle_error = 0

            # We add up all of the motor angle error in time. If this value
            # gets out of hand, we can use it to drive the robot back to
            # the reference position a bit quicker.
            motor_angle_error_acc = 0

            # The motor speed, estimated by how far the motor has turned in
            # a given amount of time.
            motor_angular_speed = 0

            # The reference speed during manouvers: how fast we would like
            # to drive, measured in RAD per second.
            motor_angular_speed_ref = 0

            # The error: the deviation of the motor speed from the
            # reference speed.
            motor_angular_speed_error = 0

            # The 'voltage' signal we send to the motor.
            # We calculate a new value each time, just right to keep the
            # robot upright.
            motor_duty_cycle = 0

            # The raw value from the gyro sensor in rate mode.
            gyro_rate_raw = 0

            # The angular rate of the robot (how fast it is falling forward
            # or backward), measured in RAD per second.
            gyro_rate = 0

            # The gyro doesn't measure the angle of the robot, but we can
            # estimate this angle by keeping track of the gyro_rate value
            # in time.
            gyro_est_angle = 0

            # Over time, the gyro rate value can drift. This causes the
            # sensor to think it is moving even when it is perfectly still.
            # We keep track of this offset.
            gyro_offset = 0

            # Start
            log.info("Hold robot upright. Press touch sensor to start.")
            self.sound.speak("Press touch sensor to start.")

            self.touch.wait_for_bump()

            # Read battery voltage
            voltage_idle = self.power_supply.measured_volts
            voltage_comp = self.power_voltage_nominal / voltage_idle

            # Offset to limit friction deadlock
            friction_offset = int(round(self.pwr_friction_offset_nom *
                                        voltage_comp))

            # Timing settings for the program
            # Time of each loop, measured in seconds.
            loop_time_target = self.timing_loop_msec / 1000
            loop_count = 0  # Loop counter, starting at 0

            # A deque (a fifo array) which we'll use to keep track of
            # previous motor positions, which we can use to calculate the
            # rate of change (speed)
            motor_angle_hist =\
                deque([0], self.motor_angle_history_length)

            # The rate at which we'll update the gyro offset (precise
            # definition given in docs)
            gyro_drift_comp_rate =\
                self.gyro_drift_compensation_factor *\
                loop_time_target * RAD_PER_SEC_PER_RAW_GYRO_UNIT

            # Calibrate Gyro
            log.info("-----------------------------------")
            log.info("Calibrating...")

            # As you hold the robot still, determine the average sensor
            # value of 100 samples
            gyro_calibrate_count = 100
            for i in range(gyro_calibrate_count):
                gyro_offset = gyro_offset + self._fast_read(self.gyro_file)
                time.sleep(0.01)
            gyro_offset = gyro_offset / gyro_calibrate_count

            # Print the result
            log.info("gyro_offset: " + str(gyro_offset))
            log.info("-----------------------------------")
            log.info("GO!")
            log.info("-----------------------------------")
            log.info("Press Touch Sensor to re-start.")
            log.info("-----------------------------------")
            self.sound.beep()

            # Remember start time
            prog_start_time = time.time()

            if self.debug:
                # Data logging
                data = OrderedDict()
                loop_times = OrderedDict()
                data['loop_times'] = loop_times
                gyro_readings = OrderedDict()
                data['gyro_readings'] = gyro_readings

            # Initial fast read touch sensor value
            touch_pressed = False

            # Driving and Steering
            speed, steering = (0, 0)

            # Record start time of loop
            loop_start_time = time.time()

            # Balancing Loop
            while not touch_pressed and not self.stop_balance.is_set():

                loop_count += 1

                # Check for drive instructions and set speed / steering
                try:
                    speed, steering = self.drive_queue.get_nowait()
                    self.drive_queue.task_done()
                except queue.Empty:
                    pass

                # Read the touch sensor (the kill switch)
                touch_pressed = self._fast_read(self.touch_file)

                # Read the Motor Position
                motor_angle_raw = ((self._fast_read(self.encoder_left_file) +
                                   self._fast_read(self.encoder_right_file)) /
                                   2.0)
                motor_angle = motor_angle_raw * RAD_PER_RAW_MOTOR_UNIT

                # Read the Gyro
                gyro_rate_raw = self._fast_read(self.gyro_file)

                # Busy wait for the loop to reach target time length
                loop_time = 0
                while(loop_time < loop_time_target):
                    loop_time = time.time() - loop_start_time
                    time.sleep(0.001)

                # Calculate most recent loop time
                loop_time = time.time() - loop_start_time

                # Set start time of next loop
                loop_start_time = time.time()

                if self.debug:
                    # Log gyro data and loop time
                    time_of_sample = time.time() - prog_start_time
                    gyro_readings[time_of_sample] = gyro_rate_raw
                    loop_times[time_of_sample] = loop_time * 1000.0

                # Calculate gyro rate
                gyro_rate = (gyro_rate_raw - gyro_offset) *\
                    RAD_PER_SEC_PER_RAW_GYRO_UNIT

                # Calculate Motor Parameters
                motor_angular_speed_ref =\
                    speed * RAD_PER_SEC_PER_PERCENT_SPEED
                motor_angle_ref = motor_angle_ref +\
                    motor_angular_speed_ref * loop_time_target
                motor_angle_error = motor_angle - motor_angle_ref

                # Compute Motor Speed
                motor_angular_speed =\
                    ((motor_angle - motor_angle_hist[0]) /
                     (self.motor_angle_history_length * loop_time_target))
                motor_angular_speed_error = motor_angular_speed
                motor_angle_hist.append(motor_angle)

                # Compute the motor duty cycle value
                motor_duty_cycle =\
                    (self.gain_gyro_angle * gyro_est_angle +
                     self.gain_gyro_rate * gyro_rate +
                     self.gain_motor_angle * motor_angle_error +
                     self.gain_motor_angular_speed *
                     motor_angular_speed_error +
                     self.gain_motor_angle_error_accumulated *
                     motor_angle_error_acc)

                # Apply the signal to the motor, and add steering
                self._set_duty(self.dc_right_file, motor_duty_cycle + steering,
                               friction_offset, voltage_comp)
                self._set_duty(self.dc_left_file, motor_duty_cycle - steering,
                               friction_offset, voltage_comp)

                # Update angle estimate and gyro offset estimate
                gyro_est_angle = gyro_est_angle + gyro_rate *\
                    loop_time_target
                gyro_offset = (1 - gyro_drift_comp_rate) *\
                    gyro_offset + gyro_drift_comp_rate * gyro_rate_raw

                # Update Accumulated Motor Error
                motor_angle_error_acc = motor_angle_error_acc +\
                    motor_angle_error * loop_time_target

            # Closing down & Cleaning up

            # Loop end time, for stats
            prog_end_time = time.time()

            # Turn off the motors
            self._fast_write(self.dc_left_file, 0)
            self._fast_write(self.dc_right_file, 0)

            # Wait for the Touch Sensor to be released
            while self.touch.is_pressed:
                time.sleep(0.01)

            # Calculate loop time
            avg_loop_time = (prog_end_time - prog_start_time) / loop_count
            log.info("Loop time:" + str(avg_loop_time * 1000) + "ms")

            # Print a stop message
            log.info("-----------------------------------")
            log.info("STOP")
            log.info("-----------------------------------")

            if self.debug:
                # Dump logged data to file
                with open("data.txt", 'w') as data_file:
                    json.dump(data, data_file)
Make the robot balance.