Hi everyone, in the last years i used the pid control, one for the lineal motions and other for the turns using the reading of the inertial sensor, in this competition i want to improve with odometry tracking system, i think that finally i figured out but i still have a big doubt.
I will use only two tracking wheels to know the X and Y coordinates, and with the inertial sensor i can know the orientation of the robot.
With that on mind i could able to do a pid loop to control the voltage of the chassis motors,( my chassis is a commun lineal chassis), so in the pid loop i can calculated an error using pitagoras theorem to know the distance between my current coordinates and my desired coordinates, an other error between my actual position(using inertial sensor) and my desired orientation.
My doubt is when i calculated the finalpower of the distance error, and the finalpower of the turs, how i need to put the voltage inside my motors?
i think that maybe this is the way, but i am not sure about that.
Leftmotors move_voltage(finalpower+finalturn)
Rightmotors move_voltage(fianlpower-finalturn)
I hope that you can help me with that, i really want to implement the odometry for my programs, i read the documentation of odometry, but i stiil having that doubt
that’s exactly right, you sum the output of the controllers running in parallel and send it to each side of the chassis, respectively. your sign convention dictates which side should add turn power or subtract turn power
think about it, as you approach the correct angle, turning power approaches 0 and linear power is the only thing taking over. then as you approach the target point, linear power approaches 0
If you do two control loops, you’d need to run one of them in a separate thread. I’d say just have them both in the same loop. This is also starting to get into a more abstract version of pid where you create pid objects to calculate the output. Otherwise you will have a lot of redundant code in your main loop. I’d say, best practice is single loop, object oriented, but whatever makes sense for you is what you should use.
Here you can take a look at how my team does a pid class. Below is how we implemented it in our drive to point function with odometry.
/**
* Use odometry to automatically drive the robot to a point on the field.
* X and Y is the final point we want the robot.
*
* Returns whether or not the robot has reached it's destination.
*/
bool TankDrive::drive_to_point(double x, double y, double speed, double correction_speed, vex::directionType dir)
{
// We can't run the auto drive function without odometry
if(odometry == NULL)
{
fprintf(stderr, "Odometry is NULL. Unable to run drive_forward()\n");
fflush(stderr);
return true;
}
if(!func_initialized)
{
// Reset the control loops
drive_pid.reset();
correction_pid.reset();
drive_pid.set_limits(-fabs(speed), fabs(speed));
correction_pid.set_limits(-fabs(correction_speed), fabs(correction_speed));
// Set the targets to 0, because we update with the change in distance and angle between the current point
// and the new point.
drive_pid.set_target(0);
correction_pid.set_target(0);
// point_orientation_deg = atan2(y - odometry->get_position().y, x - odometry->get_position().x) * 180.0 / PI;
func_initialized = true;
}
// Store the initial position of the robot
position_t current_pos = odometry->get_position();
position_t end_pos = {.x=x, .y=y};
// Create a point (and vector) to get the direction
Vector::point_t pos_diff_pt =
{
.x = x - current_pos.x,
.y = y - current_pos.y
};
Vector point_vec(pos_diff_pt);
// Get the distance between 2 points
double dist_left = OdometryBase::pos_diff(current_pos, end_pos);
int sign = 1;
if (fabs(dist_left) < config.drive_correction_cutoff)
{
// Make an imaginary perpendicualar line to that between the bot and the point. If the point is behind that line,
// and the point is within the robot's radius, use negatives for feedback control.
double angle_to_point = atan2(y - current_pos.y, x - current_pos.x) * 180.0 / PI;
double angle = fmod(current_pos.rot - angle_to_point, 360.0);
// Normalize the angle between 0 and 360
if (angle > 360) angle -= 360;
if (angle < 0) angle += 360;
// If the angle is behind the robot, report negative.
if (dir == directionType::fwd && angle > 90 && angle < 270)
sign = -1;
else if(dir == directionType::rev && (angle < 90 || angle > 270))
sign = -1;
// When inside the robot's cutoff radius, report the distance to the point along the robot's forward axis,
// so we always "reach" the point without having to do a lateral translation
dist_left *= fabs(cos(angle * PI / 180.0));
}
// Get the heading difference between where we are and where we want to be
// Optimize that heading so we don't turn clockwise all the time
double heading = rad2deg(point_vec.get_dir());
double delta_heading = 0;
// Going backwards "flips" the robot's current heading
if (dir == directionType::fwd)
delta_heading = OdometryBase::smallest_angle(current_pos.rot, heading);
else
delta_heading = OdometryBase::smallest_angle(current_pos.rot - 180, heading);
// Update the PID controllers with new information
correction_pid.update(delta_heading);
drive_pid.update(sign * -1 * dist_left);
// Disable correction when we're close enough to the point
double correction = 0;
if(is_pure_pursuit || fabs(dist_left) > config.drive_correction_cutoff)
correction = correction_pid.get();
// Reverse the drive_pid output if we're going backwards
double drive_pid_rval;
if(dir == directionType::rev)
drive_pid_rval = drive_pid.get() * -1;
else
drive_pid_rval = drive_pid.get();
// Combine the two pid outputs
double lside = drive_pid_rval + correction;
double rside = drive_pid_rval - correction;
// limit the outputs between -1 and +1
lside = (lside > 1) ? 1 : (lside < -1) ? -1 : lside;
rside = (rside > 1) ? 1 : (rside < -1) ? -1 : rside;
drive_tank(lside, rside);
printf("dist: %f\n", dist_left);
fflush(stdout);
// Check if the robot has reached it's destination
if(drive_pid.is_on_target())
{
stop();
func_initialized = false;
return true;
}
return false;
}
In any case, I think it’s best to learn by example. Teams are always so secretive about how they get things done, and that is the exact opposite of what stem is about.
note: our pid has a feedforward term named k, you can ignore that for now
