def __init__(self, l_rope_0, r_rope_0, attachment_distance, pulley_diam=4.4, Kp=2.2, Ki=0.2, Kd=0.02, Kp_neg_factor=.5, max_spd=800):
self.__l_rope_0 = float(l_rope_0)
self.__r_rope_0 = float(r_rope_0)
self.__att_dist = float(attachment_distance)
self.pulley_diam = float(pulley_diam)
self.direction = 1 # -1 is for reversing motors
self.calc_constants()
# Start the engines
self.pen_motor = PIDMotor(ev3dev.OUTPUT_D, Kp=4, Ki=0.2, Kd=0, brake=0.3, max_spd=200) # Port D (motors go from 0-3)
self.pen_motor.positionPID.precision = 4
self.pen_motor.speedPID.Kp = 0.05
#self.pen_motor = ev3dev.Motor(ev3dev.OUTPUT_D)
#self.pen_motor.stop_command = self.pen_motor.stop_command_brake
#self.pen_motor.speed_regulation_enabled = 'on'
self.left_motor = PIDMotor(ev3dev.OUTPUT_B, Kp=Kp, Ki=Ki, Kd=Kd, max_spd=max_spd)
self.right_motor = PIDMotor(ev3dev.OUTPUT_C, Kp=Kp, Ki=Ki, Kd=Kd, max_spd=max_spd)
# Build lists for iterating over all motors
self.drive_motors = [self.left_motor, self.right_motor]
self.all_motors = [self.left_motor, self.right_motor, self.pen_motor]
# Set starting point
self.set_control_zeroes()
if ev3dev.current_platform() == 'brickpi':
self.battery = BrickPiPowerSupply()
else:
self.battery = ev3dev.PowerSupply()
class RopePlotter(object):
def __init__(self, l_rope_0, r_rope_0, attachment_distance, pulley_diam=4.4, Kp=2.2, Ki=0.2, Kd=0.02, Kp_neg_factor=.5, max_spd=800):
self.__l_rope_0 = float(l_rope_0)
self.__r_rope_0 = float(r_rope_0)
self.__att_dist = float(attachment_distance)
self.pulley_diam = float(pulley_diam)
self.direction = 1 # -1 is for reversing motors
self.calc_constants()
# Start the engines
self.pen_motor = PIDMotor(ev3dev.OUTPUT_D, Kp=4, Ki=0.2, Kd=0, brake=0.3, max_spd=200) # Port D (motors go from 0-3)
self.pen_motor.positionPID.precision = 4
self.pen_motor.speedPID.Kp = 0.05
#self.pen_motor = ev3dev.Motor(ev3dev.OUTPUT_D)
#self.pen_motor.stop_command = self.pen_motor.stop_command_brake
#self.pen_motor.speed_regulation_enabled = 'on'
self.left_motor = PIDMotor(ev3dev.OUTPUT_B, Kp=Kp, Ki=Ki, Kd=Kd, max_spd=max_spd)
self.right_motor = PIDMotor(ev3dev.OUTPUT_C, Kp=Kp, Ki=Ki, Kd=Kd, max_spd=max_spd)
# Build lists for iterating over all motors
self.drive_motors = [self.left_motor, self.right_motor]
self.all_motors = [self.left_motor, self.right_motor, self.pen_motor]
# Set starting point
self.set_control_zeroes()
if ev3dev.current_platform() == 'brickpi':
self.battery = BrickPiPowerSupply()
else:
self.battery = ev3dev.PowerSupply()
# Getters & setters for plotter properties. Python style, Baby!
# After setting these, some calculations need to be done, that's why I define special functions
# And decorate them as setters and getters.
@property
def Kp(self):
return self.drive_motors[0].positionPID.Kp
@Kp.setter
def Kp(self,Kp):
for motor in self.drive_motors:
motor.positionPID.Kp = float(Kp)
@property
def Ti(self):
return self.drive_motors[0].positionPID.Ti
@Ti.setter
def Ti(self, Ti):
for motor in self.drive_motors:
motor.positionPID.Ti = float(Ti)
@property
def Td(self):
return self.drive_motors[0].positionPID.Td
@Td.setter
def Td(self, Td):
for motor in self.drive_motors:
motor.positionPID.Td = float(Td)
@property
def max_speed(self):
return self.drive_motors[0].positionPID.max_out
@max_speed.setter
def max_speed(self, n):
for motor in self.drive_motors:
motor.positionPID.max_out = int(n)
@property
def l_rope_0(self):
return self.__l_rope_0
@l_rope_0.setter
def l_rope_0(self, length):
self.__l_rope_0 = float(length)
self.calc_constants()
@property
def r_rope_0(self):
return self.__r_rope_0
@r_rope_0.setter
def r_rope_0(self, length):
self.__r_rope_0 = float(length)
self.calc_constants()
@property
def att_dist(self):
return self.__att_dist
@att_dist.setter
def att_dist(self,length):
self.__att_dist = float(length)
self.calc_constants()
def calc_constants(self):
# Some math to calculate the plotter parameters
large_gear_teeth = 24
small_gear_teeth = 8
if ev3dev.current_platform() == 'brickpi':
factor = 2
else:
factor = 1
self.cm_to_deg = -factor * 180 / 3.1415 * 2 / self.pulley_diam * large_gear_teeth / small_gear_teeth
# Calculate the height of triangle made up by the two ropes
self.v_margin = self.triangle_area(self.__l_rope_0, self.__r_rope_0, self.__att_dist) / self.__att_dist * 2
# Using pythagoras to find distance from bottom triangle point to left doorframe
self.h_margin = (self.__l_rope_0 ** 2 - self.v_margin ** 2) ** 0.5
# For convenience, the canvas is square and centered between the attachment points
self.canvas_size = self.__att_dist - 2 * self.h_margin
@staticmethod
def triangle_area(a, b, c):
"""
Calculate the area of a triangle by the lengths of it's sides using Heron's formula
:param a: Length of side a
:param b: Length of side b
:param c: Length of side c
:return: area (float)
"""
half_p = (a + b + c) / 2
return (half_p * (half_p - a) * (half_p - b) * (half_p - c)) ** 0.5
### Calculations for global (doorframe) to local (canvas) coordinates and back. ###
def motor_targets_from_norm_coords(self,x_norm, y_norm):
x,y = self.normalized_to_global_coords(x_norm,y_norm)
return self.motor_targets_from_coords(x,y)
def motor_targets_from_coords(self,x, y):
l_rope = (x ** 2 + y ** 2) ** 0.5
r_rope = ((self.__att_dist - x) ** 2 + y ** 2) ** 0.5
l_target = (l_rope - self.__l_rope_0) * self.cm_to_deg
r_target = (r_rope - self.__r_rope_0) * self.cm_to_deg
return int(l_target), int(r_target)
def coords_from_motor_pos(self,l_motor,r_motor):
l_rope = l_motor / self.cm_to_deg + self.__l_rope_0
r_rope = r_motor / self.cm_to_deg + self.__r_rope_0
y = self.triangle_area(l_rope,r_rope,self.att_dist)*2/self.att_dist
x = (l_rope**2 - y**2)**0.5
y_norm = (y - self.v_margin)/self.canvas_size
x_norm = (x - self.h_margin)/self.canvas_size
return x_norm,y_norm
def normalized_to_global_coords(self,x_norm,y_norm):
# convert normalized coordinates to global coordinates
x = x_norm * self.canvas_size + self.h_margin
y = y_norm * self.canvas_size + self.v_margin
return x,y
### Movement functions ###
def set_control_zeroes(self):
for motor in self.all_motors:
motor.position = 0
#motor.positionPID.zero = motor.position
def move_to_coord(self,x,y):
motor_b_target, motor_c_target = self.motor_targets_from_coords(x, y)
self.move_to_targets((motor_b_target, motor_c_target))
def move_to_norm_coord(self, x_norm, y_norm):
motor_b_target, motor_c_target = self.motor_targets_from_norm_coords(x_norm, y_norm)
#print "Moving to ", x_norm, ",", y_norm, "(At", motor_b_target, motor_c_target, ")"
self.move_to_targets((motor_b_target, motor_c_target))
def move_to_targets(self, targets):
# Set targets
for motor, tgt in zip(self.drive_motors, targets):
motor.position_sp = tgt
# Now wait for the motors to reach their targets
# Alas ev3dev's run_to_abs_pos is not usable. Have to use my own PID controller.
while 1:
for motor in self.drive_motors:
motor.run()
if all([motor.positionPID.target_reached for motor in self.drive_motors]):
self.left_motor.stop()
self.right_motor.stop()
break
#We're done calculating and setting all motor speeds!
time.sleep(0.02)
### Advanced plotting functions by chaining movement functions ###
def test_drive(self):
# A little disturbance in the force
self.move_to_norm_coord(0.1,0.1)
self.move_to_norm_coord(0.5,0.5)
self.move_to_norm_coord(0.0,0.0)
def plot_from_file(self, filename):
"""
Generator function for plotting from coords.csv file. After each next() it returns the pct done of the plotting
This way the plotting can easily be aborted and status can be given. Gotta love python for this.
Usage:
gen = plotter.plot_from_file(myfile)
while 1:
try:
pct_done = next(gen)
except StopIteration:
break
:param filename: str
:return: percentage done: float
"""
coords = open(filename)
num_coords = int(coords.readline()) #coords contains it's length on the first line.
#drive to the first coordinate
self.pen_up()
# read from file
x_norm, y_norm = [float(n) for n in coords.readline().split(",")]
#move
self.move_to_norm_coord(x_norm, y_norm)
self.pen_down()
for i in range(num_coords - 1):
# read from file
x_norm, y_norm = [float(n) for n in coords.readline().split(",")]
#move
self.move_to_norm_coord(x_norm, y_norm)
yield float(i+1)/num_coords*100
coords.close()
self.pen_up()
self.move_to_norm_coord(0, 0)
yield 100
def plot_circles(self, num_circles=20):
im = Image.open("uploads/picture.jpg").convert("L")
w, h = im.size
pixels = im.load()
r_min = (self.h_margin**2+self.v_margin**2)**0.5
r_max = ((self.h_margin+self.canvas_size)**2 + (self.v_margin+self.canvas_size)**2)**0.5
r_step = (r_max-r_min)/num_circles
for right_side_mode in [0, 1]: # Multiply all terms that only apply to the right side circles with right_side_mode
if right_side_mode:
drive_motor, anchor_motor = self.drive_motors
else:
anchor_motor, drive_motor = self.drive_motors
# First draw circles with left anchor point as center.
for i in range(1, num_circles, 2):
# Move to the starting point at x,y
# Calculate where a circle with radius r_min+r_step*i crosses the left margin.
x = self.h_margin + (right_side_mode * self.canvas_size)
y = ((r_min+r_step*i)**2 - self.h_margin ** 2) ** 0.5 # This is the same left and right
if y > self.v_margin+self.canvas_size:
# We reached the bottom, now we check where circles cross the bottom margin
if right_side_mode:
x = self.h_margin*2 + self.canvas_size - ((r_min + r_step*i) ** 2 - (self.v_margin + self.canvas_size) ** 2) ** 0.5
else:
x = ((r_min + r_step*i) ** 2 - (self.v_margin + self.canvas_size) ** 2) ** 0.5
y = self.v_margin+self.canvas_size # This is the same left and right
self.move_to_coord(x,y)
# Now calculate coordinates continuously until we reach the top, or right side of the canvas
# Motor B is off, so let's get it's encoder only once
while 1:
# Look at the pixel we're at and move pen up or down accordingly
x_norm, y_norm = self.coords_from_motor_pos(self.drive_motors[0].position, self.drive_motors[1].position)
pixel_location = (clamp(x_norm * w, (0,w-1)), clamp(y_norm * w, (0,h-1)))
if pixels[pixel_location] < 60 + 60 * right_side_mode:
self.pen_motor.position_sp = DOWN
if not self.pen_motor.positionPID.target_reached: drive_motor.stop()
#if not DOWN-3 < self.pen_motor.position < DOWN + 3: drive_motor.stop()
self.pen_motor.run_to_abs_pos()
#turn on motors in different direction to draw horizontalish lines
drive_motor.run_at_speed_sp(SLOW)
else:
self.pen_motor.run_to_abs_pos(position_sp=UP)
#turn on motors in different direction to draw horizontalish lines
drive_motor.run_at_speed_sp(FAST)
if y_norm <= 0:
break # reached the top
if (not right_side_mode) and x_norm >= 1:
break # reached the right side
if right_side_mode and x_norm <= 0:
break
drive_motor.stop()
# Pen up
self.pen_motor.run_to_abs_pos(position_sp=UP)
# Yield to allow pause/stop and show percentage
yield (i*50.0+right_side_mode*50.0)/num_circles * 0.66
#Good, now move to the next point and roll down.
if right_side_mode:
x = self.h_margin*2 + self.canvas_size - ((r_min + r_step*(i+1)) ** 2 - self.v_margin ** 2) ** 0.5
else:
x = ((r_min + r_step*(i+1)) ** 2 - self.v_margin ** 2) ** 0.5
y = self.v_margin
if (not right_side_mode) and x > (self.h_margin + self.canvas_size): # Reached right side
x = self.h_margin + self.canvas_size
y = ((r_min+r_step*(i+1)) ** 2 - (self.h_margin+self.canvas_size) ** 2) ** 0.5
if right_side_mode and x < self.h_margin: # Reached left side
x = self.h_margin
y = ((r_min+r_step*(i+1)) ** 2 - (self.h_margin+self.canvas_size) ** 2) ** 0.5
self.move_to_coord(x, y)
# Calculate coordinates continuously until we reach the top, or right side of the canvas
while 1:
# Look at the pixel we're at and move pen up or down accordingly
x_norm, y_norm = self.coords_from_motor_pos(self.drive_motors[0].position, self.drive_motors[1].position)
pixel_location = (int(clamp(x_norm * w, (0,w-1))), int(clamp(y_norm * w, (0,h-1))))
if pixels[pixel_location] < 60 + 60 * right_side_mode: # About 33% gray
self.pen_motor.position_sp = DOWN
if not self.pen_motor.positionPID.target_reached: drive_motor.stop()
self.pen_motor.run_to_abs_pos()
drive_motor.run_at_speed_sp(-SLOW)
else:
self.pen_motor.run_to_abs_pos(position_sp=UP)
drive_motor.run_at_speed_sp(-FAST)
if y_norm >= 1:
break # reached the bottom
if x_norm <= 0 and not right_side_mode:
break # reached the left side
if x_norm >= 1 and right_side_mode:
break
time.sleep(0.02)
drive_motor.stop()
self.pen_motor.run_to_abs_pos(position_sp=UP)
# Yield to allow pause/stop and show percentage
yield ((i+1)*50.0+right_side_mode*50.0)/num_circles * 0.66
# Now draw horizontal lines.
for i in range(0,num_circles, 2):
x = self.h_margin
y = self.v_margin + i * self.canvas_size * 1.0 / num_circles
self.move_to_coord(x,y)
while 1:
# Look at the pixel we're at and move pen up or down accordingly
x_norm, y_norm = self.coords_from_motor_pos(self.drive_motors[0].position, self.drive_motors[1].position)
pixel_location = (clamp(x_norm * w, (0,w-1)), clamp(y_norm * w, (0,h-1)))
if pixels[pixel_location] < 160:
self.pen_motor.position_sp = DOWN
if not self.pen_motor.positionPID.target_reached:
self.right_motor.stop()
self.left_motor.stop()
self.pen_motor.position_sp()
#turn on motors in different direction to draw horizontalish lines
self.right_motor.run_at_speed_sp(SLOW)
self.left_motor.run_at_speed_sp(-SLOW)
else:
self.pen_motor.position_sp(position_sp=UP)
#turn on motors in different direction to draw horizontalish lines
self.right_motor.run_at_speed_sp(FAST)
self.left_motor.run_at_speed_sp(-FAST)
if x_norm >= 1:
break
self.right_motor.stop()
self.left_motor.stop()
# Pen up
self.pen_motor.run_to_abs_pos(position_sp=UP)
yield 66 + i * 33.33 / num_circles
x = self.h_margin + self.canvas_size
y = self.v_margin + (i+1) * self.canvas_size * 1.0 / num_circles
self.move_to_coord(x,y)
while 1:
# Look at the pixel we're at and move pen up or down accordingly
x_norm, y_norm = self.coords_from_motor_pos(self.drive_motors[0].position, self.drive_motors[1].position)
pixel_location = (clamp(x_norm * w, (0,w-1)), clamp(y_norm * w, (0,h-1)))
if pixels[pixel_location] < 160:
self.pen_motor.run_to_abs_pos(position_sp=DOWN)
#turn on motors in different direction to draw horizontalish lines
self.right_motor.run_at_speed_sp(-SLOW)
self.left_motor.run_at_speed_sp(SLOW)
else:
self.pen_motor.run_to_abs_pos(position_sp=UP)
self.right_motor.run_at_speed_sp(-FAST)
self.left_motor.run_at_speed_sp(FAST)
if x_norm <= 0:
break
self.right_motor.stop()
self.left_motor.stop()
# Pen up
self.pen_motor.run_to_abs_pos(position_sp=UP)
yield 66 + (i+1) * 33.33 / num_circles
self.move_to_norm_coord(0,0)
### Calibration & manual movement functions ###
def pen_up(self):
self.pen_motor.run_to_abs_pos(position_sp=UP)
def pen_down(self):
self.pen_motor.run_to_abs_pos(position_sp=DOWN)
def left_fwd(self):
self.left_motor.run_at_speed_sp(400)
def left_stop(self):
self.left_motor.stop()
def left_back(self):
self.left_motor.run_at_speed_sp(-400)
def right_fwd(self):
self.right_motor.run_at_speed_sp(400)
def right_stop(self):
self.right_motor.stop()
def right_back(self):
self.right_motor.run_at_speed_sp(-400)
def stop_all_motors(self):
for motor in self.all_motors:
motor.stop()
print "Motors stopped"
