class Neopixel():
def __init__(self):
self.board = Arduino()
self.board.connect()
def reset(self):
return self.board.sendCommand(Commands.RESET_NEO_PIXELS,0,0)
def writePixel(self,pixel,r,g,b):
return self.board.sendCommand(Commands.WRITE_NEO_PIXEL,0,pixel,r,g,b)
class Encoder():
def __init__(self):
self.board = Arduino()
self.board.connect()
def getRevsright(self):
rightrevs = self.board.sendCommand(Commands.READ_RIGHT_ENCODER,0,-1)
return rightrevs
def getRevsleft(self):
leftrevs = self.board.sendCommand(Commands.READ_LEFT_ENCODER,0,-1)
return leftrevs
class Ultrasound():
def __init__(self):
self.board = Arduino()
self.board.connect()
def getDistance(self):
distance = int(self.board.sendCommand(Commands.READ_ULTRASOUND,TRIGGER_PIN,0))
# large distances are reported as 0, so change to max distance
if distance == 0:
distance = 100
# limit to 100
elif distance > 100:
distance = 100
return distance
class ArduinoTracker(object):
def __init__(self, video_src, port, baud):
self.cam = cv2.VideoCapture(video_src)
ret, self.frame = self.cam.read()
cv2.namedWindow('Arduino Tracker')
cv2.setMouseCallback('Arduino Tracker', self.mouse)
self.selection = None
self.drag_start = None
self.tracking_state = 0
self.dest = None
self.state = 'Servo: Pivot'
self.pivot_dir = 'l' # default pivot direction left
self.move_dir = 'f' # default move direction forward
if not port:
port = '/dev/tty.usbmodem1A1341'
if not baud:
baud = 115200
self.robot = Arduino(port, baud)
self.prev_angle = 0
self.prev_dist = 0
self.counter = 0
def mouse(self, event, x, y, flags, param):
'''
Actions on mouse click.
'''
if event == cv2.EVENT_LBUTTONDOWN:
self.drag_start = (x, y)
if self.tracking_state == 1:
self.dest = (x,y)
self.state = 'Idle'
else:
self.tracking_state = 0
if self.drag_start:
if flags & cv2.EVENT_FLAG_LBUTTON:
h, w = self.frame.shape[:2]
xo, yo = self.drag_start
x0, y0 = np.maximum(0, np.minimum([xo, yo], [x, y]))
x1, y1 = np.minimum([w, h], np.maximum([xo, yo], [x, y]))
self.selection = None
if x1-x0 > 0 and y1-y0 > 0:
self.selection = (x0, y0, x1, y1)
else:
self.drag_start = None
if self.selection is not None:
self.tracking_state = 1
def dist(self, pt1, pt2):
'''
Calculate distance between two points.
'''
return ((pt1[0] - pt2[0])**2 + (pt1[1] - pt2[1])**2)**0.5
def doAction(self, diff, center):
'''
Handle actions and state changes
'''
# Test default direction
if self.state == 'Servo: Pivot':
self.prev_angle = diff
self.robot.sendCommand(self.pivot_dir)
self.state = 'Pivot: Wait'
# Wait 4 cycles
elif self.state == 'Pivot: Wait':
self.counter += 1
if self.counter == 4:
self.counter = 0
self.robot.sendCommand('s')
self.state = 'Determine Direction'
# If default pivot direction wrong, change direction
elif self.state == 'Determine Direction':
if (
(abs(diff) > abs(self.prev_angle) and abs(self.prev_angle) < 90) or
(abs(diff) < abs(self.prev_angle) and abs(self.prev_angle) > 90)
):
if self.pivot_dir == 'l':
self.pivot_dir = 'r'
else:
self.pivot_dir = 'l'
self.robot.sendCommand(self.pivot_dir)
self.state = 'Turning'
# Continue turning until angle between lines is small
elif self.state == 'Turning':
d = self.dist(center, self.dest)
if d < 50:
self.state = 'Done'
self.robot.sendCommand('s')
if abs(diff) < 8 or abs(diff) > 172:
self.state = 'Correct'
self.robot.sendCommand('s')
# Correc the angle slightly
elif self.state == 'Correct':
if self.pivot_dir == 'l':
self.robot.sendCommand('r')
else:
self.robot.sendCommand('l')
self.state = 'Correct: Wait'
# Wait 3 cycles
elif self.state == 'Correct: Wait':
self.counter += 1
if self.counter == 3:
self.counter = 0
self.robot.sendCommand('s')
self.state = 'Servo: Move'
# Test default movement direction
elif self.state == 'Servo: Move':
self.prev_dist = self.dist(center, self.dest)
self.robot.sendCommand(self.move_dir)
self.state = 'Move: Wait'
# Wait 6 cycles -- Movement direction is more critical, so wait longer
elif self.state == 'Move: Wait':
self.counter += 1
if self.counter == 6:
self.counter = 0
self.robot.sendCommand('s')
self.state = 'Determine Motion'
# If movement direction wrong, change direction
elif self.state == 'Determine Motion':
d = self.dist(center, self.dest)
if d > self.prev_dist:
if self.move_dir == 'f':
self.move_dir = 'b'
else:
self.move_dir = 'f'
self.robot.sendCommand(self.move_dir)
self.state = 'Moving'
# Move until distance is small or if angle becomes too large
elif self.state == 'Moving':
if abs(diff) > 45 and abs(diff) < 135:
self.state = 'Servo: Pivot'
self.robot.sendCommand('s')
d = self.dist(center, self.dest)
if d < 50:
self.robot.sendCommand('s')
self.state = 'Done'
def run(self):
'''
Main loop.
'''
while True:
# read image from camera
ret, self.frame = self.cam.read()
vis = self.frame.copy()
hsv = cv2.cvtColor(self.frame, cv2.COLOR_BGR2HSV)
# create mask
mask = cv2.inRange(hsv, np.array((0., 60., 32.)), np.array((180., 255., 255.)))
# track the selected object
if self.selection:
x0, y0, x1, y1 = self.selection
self.track_window = (x0, y0, x1-x0, y1-y0)
hsv_roi = hsv[y0:y1, x0:x1]
mask_roi = mask[y0:y1, x0:x1]
hist = cv2.calcHist( [hsv_roi], [0], mask_roi, [16], [0, 180] )
cv2.normalize(hist, hist, 0, 255, cv2.NORM_MINMAX);
self.hist = hist.reshape(-1)
vis_roi = vis[y0:y1, x0:x1]
cv2.bitwise_not(vis_roi, vis_roi)
vis[mask == 0] = 0
if self.tracking_state == 1:
self.selection = None
prob = cv2.calcBackProject([hsv], [0], self.hist, [0, 180], 1)
prob &= mask
term_crit = ( cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 1 )
track_box, self.track_window = cv2.CamShift(prob, self.track_window, term_crit)
# show center of ellipse
center = (int(track_box[0][0]), int(track_box[0][1]))
cv2.circle(vis, center, 8, (0,255,0), -1)
# show destination
cv2.circle(vis, self.dest, 8, (0,0,255), -1) # red circle at destination
# show direction line
if self.dest:
cv2.line(vis, self.dest, center, (255,0,0), 2) # blue line to destination
# calculate orientation
or_angle = track_box[2]
# show orientation axis
pt2_x = int(round(center[0] + 100 * math.sin(-math.pi/180 * or_angle), 0))
pt2_y = int(round(center[1] + 100 * math.cos(-math.pi/180 * or_angle), 0))
pt2 = (pt2_x, pt2_y)
try:
cv2.line(vis, center, pt2, (0, 255, 0), 2) # green line through orientation axis
except:
pt2 = (center[0], center[1] + 30)
cv2.line(vis, center, pt2, (0, 255, 0), 2)
print(center)
try:
cv2.ellipse(vis, track_box, (0, 255, 0), 2) # green ellipse around selection
except:
print(track_box)
# Arduino calculations and movement
if self.dest:
# Calculate angle difference
# Note that angles will be measured from horizontal and in degrees
or_angle = 90 - or_angle # correct orientation angle for consistent angle measure
dir_dx = center[0] - self.dest[0]
dir_dy = center[1] - self.dest[1]
dir_angle = math.atan2(dir_dy, dir_dx) # atan2 is better than atan
dir_angle = 180 - dir_angle * 180/math.pi # correct direction angle
diff = dir_angle - or_angle
if diff > 180:
diff = 180 - diff
self.doAction(diff, center)
cv2.imshow('Arduino Tracker', vis)
# Quit on ESC
ch = 0xFF & cv2.waitKey(5)
if ch == 27:
break
self.robot.sendCommand('s')
cv2.destroyAllWindows()
class Mission():
# tested
def __init__(self):
if pi is True:
self.board = Arduino()
self.board.connect()
self.move = Motors()
GPIO.setup(GREEN_LED_GPIO,GPIO.OUT)
GPIO.setup(RED_LED_GPIO,GPIO.OUT)
GPIO.setup(BUTTON, GPIO.IN, pull_up_down=GPIO.PUD_UP)
# load the sample hash
with open(rfid_hash) as fh:
self.rfid_hash = json.load(fh)
# won't test
def startMission(self):
if pi is True:
GPIO.output(GREEN_LED_GPIO,False)
GPIO.output(RED_LED_GPIO,True)
print("waiting for button...")
while GPIO.input(BUTTON) == False:
time.sleep(0.1)
GPIO.output(RED_LED_GPIO,False)
# won't test
def endMission(self):
if pi is True:
GPIO.output(GREEN_LED_GPIO,True)
# tested
def deleteData(self):
try:
os.unlink(data_file)
except OSError:
pass
# tested
# stores a dict as json on the disk
# appends to the data file
def saveData(self, sample):
with open(data_file,'a') as fh:
fh.write(json.dumps(sample) + "\n")
# tested
# returns a list of dicts
def loadData(self):
data = []
try:
with open(data_file,) as fh:
for sample in fh.readlines():
data.append(json.loads(sample))
except IOError:
pass
return data
# won't test
# fetches an RFID
def getLocation(self):
if pi is True:
rfid = ""
# keep moving until robot gets a location
while True:
rfid = self.board.sendCommand(Commands.READ_RFID,0,0)
# when I catalogued the RFIDs I missed the last char!
rfid = rfid[0:11]
if len(rfid) == 11:
break
self.move.forward(70)
time.sleep(0.1)
self.move.stop()
time.sleep(1)
return rfid
# tested
# indexes into the sample database with the RFID and returns sample as dict
def takeSample(self, location):
try:
return self.rfid_hash[location]
except KeyError:
raise Exception("unknown location [%s]" % location)
# tested
# uploads a sample dict with a team name (string)
def uploadSample(self, sample, team):
# fetch team ID from nane
r = requests.get(mc_url + '/api/team/' + team)
if r.status_code == 400:
raise Exception(r.text)
team_id = json.loads(r.text)['id']
sample['team'] = str(team_id)
# posts it
r = requests.post(mc_url + '/api/sample', json=sample)
if r.status_code == 400:
return r.text
#exeception stops the program
#raise Exception(r.text)
new_sample = json.loads(r.text)
print("uploaded sample %d" % new_sample['id'])
return new_sample
def getAllSamples(self):
r = requests.get(mc_url + '/api/samples')
samples = json.loads(r.text)['samples']
return samples
class Motors():
def __init__(self):
self.board = Arduino()
self.board.connect()
def enable(self):
self.board.sendCommand(Commands.WRITE_DIGITAL,7,1) # puts motor drive into Phase/Enable mode
def disable(self):
self.board.sendCommand(Commands.WRITE_DIGITAL,7,0)# puts motor drive into IN/IN mode
def forward(self, speed):
self.enable()
commandL = self.speedToCommand(speed)
commandR = self.speedToCommand(speed)
self.board.sendCommand(Commands.WRITE_DIGITAL,LEFT_MOTOR_DIRECTION,1)
self.board.sendCommand(Commands.WRITE_DIGITAL,RIGHT_MOTOR_DIRECTION,0)
self.board.sendCommand(Commands.WRITE_PWM,LEFT_MOTOR_SPEED,commandR)
self.board.sendCommand(Commands.WRITE_PWM,RIGHT_MOTOR_SPEED,commandL)
def getLeft(self):
value = self.board.sendCommand(Commands.READ_LEFT_ENCODER,0,0)
return value
def getRight(self):
value = self.board.sendCommand(Commands.READ_RIGHT_ENCODER,0,0)
return value
def getRightDistance(self):
value = self.board.sendCommand(Commands.READ_RIGHT_DISTANCE,0,0)
return value
def getLeftDistance(self):
value = self.board.sendCommand(Commands.READ_LEFT_DISTANCE,0,0)
return value
def getAtPosition(self):
value = self.board.sendCommand(Commands.AT_POSITION,0,0)
return value
def backward(self, speed):
self.enable()
commandL = self.speedToCommand(speed)
commandR = self.speedToCommand(speed)
self.board.sendCommand(Commands.WRITE_PWM,LEFT_MOTOR_SPEED,commandL)
self.board.sendCommand(Commands.WRITE_PWM,RIGHT_MOTOR_SPEED,commandR)
self.board.sendCommand(Commands.WRITE_DIGITAL,LEFT_MOTOR_DIRECTION,0)
self.board.sendCommand(Commands.WRITE_DIGITAL,RIGHT_MOTOR_DIRECTION,1)
def leftMotor(self, speed, direction):
self.enable()
command = self.speedToCommand(speed)
if(direction==1):
self.board.sendCommand(Commands.WRITE_DIGITAL,LEFT_MOTOR_DIRECTION,1)
self.board.sendCommand(Commands.WRITE_PWM,LEFT_MOTOR_SPEED,command)
else:
self.board.sendCommand(Commands.WRITE_PWM,LEFT_MOTOR_SPEED,command)
self.board.sendCommand(Commands.WRITE_DIGITAL,LEFT_MOTOR_DIRECTION,0)
def rightMotor(self, speed, direction):
self.enable()
command = self.speedToCommand(speed)
if(direction==1):
self.board.sendCommand(Commands.WRITE_DIGITAL,RIGHT_MOTOR_DIRECTION,0)
self.board.sendCommand(Commands.WRITE_PWM,RIGHT_MOTOR_SPEED,command)
else:
self.board.sendCommand(Commands.WRITE_PWM,RIGHT_MOTOR_SPEED,command)
self.board.sendCommand(Commands.WRITE_DIGITAL,RIGHT_MOTOR_DIRECTION,1)
def stop(self):
self.board.sendCommand(Commands.WRITE_DIGITAL,LEFT_MOTOR_SPEED,0)
self.board.sendCommand(Commands.WRITE_DIGITAL,RIGHT_MOTOR_SPEED,0)
#self.position(0)
#self.rotation(0)
def speedToCommand(self,speed):
#speed between 0-100
command = int(speed * 2.55)
if(command > 255):
command = 255
return command
# lower level command to move
def position(self,pos,speed,block):
self.board.sendCommand(Commands.POSITION,DUMMY_PIN,pos)
if block:
there=int(self.getAtPosition())
while int(self.getAtPosition()) == 0:
print("on my way")
sleep(0.2)
print("got there! - Now I'll do next command")
def rotate(self,angle,speed,block):
self.board.sendCommand(Commands.ROTATE,DUMMY_PIN,angle)
if block:
there=int(self.getAtPosition())
while int(self.getAtPosition()) == 0:
print("on my way")
sleep(0.2)
print("got there! - Now I'll do next command")
def moveForward(self,pos,speed=DEFAULT_SPEED, block=True):
self.position(pos, speed, block)
def moveBackward(self,pos,speed=DEFAULT_SPEED, block=True):
self.position(-pos, speed,block)
def turnLeft(self,angle,speed=DEFAULT_SPEED, block=True):
self.rotate( -angle, speed, block)
def turnRight(self,angle,speed=DEFAULT_SPEED, block=True):
self.rotate( angle, speed, block)
def __del__(self):
self.stop()
