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);
}
}