Help with Beaglebone Black balancing robot

Hello everyone, I want to start by saying this question post might become long and I hope you guys hang on and are able to help me. So here we go :)

First of all the robot is barely able to balance. Here is a video of the robot trying to balance. The problem seems to be that the robot is not able to reach the setpoint fast enough. This is a speculation so you guys give me your ideas on what the problem is also.

  HERE IS THE VIDEO (I couldn't embed it in the post)

 As you can see in the first 30 seconds of video the robot is barely able to balance. Starting from 0:30 you can see me tilting the robot; while I do that you can see the robot accelerating towards the directions it is "falling". 

Information about the hardware:

The acrylic itself was laser cut at school it is approximately 12 inches in width and 3 inches in height. The battery is currently placed at the top, it is rated at  9.6V 1500Mah . I am using the Beaglebone Black Rev C running debian. The motors are 200rpm motors I bought of some indian website rated for 12V. I am using the Pololu TB6612FNG Dual Motor Driver Carrier, The IMU is a MPU6050; trying to get this working with the BBB with such minimal documentation for the IMU itself was hell! 

Robot Pictures:

20140723_210311.jpg

This is the whole robot the BBB is on the middle rack.

20140723_210214.jpg

MPU6050 from the side and the motor driver next to it hidden by the wires.

20140723_210244.jpg

Motors and wheels the first deck holds the motor driver and MPU6050

 This shows the MPU6050 in the center.

Information about the Software:

The program was made in Python; I am not sure if the problem is caused by the fact that I’m using python since it is a relatively slow language which could result in slower reactions. Also I am using the Adafruit_BBIO module/library for GPIO and PWM control. For those who are not familiar with the library; 0 is OFF and 100 is TOTAL ON for the PWM range.

To calculate the angle of tilt I am using a complementary filter merging the Gyro and Accelerometer readings. The setpoint is at 0° degrees that is the measurement when it is completely vertical. Then I used a PID algorithm to make it reach the setpoint smoothly. The best constants seem to be Kp=0.62, Ki=1.55, Kd=0.52 these were the constants used in the video.

Here is the current code:

The code might seem very primitive and stupid for some of you. :slight_smile:

import smbus
import math
import time
import Adafruit_BBIO.PWM as PWM
import Adafruit_BBIO.GPIO as GPIO

error=0

kp = float(raw_input())
ki = float(raw_input())
kd = float(raw_input())
integrated_error = 0
last_error=0
preverror = 0
previous_i = 0
lastprop = 0
integral = 0
motorspeed = 0
speed = 0

rightpwm = “P8_13”
leftpwm = “P8_19”
GPIO.setup(“P8_8”, GPIO.OUT)
GPIO.setup(“P8_10”, GPIO.OUT)
GPIO.setup(“P8_7”, GPIO.OUT)
GPIO.setup(“P8_9”, GPIO.OUT)

PWM.start(rightpwm, 0)
PWM.start(leftpwm, 0)

# Power management registers

power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

gyro_scale = 131.0
accel_scale = 16384.0

address = 0x68 # This is the address value read via the i2cdetect command

def read_all():
raw_gyro_data = bus.read_i2c_block_data(address, 0x43, 6)
raw_accel_data = bus.read_i2c_block_data(address, 0x3b, 6)

gyro_scaled_x <span style="color: #308080;">=</span> twos_compliment<span style="color: #308080;">(</span><span style="color: #308080;">(</span>raw_gyro_data<span style="color: #308080;">[</span><span style="color: #008c00;">0</span><span style="color: #308080;">]</span> <span style="color: #308080;">&lt;</span><span style="color: #308080;">&lt;</span> <span style="color: #008c00;">8</span><span style="color: #308080;">)</span> <span style="color: #308080;">+</span> raw_gyro_data<span style="color: #308080;">[</span><span style="color: #008c00;">1</span><span style="color: #308080;">]</span><span style="color: #308080;">)</span> <span style="color: #308080;">/</span> gyro_scale
gyro_scaled_y <span style="color: #308080;">=</span> twos_compliment<span style="color: #308080;">(</span><span style="color: #308080;">(</span>raw_gyro_data<span style="color: #308080;">[</span><span style="color: #008c00;">2</span><span style="color: #308080;">]</span> <span style="color: #308080;">&lt;</span><span style="color: #308080;">&lt;</span> <span style="color: #008c00;">8</span><span style="color: #308080;">)</span> <span style="color: #308080;">+</span> raw_gyro_data<span style="color: #308080;">[</span><span style="color: #008c00;">3</span><span style="color: #308080;">]</span><span style="color: #308080;">)</span> <span style="color: #308080;">/</span> gyro_scale
gyro_scaled_z <span style="color: #308080;">=</span> twos_compliment<span style="color: #308080;">(</span><span style="color: #308080;">(</span>raw_gyro_data<span style="color: #308080;">[</span><span style="color: #008c00;">4</span><span style="color: #308080;">]</span> <span style="color: #308080;">&lt;</span><span style="color: #308080;">&lt;</span> <span style="color: #008c00;">8</span><span style="color: #308080;">)</span> <span style="color: #308080;">+</span> raw_gyro_data<span style="color: #308080;">[</span><span style="color: #008c00;">5</span><span style="color: #308080;">]</span><span style="color: #308080;">)</span> <span style="color: #308080;">/</span> gyro_scale

accel_scaled_x <span style="color: #308080;">=</span> twos_compliment<span style="color: #308080;">(</span><span style="color: #308080;">(</span>raw_accel_data<span style="color: #308080;">[</span><span style="color: #008c00;">0</span><span style="color: #308080;">]</span> <span style="color: #308080;">&lt;</span><span style="color: #308080;">&lt;</span> <span style="color: #008c00;">8</span><span style="color: #308080;">)</span> <span style="color: #308080;">+</span> raw_accel_data<span style="color: #308080;">[</span><span style="color: #008c00;">1</span><span style="color: #308080;">]</span><span style="color: #308080;">)</span> <span style="color: #308080;">/</span> accel_scale
accel_scaled_y <span style="color: #308080;">=</span> twos_compliment<span style="color: #308080;">(</span><span style="color: #308080;">(</span>raw_accel_data<span style="color: #308080;">[</span><span style="color: #008c00;">2</span><span style="color: #308080;">]</span> <span style="color: #308080;">&lt;</span><span style="color: #308080;">&lt;</span> <span style="color: #008c00;">8</span><span style="color: #308080;">)</span> <span style="color: #308080;">+</span> raw_accel_data<span style="color: #308080;">[</span><span style="color: #008c00;">3</span><span style="color: #308080;">]</span><span style="color: #308080;">)</span> <span style="color: #308080;">/</span> accel_scale
accel_scaled_z <span style="color: #308080;">=</span> twos_compliment<span style="color: #308080;">(</span><span style="color: #308080;">(</span>raw_accel_data<span style="color: #308080;">[</span><span style="color: #008c00;">4</span><span style="color: #308080;">]</span> <span style="color: #308080;">&lt;</span><span style="color: #308080;">&lt;</span> <span style="color: #008c00;">8</span><span style="color: #308080;">)</span> <span style="color: #308080;">+</span> raw_accel_data<span style="color: #308080;">[</span><span style="color: #008c00;">5</span><span style="color: #308080;">]</span><span style="color: #308080;">)</span> <span style="color: #308080;">/</span> accel_scale

<span style="color: #200080; font-weight: bold;">return</span> <span style="color: #308080;">(</span>gyro_scaled_x<span style="color: #308080;">,</span> gyro_scaled_y<span style="color: #308080;">,</span> gyro_scaled_z<span style="color: #308080;">,</span> accel_scaled_x<span style="color: #308080;">,</span> accel_scaled_y<span style="color: #308080;">,</span> accel_scaled_z<span style="color: #308080;">)</span>

def twos_compliment(val):
if (val >= 0x8000):
return -((65535 - val) + 1)
else:
return val

def dist(a, b):
return math.sqrt((a a) + (b b))

def get_y_rotation(x,y,z):
radians = math.atan2(x, dist(y,z))
return -math.degrees(radians)

def get_x_rotation(x,y,z):
radians = math.atan2(y, dist(x,z))
return math.degrees(radians)
def forward():

speed1 <span style="color: #308080;">=</span> constrain<span style="color: #308080;">(</span>speed<span style="color: #308080;">,</span> <span style="color: #008c00;">0</span><span style="color: #308080;">,</span> <span style="color: #008c00;">100</span><span style="color: #308080;">)</span>
PWM<span style="color: #308080;">.</span>set_duty_cycle<span style="color: #308080;">(</span>rightpwm<span style="color: #308080;">,</span> speed1<span style="color: #308080;">)</span>
PWM<span style="color: #308080;">.</span>set_duty_cycle<span style="color: #308080;">(</span>leftpwm<span style="color: #308080;">,</span> speed1<span style="color: #308080;">)</span>
PWM<span style="color: #308080;">.</span>set_frequency<span style="color: #308080;">(</span>rightpwm<span style="color: #308080;">,</span> <span style="color: #008c00;">2000</span><span style="color: #308080;">)</span>
PWM<span style="color: #308080;">.</span>set_frequency<span style="color: #308080;">(</span>leftpwm<span style="color: #308080;">,</span> <span style="color: #008c00;">2000</span><span style="color: #308080;">)</span>
<span style="color: #200080; font-weight: bold;">print</span> <span style="color: #1060b6;">"forward:   "</span><span style="color: #308080;">,</span>speed
GPIO<span style="color: #308080;">.</span>output<span style="color: #308080;">(</span><span style="color: #1060b6;">"P8_8"</span><span style="color: #308080;">,</span> GPIO<span style="color: #308080;">.</span>HIGH<span style="color: #308080;">)</span>
GPIO<span style="color: #308080;">.</span>output<span style="color: #308080;">(</span><span style="color: #1060b6;">"P8_10"</span><span style="color: #308080;">,</span> GPIO<span style="color: #308080;">.</span>LOW<span style="color: #308080;">)</span>
GPIO<span style="color: #308080;">.</span>output<span style="color: #308080;">(</span><span style="color: #1060b6;">"P8_7"</span><span style="color: #308080;">,</span> GPIO<span style="color: #308080;">.</span>LOW<span style="color: #308080;">)</span>
GPIO<span style="color: #308080;">.</span>output<span style="color: #308080;">(</span><span style="color: #1060b6;">"P8_9"</span><span style="color: #308080;">,</span> GPIO<span style="color: #308080;">.</span>HIGH<span style="color: #308080;">)</span>

def backward():

speed2 <span style="color: #308080;">=</span> constrain<span style="color: #308080;">(</span><span style="color: #308080;">-</span>speed<span style="color: #308080;">,</span> <span style="color: #008c00;">0</span><span style="color: #308080;">,</span> <span style="color: #008c00;">100</span><span style="color: #308080;">)</span>
PWM<span style="color: #308080;">.</span>set_duty_cycle<span style="color: #308080;">(</span>rightpwm<span style="color: #308080;">,</span> speed2<span style="color: #308080;">)</span>
PWM<span style="color: #308080;">.</span>set_duty_cycle<span style="color: #308080;">(</span>leftpwm<span style="color: #308080;">,</span> speed2<span style="color: #308080;">)</span>
PWM<span style="color: #308080;">.</span>set_frequency<span style="color: #308080;">(</span>rightpwm<span style="color: #308080;">,</span> <span style="color: #008c00;">2000</span><span style="color: #308080;">)</span>
PWM<span style="color: #308080;">.</span>set_frequency<span style="color: #308080;">(</span>leftpwm<span style="color: #308080;">,</span> <span style="color: #008c00;">2000</span><span style="color: #308080;">)</span>

<span style="color: #200080; font-weight: bold;">print</span> <span style="color: #1060b6;">"backward:   "</span><span style="color: #308080;">,</span> speed2
GPIO<span style="color: #308080;">.</span>output<span style="color: #308080;">(</span><span style="color: #1060b6;">"P8_8"</span><span style="color: #308080;">,</span> GPIO<span style="color: #308080;">.</span>LOW<span style="color: #308080;">)</span>
GPIO<span style="color: #308080;">.</span>output<span style="color: #308080;">(</span><span style="color: #1060b6;">"P8_10"</span><span style="color: #308080;">,</span> GPIO<span style="color: #308080;">.</span>HIGH<span style="color: #308080;">)</span>
GPIO<span style="color: #308080;">.</span>output<span style="color: #308080;">(</span><span style="color: #1060b6;">"P8_7"</span><span style="color: #308080;">,</span> GPIO<span style="color: #308080;">.</span>HIGH<span style="color: #308080;">)</span>
GPIO<span style="color: #308080;">.</span>output<span style="color: #308080;">(</span><span style="color: #1060b6;">"P8_9"</span><span style="color: #308080;">,</span> GPIO<span style="color: #308080;">.</span>LOW<span style="color: #308080;">)</span>

def stop():
#print “stop”

GPIO<span style="color: #308080;">.</span>output<span style="color: #308080;">(</span><span style="color: #1060b6;">"P8_8"</span><span style="color: #308080;">,</span> GPIO<span style="color: #308080;">.</span>LOW<span style="color: #308080;">)</span>
GPIO<span style="color: #308080;">.</span>output<span style="color: #308080;">(</span><span style="color: #1060b6;">"P8_10"</span><span style="color: #308080;">,</span> GPIO<span style="color: #308080;">.</span>LOW<span style="color: #308080;">)</span>
GPIO<span style="color: #308080;">.</span>output<span style="color: #308080;">(</span><span style="color: #1060b6;">"P8_7"</span><span style="color: #308080;">,</span> GPIO<span style="color: #308080;">.</span>LOW<span style="color: #308080;">)</span>
GPIO<span style="color: #308080;">.</span>output<span style="color: #308080;">(</span><span style="color: #1060b6;">"P8_9"</span><span style="color: #308080;">,</span> GPIO<span style="color: #308080;">.</span>LOW<span style="color: #308080;">)</span>

def constrain(n, minn, maxn):
if n < minn:
return minn
elif n > maxn:
return maxn
else:
return n

bus = smbus.SMBus(1)

# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)
while(1):
now = time.time()

K <span style="color: #308080;">=</span> <span style="color: #008000;">0.98</span>
K1 <span style="color: #308080;">=</span> <span style="color: #008c00;">1</span> <span style="color: #308080;">-</span> K

time_diff <span style="color: #308080;">=</span> <span style="color: #008000;">0.01</span>

<span style="color: #308080;">(</span>gyro_scaled_x<span style="color: #308080;">,</span> gyro_scaled_y<span style="color: #308080;">,</span> gyro_scaled_z<span style="color: #308080;">,</span> accel_scaled_x<span style="color: #308080;">,</span> accel_scaled_y<span style="color: #308080;">,</span> accel_scaled_z<span style="color: #308080;">)</span> <span style="color: #308080;">=</span> read_all<span style="color: #308080;">(</span><span style="color: #308080;">)</span>

last_x <span style="color: #308080;">=</span> get_x_rotation<span style="color: #308080;">(</span>accel_scaled_x<span style="color: #308080;">,</span> accel_scaled_y<span style="color: #308080;">,</span> accel_scaled_z<span style="color: #308080;">)</span>
last_y <span style="color: #308080;">=</span> get_y_rotation<span style="color: #308080;">(</span>accel_scaled_x<span style="color: #308080;">,</span> accel_scaled_y<span style="color: #308080;">,</span> accel_scaled_z<span style="color: #308080;">)</span>

gyro_offset_x <span style="color: #308080;">=</span> gyro_scaled_x 
gyro_offset_y <span style="color: #308080;">=</span> gyro_scaled_y

gyro_total_x <span style="color: #308080;">=</span> <span style="color: #308080;">(</span>last_x<span style="color: #308080;">)</span> <span style="color: #308080;">-</span> gyro_offset_x
gyro_total_y <span style="color: #308080;">=</span> <span style="color: #308080;">(</span>last_y<span style="color: #308080;">)</span> <span style="color: #308080;">-</span> gyro_offset_y

<span style="color: #200080; font-weight: bold;">for</span> i <span style="color: #200080; font-weight: bold;">in</span> <span style="color: #e34adc;">range</span><span style="color: #308080;">(</span><span style="color: #008c00;">0</span><span style="color: #308080;">,</span> <span style="color: #e34adc;">int</span><span style="color: #308080;">(</span><span style="color: #008000;">3.0</span> <span style="color: #308080;">/</span> time_diff<span style="color: #308080;">)</span><span style="color: #308080;">)</span><span style="color: #308080;">:</span>
    time<span style="color: #308080;">.</span>sleep<span style="color: #308080;">(</span>time_diff <span style="color: #308080;">-</span> <span style="color: #008000;">0.005</span><span style="color: #308080;">)</span> 
    
    <span style="color: #308080;">(</span>gyro_scaled_x<span style="color: #308080;">,</span> gyro_scaled_y<span style="color: #308080;">,</span> gyro_scaled_z<span style="color: #308080;">,</span> accel_scaled_x<span style="color: #308080;">,</span> accel_scaled_y<span style="color: #308080;">,</span> accel_scaled_z<span style="color: #308080;">)</span> <span style="color: #308080;">=</span> read_all<span style="color: #308080;">(</span><span style="color: #308080;">)</span>
    
    gyro_scaled_x <span style="color: #308080;">-</span><span style="color: #308080;">=</span> gyro_offset_x
    gyro_scaled_y <span style="color: #308080;">-</span><span style="color: #308080;">=</span> gyro_offset_y
    
    gyro_x_delta <span style="color: #308080;">=</span> <span style="color: #308080;">(</span>gyro_scaled_x <span style="color: #308080;">*</span> time_diff<span style="color: #308080;">)</span>
    gyro_y_delta <span style="color: #308080;">=</span> <span style="color: #308080;">(</span>gyro_scaled_y <span style="color: #308080;">*</span> time_diff<span style="color: #308080;">)</span>

    gyro_total_x <span style="color: #308080;">+</span><span style="color: #308080;">=</span> gyro_x_delta
    gyro_total_y <span style="color: #308080;">+</span><span style="color: #308080;">=</span> gyro_y_delta

    rotation_x <span style="color: #308080;">=</span> get_x_rotation<span style="color: #308080;">(</span>accel_scaled_x<span style="color: #308080;">,</span> accel_scaled_y<span style="color: #308080;">,</span> accel_scaled_z<span style="color: #308080;">)</span>
    rotation_y <span style="color: #308080;">=</span> get_y_rotation<span style="color: #308080;">(</span>accel_scaled_x<span style="color: #308080;">,</span> accel_scaled_y<span style="color: #308080;">,</span> accel_scaled_z<span style="color: #308080;">)</span>

    last_x <span style="color: #308080;">=</span> K <span style="color: #308080;">*</span> <span style="color: #308080;">(</span>last_x <span style="color: #308080;">+</span> gyro_x_delta<span style="color: #308080;">)</span> <span style="color: #308080;">+</span> <span style="color: #308080;">(</span>K1 <span style="color: #308080;">*</span> rotation_x<span style="color: #308080;">)</span>
    last_y <span style="color: #308080;">=</span> K <span style="color: #308080;">*</span> <span style="color: #308080;">(</span>last_y <span style="color: #308080;">+</span> gyro_y_delta<span style="color: #308080;">)</span> <span style="color: #308080;">+</span> <span style="color: #308080;">(</span>K1 <span style="color: #308080;">*</span> rotation_y<span style="color: #308080;">)</span>


    error <span style="color: #308080;">=</span> <span style="color: #008000;">0.0</span> <span style="color: #308080;">+</span> rotation_y
    p <span style="color: #308080;">=</span> kp<span style="color: #308080;">*</span>error
    integrated_error <span style="color: #308080;">=</span> constrain<span style="color: #308080;">(</span>integrated_error<span style="color: #308080;">,</span><span style="color: #308080;">-</span><span style="color: #008c00;">50</span><span style="color: #308080;">,</span><span style="color: #008c00;">50</span><span style="color: #308080;">)</span> <span style="color: #308080;">+</span> error
    i <span style="color: #308080;">=</span> ki<span style="color: #308080;">*</span>integrated_error
    d <span style="color: #308080;">=</span> kd<span style="color: #308080;">*</span><span style="color: #308080;">(</span>error <span style="color: #308080;">-</span> last_error<span style="color: #308080;">)</span>
    

    speed <span style="color: #308080;">=</span> constrain<span style="color: #308080;">(</span><span style="color: #308080;">(</span>p <span style="color: #308080;">+</span> i <span style="color: #308080;">+</span> d<span style="color: #308080;">)</span><span style="color: #308080;">,</span> <span style="color: #308080;">-</span><span style="color: #008c00;">100</span><span style="color: #308080;">,</span> <span style="color: #008c00;">100</span><span style="color: #308080;">)</span>
last_error <span style="color: #308080;">=</span> error


    <span style="color: #595979;">#print speed #debug</span>

 	<span style="color: #200080; font-weight: bold;">if</span> error <span style="color: #308080;">&gt;</span> <span style="color: #008c00;">0</span><span style="color: #308080;">:</span>
    

        forward<span style="color: #308080;">(</span><span style="color: #308080;">)</span>
    <span style="color: #595979;">#print "forward"</span>
    


    <span style="color: #200080; font-weight: bold;">elif</span> error <span style="color: #308080;">&lt;</span> <span style="color: #008c00;">0</span><span style="color: #308080;">:</span>

    backward<span style="color: #308080;">(</span><span style="color: #308080;">)</span>

	    <span style="color: #595979;">#print "backward"</span>

 


Note: 

The Beaglebone is currently tethered to the laptop for power; later it will be powered from a battery pack once I get it working.

https://www.youtube.com/watch?v=q4WY1mVJZHE

Congratulations
You’ve done the difficult part in getting the robot built. I’m still in the cutting materials stage, but I don’t have access to a laser cutter now.

There is a library for the MPU 6050 available for the Arduino. It might be easy to convert.

A quick look at your code, the first thing I see that looks hinky is the twos_compliment function. On most machines, the twos compliment of x is -x. Basically it is a bit wise not of the number plus one.

In preparation for building my own, I’ve been reading papers. OK, I come from academia, sorry. The library I talked about (google for it and check the robots list for the last balancing bot which mentioned the library) filters things for you. If you don’t then you have to read the data sheet of the 6050 to see how fast both the accelerometer and the gyroscope update and how noisy they are.

It looks like you’re doing a Kahlman filter on the data which is useful, if not necessary.

You should probably be running this loop at a fixed interval. I’d suggest 10 or 20 times per second.

If you can’t get this speed out of the BBB itself, I would use one of the BBB’s PRUs which are extremely fast and are not tied to Linux.

Or put an Arduino or other small microprocessor to talk to the motor controller (a Teensy 3.1?).

Ps. I’d worry about getting it to balance in place before you work on moving the thing.

Thank you for spending the

Thank you for spending the time to read this  DangerousThing, I know which library you are talking about; I believe it is called the i2cdevlib it is a huge and complicated library and that would take forever to convert. About the part with the twos_compliment I think it is correct since I return the regative value of the number plus one; I could be wrong, but I feel like it is right.

For the data I am using a Complementary Filter I said that at the bottom of the post

I have been thinking about using the PRU, but I think it requires its own language.

Also in the video I was not trying to make it move I was showing how it was able to get to the setpoint when I tilt it and give it the sense that it is falling

For modern machines
For modern machines twos_compliment(x) = -x; at least for integer values. -x + 1 is not the 2s compliment of x.

The other definition (the bit wise negation of x + 1) is merely the definition. It’s much faster to just use -x rather than a function call with if statements and such. You’re just slowing things down and I’m not sure you’re getting the right answer.

And the library I was talking about seemed to be specifically for the IMU you’re using. I’ve never used it. I don’t even use the Arduino, though I should learn.

Also, I just wanted to know

Also, I just wanted to know if the PID algorithm is correct?

May I suggest a paper…
I worked for Penn State for too many years, so my first instinct is to search for papers on a subject.

Your type of robot is sometimes called an Inverse Pendulum Robot, which is what I searched on.

One of the papers I found that was the most clear (least unclear maybe) was “Balancing and Navigation Control of a Mobile Inverted Pendulum Robot using sensor fusion of low cost sensors”.

A mouthful, perhaps, but it gives all the equations.

You’ll probably need a Python library on matrices and one for the Kahlman filter.

This paper assumes that the accelerometer is slow and relatively clean, while the gyro is faster but prone to drift when integrated.

I’d include the paper, but I don’t have copyright ownership and it’s easy to find. I was just searching on Inverse Pendulum Robot. I like Google.

I like google as well!

I like google as well! :slight_smile:

I’ll go through the paper; thank you!

Hi everyone I tried it the

Hi everyone I tried it the way 6677 suggested which was

def twosComplement(val):

    return -val

, but it seems to be wrong because when I tilt the robot towards one side it works, but then when I tilt it to the other side it is stuck on -75. The code I was using before which was

def twos_compliment(val):
if (val >= 0x8000):
return -((65535 - val) + 1)
else:
return val

gave perfect values with 0 being perfectly vertical and tilting either side giving positive or negative values.

For example tilting right 45 degrees gave me 45, and tilting left gave me -45.

 

I don’t know why one works and the other doesn’t.

I am so not familiar with

I am so not familiar with the BBB but, the PID equations look correct in the code. It’s basically about “neutralizing” the errors your robot makes by multiplying them with a couple of constants. 

I think you can get your robot running through trial and error. Once you implement everything the others have mentioned regarding the code, you might want to lift the robot at a random height and manually tilt it just to note the wheel spins. But I guess that one is correct, judging by your video.

I have not built a balancing robot so please take eveything by a pint of salt, but I have a feeling that the robot is a bit too high? I had seen Bajdi’s balancing robot and a recent one here on LMR, they were “one-storey” shorter than yours, which means, they balanced better due to their low center of gravity. But that’s just an analysis from an intuitive point of view.

So, how about taking off “one-storey” from your robot because I think it’s hindering it’s movements and balance. Maybe that would just work out? Again, it’s just my instincts.

Oh, and by one-storey, I didn’t mean theirs was a one-storey platform and yours a two-storey. I just meant, it could be a tad too taller? Nice work anyways!

Thank you for spending the

Thank you for spending the time to read this!

I will do some more reasearch on inverted pendulums, but I think the higher it is the more stable. For example, try balancing a short stick on your finger, then compare that to balancing a very long stick with a weight at the top. The longer and more weight on top the easier it is to balance.

Regardless I will do some more research.

 

Once again Thank you

How is the build going?
How is the build going? I’m curious.