Drawing Robot
For an undergraduate robotics course (MAE 106), I was tasked with creating a robot that can draw using 2 servomotors and an Arduino.
We competed to draw "ZOT" as fast and accurately as possible. Our team ended up placing first out of 6 teams.
Our physical design was uniquely based on polar coordinates, such that one servo controls the angular coordinate and the other (high-speed) servo controls the radial coordinate via rack and pinion.
I traced the path from the template in a vector graphics software (Inkscape) and converted the vector image into a .csv file of polar coordinates using Python.
coord_processing.py
import re
import math
import statistics
input_path = 'C:/Users/Tae Rugh/Desktop/Polar_Drawbot/zot.txt'
output_path = 'C:/Users/Tae Rugh/Desktop/Polar_Drawbot/zot.csv'
coord_pattern = r'<(\d+\.\d+), (\d+\.\d+)>'
x_offset = -138 #mm
y_offset = 0 #mm
# Creates a list of 2-tuples representing Cartesian coordinate points
def read_input():
coords = []
with open(input_path,'r') as input_file:
for line in input_file:
for coord in re.findall(coord_pattern,line):
coord = [float(coord[0])+x_offset, float(coord[1])+y_offset]
if coords == [] or coord != coords[-1]:
coords.append(coord)
if coords[0] == coords[-1]:
coords.pop(-1)
return coords
# Generates a list of distances between neighboring coordinates
def calc_distances(cart_coords):
distances = []
for i in range(1,len(cart_coords)):
d = ((cart_coords[i][0] - cart_coords[i-1][0])**2 + (cart_coords[i][1] - cart_coords[i-1][1])**2)**.5
distances.append(d)
return distances
# converts Cartesian coordinates to polar
def cart_to_polar():
polar_coords = []
for c in cart_coords:
r = (c[0]**2 + c[1]**2)**.5
theta = math.degrees(math.atan(c[0]/c[1])) + 90
polar_coords.append((r,theta))
return polar_coords
# Writes the coordinate points in .csv format to the output file
def write_coords(coords):
with open(output_path,'w') as output_file:
output_file.write('r,theta,dist\n')
for coord in coords:
output_file.write(f'{coord[0]},{coord[1]}\n')
if __name__ == '__main__':
print(f'Reading data from {input_path} ...')
cart_coords = read_input()
print(f' {len(cart_coords)} Cartesian coordinate points extracted\n')
print('Calculating distances ...')
distances = calc_distances(cart_coords)
print(f' distance: Min = {round(min(distances),1)}mm, Max = {round(max(distances),1)}mm, Mean = {round(statistics.mean(distances),1)}mm\n')
print('Converting to polar coordinates ...')
polar_coords = cart_to_polar()
print(f' r: Min (>48mm) = {round(min(c[0] for c in polar_coords),1)}mm, Max (<138mm) = {round(max(c[0] for c in polar_coords),1)}mm')
print(f' theta: Min (>0deg) = {round(min(c[1] for c in polar_coords),1)}deg, Max (<180deg) = {round(max(c[1] for c in polar_coords),1)}deg\n')
print(f'Writing data to {output_path} ...')
write_coords(polar_coords)
print(' Success!\n')
if input('Print radii? (Y/N): ').upper() == 'Y':
print('radii = ')
for coord in polar_coords:
print(round(coord[0],2),end=',')
print('\n')
if input('Print angles? (Y/N): ').upper() == 'Y':
print('angles = ')
for coord in polar_coords:
print(round(coord[1],2),end=',')
print('\n')
For the Arduino code, I created a class "Servo360" which implements the feedback control and includes other useful functions for the high-speed servo motor. This was necessary since there aren't currently any great open-source Arduino libraries for high-speed servo motors. I am considering developing this further and releasing a public library for this. For this project though, all that's left to do is to iterate through the list of polar coordinates and command the servos to go to the appropriate angles. I linearly interpolated 20 times between each coordinate to increase precision.
servo360.cpp
// Control Constants
static const float Kp = .25; // feedback constant
static const float offset = 92.5; // servo input corrensponding to 0 velocity
static const float tpr = 44; // travel per revolution (mm/rev)
static const float pi = 3.141592;
// Feedback Constants
static const int unitsFC = 360; // Units in a full circle
static const int q2min = unitsFC/4; // For checking if in 1st quadrant
static const int q3max = q2min * 3; // For checking if in 4th quadrant
static const float dcMin = .029; // Minimum duty cycle
static const float dcMax = .971; // Maximum duty cycle
class Servo360: public Servo {
private:
// Control Variables
float r = 138; // mm
float targetAngle, errorAngle, output;
// Feedback Variables
int turns = 0;
float angle0, angle, dc, theta, thetaP, tHigh, tLow, tCycle;
public:
int pinControl;
int pinFeedback;
void initialize() {
Servo::attach(pinControl);
angle0 = get_angle();
}
void set_angle(float targetAngle) {
Servo360::get_angle();
errorAngle = angle - targetAngle;
output = errorAngle * Kp + offset;
if(output > 180) output = 180;
if(output < 0) output = 0;
Servo::write(output);
}
void set_r(float targetR) {
targetAngle = angle0 + ((targetR - 138) * (360 / tpr));
Servo360::set_angle(targetAngle);
}
float get_angle(void) {
while(1) {
tHigh = pulseIn(pinFeedback, 1); // Measure high pulse
tLow = pulseIn(pinFeedback, 0); // Measure low pulse
tCycle = tHigh + tLow; // Calculate cycle time
if((tCycle > 1000) && (tCycle < 1200)) break; // Validate cycle time
}
dc = tHigh / tCycle; // Calculate duty cycle
theta = ((dc - dcMin)*unitsFC) / (dcMax - dcMin + .01); // Calculate theta
if(theta < 0) theta = 0; // Validate theta
else if(theta > (unitsFC - 1)) theta = unitsFC - 1;
if((theta < q2min) && (thetaP > q3max)) turns++; // Keep track of turns
else if((thetaP < q2min) && (theta > q3max)) turns --;
if (turns >= 0) angle = (turns * unitsFC) + theta; // Calculate angle
else if(turns < 0) angle = ((turns + 1) * unitsFC) - (unitsFC - theta);
thetaP = theta; // Update thetaP
return angle;
}
};
drawbot.ino
#include <servo360.cpp>
float radii[] = {87.95,83.96,80.49,76.64,72.96,69.64,67.49,67.3,69.54,73.41,77.41,81.18,84.53,87.21,89.49,91.99,95.17,98.98,103.07,107.0,110.9,114.51,117.84,120.85,123.56,125.83,127.67,128.08,126.31,122.71,118.9,115.59,112.57,109.93,107.47,104.96,102.72,100.35,98.09,95.54,92.8,90.13,87.49,84.77,81.93,79.34,78.39,79.19,81.88,85.0,88.53,92.4,96.36,100.01,102.2,101.91,100.65,98.75,96.66,94.24,91.6,88.96,85.99,82.45,78.8,74.91,73.34,74.46,76.98,80.41,84.33,88.38,92.36,96.06,98.7,100.01,99.89,99.19,98.04,96.1,93.6,91.21,88.87,86.93,85.5,84.27,83.2,82.59,82.26,81.35,80.09,79.64,79.77,80.64,81.68,83.23,84.92,87.28,90.67,93.25,95.8,98.58,101.43,98.53,95.79,93.24,90.62,87.91,85.42,84.11,86.46,87.75,89.93,92.41,95.06,98.11,101.22,104.66,108.36,112.41,115.58,117.3,117.96,117.93,117.33,116.3,115.16,113.64,113.28};
float angles[] = {64.84,65.4,65.93,66.75,67.99,69.86,72.62,76.1,79.02,79.71,79.17,77.84,76.15,74.14,72.43,74.4,75.86,76.67,76.91,76.79,76.16,75.3,74.2,72.91,71.6,70.25,68.6,66.83,65.55,65.25,65.8,66.95,68.32,69.89,71.49,73.28,75.02,76.85,78.77,80.63,82.48,84.44,86.34,88.28,90.14,92.35,92.64,90.39,88.24,86.58,85.22,84.55,84.74,85.72,87.54,89.82,92.01,94.06,96.06,98.01,99.87,101.76,103.58,105.06,106.19,105.45,102.66,99.71,97.32,96.12,95.4,95.36,95.82,96.85,98.73,101.11,103.44,105.74,108.03,110.1,112.01,114.11,116.33,118.86,121.37,124.2,127.07,130.0,132.87,135.49,134.98,132.11,129.16,126.1,123.35,120.72,118.18,115.87,117.4,119.38,121.26,123.06,124.81,123.13,121.36,119.48,117.56,115.65,113.5,112.37,114.55,115.64,113.4,111.39,109.62,108.01,106.57,105.37,104.39,104.62,105.87,107.75,109.69,111.63,113.57,115.52,117.41,119.29,119.71};
Servo servo1; // normal servo1 controls the angle (theta in polar coordinates)
Servo360 servo2; // high-speed servo2 controls the extension (r in polar coordinates)
float delta_r, delta_a;
void setup() {
servo1.attach(2); // servo1 control wire is on digital pin 2
servo2.pinControl = 3; // servo2 control wire is on digital pin 3
servo2.pinFeedback = 5; // servo2 feedback wire is on digital pin 5
servo2.initialize();
for (int i=0; i<500; i++) {
go_to(radii[0],angles[0]);
}
delay(10000); // wait 10 seconds (gives time to place the sharpie down)
for (int i = 0; i < sizeof(radii)/sizeof(radii[0])-1 ; i++) { // iterate through all of the coordinate points
delta_r = radii[i+1]-radii[i];
delta_a = angles[i+1]-angles[i];
for (int j = 0; j < 20; j++) {
go_to(radii[i] + (j/20)*delta_r,angles[i] + (j/20)*delta_a);
}
}
}
void loop() {
delay(1000);
}
void go_to(float radius, float angle) {
servo1.write(angle);
servo2.set_r(radius);
delay(5);
}