|
gyro code
we used a gyro to balance our arms but the gyro would return to center after about a second or two so we made a pendulum.
here is our code before we took it out. you may need to tweak some variables.
If p3_sw_top = 1 then
If p3_y > 127 then
If balancing_motor_limit_forward = 0 then balancing_motor = p3_y
else
if p3_y < 127 then
if balancing_motor_limit_reverse = 0 then balancing_motor = p3_y
endif
endif
else
If state = 0 then
balancing_motor = 183
If balancing_motor_limit_forward = 1 then balancing_motor = 127
If gyro => 180 then state = 1
else
If state = 1 then
balancing_motor = 63
If balancing_motor_limit_reverse = 1 then balancing_motor = 127
counter = counter + 1
If counter => 30 then
state = 2
endif
else
If state = 2 then balancing_motor = 127
endif
endif
endif
|