This pseudo-code should get you started...
Code:
commanded_speed = Get_Our_Desired_Speed()
robot_speed = Get_Actual_Robot_Velocity()
wheel_speed = Get_Wheel_Velocity()
speed_error = robot_speed - wheel_speed
// Detect if we are slipping
If absolute value of speed_error is greater than tolerance then we are slipping
{
// If we are, get back our traction ASAP!
Set commanded_speed to robot_speed
}
Else
{
// If we aren't slipping, let's keep it that way!
Limit commanded_speed based on maximum acceleration to prevent slipping
}
Send commanded_speed to the motors
Close the loop between commanded_speed and wheel_speed with a PID controller (optional)
Hopefully this will be useful!
Also, keep in mind that if you turn differentially (tank-style), your wheels ARE going to slip a little bit. Take that into account or your idler wheel is going to freak out your traction control system every time you turn.