예제 #1
0
파일: neopixel.py 프로젝트: mattvenn/v2
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)
예제 #2
0
파일: encoder.py 프로젝트: mattvenn/v2
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
예제 #3
0
파일: ultrasound.py 프로젝트: mattvenn/v2
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
예제 #4
0
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()
예제 #5
0
파일: mission.py 프로젝트: mattvenn/v2
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
예제 #6
0
파일: motors.py 프로젝트: pi-bot/v2
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()