Try to run a 2 motor robot in a perfect circle by itself.......It wont :-(
There are many variables that prevent perfection:
wheel friction, wheel motor speed mismatches, gear backlash, wheel slip,
wheel diameter mismatches, varying battery voltage, motor differences,
AND resolution of the PWM controlling the motor speed.
However ....applying a simple error correction system while the robot circles...the results are pretty amazing.
First, let's understand how the robot drives in a circle.
In this illustration , the left hand side wheel has to go faster and further than the right hand side wheel to get back to the starting line.
The ratio of left side speed to right side speed sets the circle diameter (turning radius).
Theoretically, tracing out a circle should be easy, its just a ratio of wheel position (and speeds)
The ratio is difficult to maintain precisely, and there are so many variables.....
BUT by using motor encoder to regulate motor speed or use the distance traveled of the to adjust the motor speed to keep that circle perfect.
My control solution is determining the traveled position of each wheel ( which also has to be be at the same ratio of inner circle to outer circle diameter). if the traveled position of the right side wheel is not as calculated for a perfect circle to what the left wheel has traveled, it's speed reference can be sped up or slowed down, applying a correction to compensate for errors.
The Arduino program speeds up or slows down the right hand side wheel speed by 2% when the actual encoder counts are + or - 10 counts
off from calculated value.
This robot has a MKR1000 for brains, a motor carrier shield for variable speed motor control, and
inputs for the motor encoders.
( For more info on encoders, here is place https://dronebotworkshop.com/rotary-encoders-arduino/ )
Here is a video of the robot's vigorous circular motion https://youtu.be/Z20punl1SeQ
" with a vigorous Circular motion hitherto unknown to the people of this area, but destined To take the place of the mudshark in your mythology
Here it goes, the circular motion, now rub it!" sorry, I couldnt resist exerpt from *** Nanook Rubs It by Frank Zappa ****
The Robot Wheel Diameter inches = 3.5625
The Robot Wheel circumference inches 11.19181
One turn of the wheel measured 1190 encoder counts (measured value as info on PPR, actual gear ratio or motor top speed is not available)
The robot width (to the centerline of each wheel ) is 3.25 inches.
For simple testing, a 12 inch circle diameter was chosen for the robot's path.
The left side wheel path diameter is 13.625 inches, the right side wheel path diameter is 10.375 inches.
The ratio of inner path to outer path is ~ .76146788990825688073394495412844
Running a baseline value of speed ratios with the hardware, and the calculated values didnt quite work as calculated.
The motors are so different at the slow speed points being operated at, and the small circle diameter the speed ratio had to be empirically set for .55 ( 11 / 20)
Using the MKRMotorCarrier Arduino library, the motor control is set as an integer PWM Duty Cycle, -100 to 100.
1% is the best resolution speed reference that can be written for PWM.
#include <MKRMotorCarrier.h>
#define INTERRUPT_PIN 6
//Variable to store the battery voltage
static int batteryVoltage;
//Variable to change the motor speed and direction
static int duty = 50;
int TestRevs=5;
int M3Duty=11;
int M4Duty=20;
int SpeedRef=50;
int M3_Enc1_Counts=0;
int M4_Enc2_Counts=0;
float M3_Enc1_Counts_Float=0;
float M4_Enc1_Counts_Float=0;
float CircleDiameterInches = 12.0; ///centerline of tracking diameter
float InnerTrackDiameter = 1.00000; ///from centerline of tracking diameter
float OuterTrackDiameter = 1.00000; ///from centerline of tracking diameter
float TrackingSpeedRatio = 1.00000; ///
float M4_Enc2_CountsFloat=0.00000; // counts converterd to float
float M3_Enc1_CountsFloat=0.00000; // counts converterd to float
float deadbandFloat = 10.0;
int SpeedTrim = 0; // speed trim to the Mx Duty Cycle
void setup()
{
// Serial port initialization
Serial.begin(115200);
// while (!Serial);
//Establishing the communication with the motor shield
controller.begin();
// if (controller.begin())
// {
// Serial.print("MKR Motor Shield connected, firmware version ");
// Serial.println(controller.getFWVersion());
// }
// else
// {
// Serial.println("Couldn't connect! Is the red led blinking? You may need to update the firmware with FWUpdater sketch");
// while (1);
// }
// Reboot the motor controller; brings every value back to default
// Serial.println("reboot");
controller.reboot();
delay(500);
// Reset the encoder internal counter to zero (can be set to any initial value)
// Serial.println("reset counters");
encoder1.resetCounter(0);
encoder2.resetCounter(0);
//Take the battery status
// float batteryVoltage = (float)battery.getConverted();
// Serial.print("Battery voltage: ");
// Serial.print(batteryVoltage);
// Serial.print("V, Raw ");
// Serial.println(battery.getRaw());
delay (3000);
/// calculate global tracking constants
float InnerTrackDiameter = (CircleDiameterInches-(1.625*2)); ///from centerline of tracking diameter
float OuterTrackDiameter = (CircleDiameterInches+(1.625*2)); ///from centerline of tracking diameter
float TrackingSpeedRatio = OuterTrackDiameter/InnerTrackDiameter; /// ratio of wheel travel diameters
}
void loop() {
//Take the battery status
// float batteryVoltage = (float)battery.getConverted();
// Serial.println("battery = ");
// Serial.println(batteryVoltage);
M3_Enc1_Counts=encoder1.getRawCount();
M4_Enc2_Counts=encoder2.getRawCount();
M3_Enc1_CountsFloat = M3_Enc1_Counts; // save to a floating point register
M4_Enc2_CountsFloat = M4_Enc2_Counts; // save to a floating point register
// compare Enc2 outer wheel counts to inner wheel counts* speed ratio
// if inner wheel enc1 has too many counts , then slow down M3Duty
// if outer wheel enc2 has too many counts, then speed up M3Duty
// if counts are within a deadband, then M3Duty stays as default
float InnerTrackDiameter = (CircleDiameterInches-(1.625*2)); ///from centerline of tracking diameter
float OuterTrackDiameter = (CircleDiameterInches+(1.625*2)); ///from centerline of tracking diameter
float TrackingSpeedRatio = OuterTrackDiameter/InnerTrackDiameter; /// ratio of wheel travel diameters
// Serial.println("M3_Enc1_CountsFloat=");
// Serial.println(M3_Enc1_CountsFloat);
// Serial.println(" ");
// Serial.println("M4_Enc2_CountsFloat=");
// Serial.println(M4_Enc2_CountsFloat);
// Serial.println("");
// Serial.print("TrackingSpeedRatio=");
// Serial.println(TrackingSpeedRatio);
// M4 is set faster than M3 . if M4 is 10 encoder counts more than the inner encoder*circle ratio then speed up the inner wheel M3
if ((M4_Enc2_CountsFloat) > ((TrackingSpeedRatio*M3_Enc1_CountsFloat)+ deadbandFloat))
{ SpeedTrim=1; } //slowdown the inner wheel duty cycle
// M4 is set faster than M1 . if M4 is 10 encoder counts less than the inner encoder*circle ratio then slow down the inner wheel M3
else if ((M4_Enc2_CountsFloat) < ((TrackingSpeedRatio*M3_Enc1_CountsFloat)- deadbandFloat))
{ SpeedTrim= -1; } //slowdown the inner wheel duty cycle
else { SpeedTrim= 0; } //if encoder counts are within +- 10 counts, then dont trim M3 speed ref
if (M4_Enc2_Counts < (5211))
{
M3.setDuty(M3Duty+SpeedTrim);
M4.setDuty(M4Duty);
// Serial.print("Encoder2 Pos [counts]: ");
// Serial.println(M4_Enc2_Counts);
///Serial.print("Encoder1 Pos [counts]: ");
// Serial.println(M3_Enc1_Counts);
// delay (10);
}
else if ((M4_Enc2_Counts >= (5211)) && (M4_Enc2_Counts <(6500)))
{
M3.setDuty(0);
M4.setDuty(0);
}
else
{ M3.setDuty(0);
M4.setDuty(0);
//Chose the encoder to use:encoder1(default) or encoder2
// Serial.print("Encoder1 Pos [counts]: ");
// Serial.println(encoder1.getRawCount());
// Serial.print("Encoder1 vel [counts/sec]: ");
// Serial.println(encoder1.getCountPerSecond());
//Serial.print("Encoder2 vel [counts/sec]: ");
// Serial.println(encoder2.getCountPerSecond());
// Serial.print("Encoder2 Pos [counts]: ");
// Serial.println(encoder2.getRawCount());
delay(5000);
}
// }
//Keep active the communication MKR1000 & MKRMotorCarrier
//Ping the samd11
controller.ping();
//wait
delay(50);
}
Top Comments