"segwaybot" problems, please help
Posted: 28 May 2011, 19:33
I'm trying to create a self-balancing robot using the HiTechnic gyro sensor, but I seem to be totally unable to get it to work.
First off, I'm trying to create my own bot design (rather than just exactly copy someone else's) based on the NXTWay-Gyro bot described here:
http://www.techbricks.nl/My-NXT-project ... robot.html
I can post a photo of the design if desired, but basically it's pretty much the same as most of the other designs except I have the axles about 4" below the brick, I'm using the small NXT 2.0 wheels, and the gyro sensor is placed about 1" above the brick with the ultrasonic sensor just above it. (I figured that a higher center of gravity might make it more stable though I'm not sure if I'm right about that.)
I'm basically having two problems. The first is that the "body angle" calculation seems to be resulting in wild numbers that make no sense. Here's the code for that:
(I'm using essentially the same code on the web page but I changed the variable names to something more descriptive in order to help me understand the code. The two character variable names weren't making much sense to me, especially since they seem to be based on abbreviations of dutch words or something.)
I don't entirely understand how body_angle is supposed to work, but when I display it on the screen it seems to jump up to 10,000+ even if the vertical angle is only moving back and forth by a few degrees. And it seems to jump around in a way that isn't proportional to the angle the bot is held. corrected_gyro_reading appears to be reasonable though, as does the bias compensation.
The second problem I'm having is that I don't understand these constants (10, 4, 200, etc) which I'm assuming I need to change given that the center of gravity of the bot and location of the gyro sensor are different.
Below is the full code of the balance routine. I'm hoping someone here can help because I don't understand the math needed to derive this formula "from scratch", and I don't really understand the PID stuff. Can anyone point me anywhere or offer me any advice?
First off, I'm trying to create my own bot design (rather than just exactly copy someone else's) based on the NXTWay-Gyro bot described here:
http://www.techbricks.nl/My-NXT-project ... robot.html
I can post a photo of the design if desired, but basically it's pretty much the same as most of the other designs except I have the axles about 4" below the brick, I'm using the small NXT 2.0 wheels, and the gyro sensor is placed about 1" above the brick with the ultrasonic sensor just above it. (I figured that a higher center of gravity might make it more stable though I'm not sure if I'm right about that.)
I'm basically having two problems. The first is that the "body angle" calculation seems to be resulting in wild numbers that make no sense. Here's the code for that:
Code: Select all
corrected_gyro_reading = SensorRaw(GYRO_PORT) - gyro_bias;
body_angle += corrected_gyro_reading;
I don't entirely understand how body_angle is supposed to work, but when I display it on the screen it seems to jump up to 10,000+ even if the vertical angle is only moving back and forth by a few degrees. And it seems to jump around in a way that isn't proportional to the angle the bot is held. corrected_gyro_reading appears to be reasonable though, as does the bias compensation.
The second problem I'm having is that I don't understand these constants (10, 4, 200, etc) which I'm assuming I need to change given that the center of gravity of the bot and location of the gyro sensor are different.
Code: Select all
if (abs(wheel_speed) > 15) {
gyro_bias -= sign(wheel_speed);
wheel_speed = 0;
}
// NXT small wheels
motor_speed = ( body_angle + corrected_gyro_reading * 10
+ wheel_position * 4 + wheel_speed * 200 ) >> 4 ;
motor_speed += sign(motor_speed) * FRICTION_COMPENSATION;
Code: Select all
task balance()
{
int corrected_gyro_reading, gyro_drift_compensation;
int body_angle = 0;
int wheel_position, last_wheel_position, wheel_movement, wheel_speed;
int motor_speed;
long start_tick, run_time;
string message;
#define LOOPS_PER_SECOND 100
body_angle = 0;
while(true) {
start_tick = CurrentTick();
// can be negative (if it tilts backwards);
corrected_gyro_reading = SensorRaw(GYRO_PORT) - gyro_bias;
body_angle += corrected_gyro_reading;
sprintf(message, "Reading: %06d", corrected_gyro_reading);
TextOut(0, LCD_LINE4, message);
sprintf(message, "Angle: %06d", body_angle);
TextOut(0, LCD_LINE5, message);
wheel_position = MotorTachoCount(OUT_A);
wheel_movement = wheel_position - last_wheel_position;
last_wheel_position = wheel_position;
wheel_speed += wheel_movement;
if (abs(wheel_speed) > 15) {
gyro_bias -= sign(wheel_speed);
wheel_speed = 0;
}
// NXT small wheels
motor_speed = ( body_angle + corrected_gyro_reading * 10
+ wheel_position * 4 + wheel_speed * 200 ) >> 4 ;
motor_speed += sign(motor_speed) * FRICTION_COMPENSATION;
sprintf(message, "Speed: %-06d", motor_speed);
TextOut(0, LCD_LINE6, message);
SetOutput(OUT_AB, //PID motor control
PowerField, motor_speed, //calculated correction
OutputModeField, OUT_MODE_MOTORON,
RunStateField, OUT_RUNSTATE_RUNNING,
UpdateFlagsField, UF_UPDATE_MODE+UF_UPDATE_SPEED);
run_time = CurrentTick() - start_tick;
Acquire(run_count_mutex);
run_count++;
Release(run_count_mutex);
Wait((SEC_1 / LOOPS_PER_SECOND) - run_time);
}
}