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:
This is the whole robot the BBB is on the middle rack.
MPU6050 from the side and the motor driver next to it hidden by the wires.
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.
https://www.youtube.com/watch?v=q4WY1mVJZHEimport smbus
import math
import time
import Adafruit_BBIO.PWM as PWM
import Adafruit_BBIO.GPIO as GPIOerror=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 = 0rightpwm = “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 = 0x6cgyro_scale = 131.0
accel_scale = 16384.0address = 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;"><</span><span style="color: #308080;"><</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;"><</span><span style="color: #308080;"><</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;"><</span><span style="color: #308080;"><</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;"><</span><span style="color: #308080;"><</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;"><</span><span style="color: #308080;"><</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;"><</span><span style="color: #308080;"><</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 valdef 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 nbus = 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;">></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;"><</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.