def onEvent(self, evt): #### DO NOT MODIFY -------------------------------------------- # periodically, show the sensor reading we got from the waypointServer if self.timeForStatus(): self.showSensors() progress(self.robSim.logLaserValue(self.now)) # generate simulated laser readings elif self.timeForLaser(): self.robSim.logLaserValue(self.now) # update the robot and simulate the tagStreamer if self.timeForFrame(): self.emitTagMessage() #### MODIFY FROM HERE ON ---------------------------------------- if evt.type == KEYDOWN: if evt.key == K_UP and not self.moveP.isRunning(): self.moveP.dist = 100.0 self.moveP.start() return progress("(say) Move forward") elif evt.key == K_DOWN and not self.moveP.isRunning(): self.moveP.dist = -100.0 self.moveP.start() return progress("(say) Move back") if evt.key == K_LEFT and not self.turnP.isRunning(): self.turnP.ang = 0.5 self.turnP.start() return progress("(say) Turn left") if evt.key == K_RIGHT and not self.turnP.isRunning(): self.turnP.ang = -0.5 self.turnP.start() return progress("(say) Turn right") ### DO NOT MODIFY ----------------------------------------------- else: # Use superclass to show any other events return JoyApp.onEvent(self, evt) return # ignoring non-KEYDOWN events
def onEvent(self, evt): # periodically, show the sensor reading we got from the waypointServer if self.timeForStatus(): self.showSensors() progress(self.robSim.logLaserValue(self.now)) # generate simulated laser readings elif self.timeForLaser(): self.robSim.logLaserValue(self.now) # update the robot and simulate the tagStreamer if self.timeForFrame(): self.emitTagMessage() if evt.type == KEYDOWN: if evt.key == K_UP: self.robSim.move(0.5) return progress("(say) Move forward") elif evt.key == K_DOWN: self.robSim.move(-0.5) return progress("(say) Move back") elif evt.key == K_LEFT: self.robSim.turn(0.5) return progress("(say) Turn left") elif evt.key == K_RIGHT: self.robSim.turn(-0.5) return progress("(say) Turn right") # Use superclass to show any other events else: return JoyApp.onEvent(self, evt) return # ignoring non-KEYDOWN events
def onEvent(self,evt): if evt.type != KEYDOWN: return else: f = "1234567890".find(evt.unicode) if f>=0: progress("MXs to " + str(f)) for s in self.smx: s.set_ang(f*0.1) return return JoyApp.onEvent(self,evt)
def onEvent(self, evt): # periodically, show the sensor reading we got from the waypointServer if self.timeForStatus(): self.showSensors() progress(self.robSim.logLaserValue(self.now)) # generate simulated laser readings elif self.timeForLaser(): self.robSim.logLaserValue(self.now) # update the robot and simulate the tagStreamer if self.timeForFrame(): self.emitTagMessage() if evt.type == KEYDOWN: if evt.key == K_UP and not self.moveP.isRunning(): self.moveP.dist = 0.2 self.moveP.start() return progress("(say) Move forward") elif evt.key == K_DOWN and not self.moveP.isRunning(): self.moveP.dist = -0.2 self.moveP.start() return progress("(say) Move back") if evt.key == K_LEFT and not self.turnP.isRunning(): self.turnP.ang = 0.3 self.turnP.start() return progress("(say) Turn left") if evt.key == K_RIGHT and not self.turnP.isRunning(): self.turnP.ang = -0.3 self.turnP.start() return progress("(say) Turn right") elif evt.key == K_a: self.autoP.start() return progress("(say) Moving autonomously") elif evt.key == K_s: self.autoP.stop() return progress("(say) Stop autonomous control") # Use superclass to show any other events else: return JoyApp.onEvent(self, evt) return # ignoring non-KEYDOWN events
def onEvent(self, evt): """ The keyboard row: asdfghjkl moves your motors one way The keyboard row: zxcvbnm,. moves your motors the other way 'q' will quit and store the results in a results.png image and results.csv file. """ # Ignore everything except keydown events if evt.type != KEYDOWN: return # Punt exit keys if evt.key in {K_q, K_ESCAPE}: return JoyApp.onEvent(self, evt) # row of 'a' on QWERTY keyboard increments motors p = "asdfghjkl".find(evt.unicode) if p >= 0: self.arm[p].set_pos(self.arm[p].get_goal() + 500) return # row of 'z' in QWERTY keyboard decrements motors p = "zxcvbnm,.".find(evt.unicode) if p >= 0: self.arm[p].set_pos(self.arm[p].get_goal() - 500) return
def onEvent( self, evt ): # periodically, show the sensor reading we got from the waypointServer if self.timeForStatus(): self.showSensors() # generate simulated laser readings if self.timeForLaser(): progress( self.robSim.logLaserValue(self.now) ) # update the robot and simulate the tagStreamer if self.timeForFrame(): self.emitTagMessage() if self.timeForFeedback(): self.robSim.computeStep() #" #if evt.type == KEYDOWN: # #slip event # if evt.key == K_s: # self.robSim.slipmove() # elif evt.key == K_m: #if evt.key == K_UP: # self.robSim.move(0.5) # return progress('(say) Move forward') # elif evt.key == K_DOWN: # self.robSim.move(-0.5) # return progress('(say) Move back') # elif evt.key == K_LEFT: # self.robSim.turn(-0.5) # return progress('(say) Turn left') # elif evt.key == K_RIGHT: # self.robSim.turn(0.5) # return progress('(say) Turn right') # else: # self.robSim.move() # Use superclass to show any other events return JoyApp.onEvent(self,evt)
def onEvent(self, evt): if evt.type == KEYDOWN: return JoyApp.onEvent(self, evt)
def onEvent( self , evt ): #print 'Event Occurred', str(evt) if self.timeForControl(): # Print self.currState = self.vplan.getState() # Get latest state feedback from vision system self.printState() waypointsRaw = np.float32(self.sensor.lastWaypoints[1]) waypoints = self.swapWaypointXY(waypointsRaw) print 'Waypoints:',str(waypoints) self.vplan.setWaypoints(waypoints)# Pass waypoints to vision system self.tagEst = self.robotTagLocation() print 'Tag Estimate', str(self.tagEst) #self.vplan.setTagLocation(self.tagEst) #print 'Current state:', str(currState) if(self.opState == DoritoState.RUNNING): # Only update commands while running print 'Handling Control Update' print 'Found', len(waypoints), ' waypoints' if(len(waypoints) > 0): # If waypoint exists currWp = waypoints[1] print 'Next Waypoint:', str(currWp) # tError, rError = self.controlHandler(currWp) # Run control handler print 'X-Y Error', tError #self.vplan.setControlVectorRender(self.tagEst, self.tagEst + tError) print 'Rotation Error', rError else: print 'No more waypoints available, holding' if(self.servoErrorFlag == False): print self.servoErrorFlag #self.drive.setSpeed(np.asfarray([0.,0.]), 0) # Send command requests to the motion drive# Stop holdWP = vu.toWaypointCoordinates( np.array( [self.currState['tagX'],self.currState['tagY']] )) # Current tag location in waypoint space self.controlHandler(holdWP)# Run controller to hold orientation # Manual control handling if evt.type == KEYUP: if evt.key == K_UP: self.controls['up'] = False elif evt.key == K_DOWN: self.controls['down'] = False elif evt.key == K_LEFT: self.controls['left'] = False elif evt.key == K_RIGHT: self.controls['right'] = False elif evt.key == K_PAGEUP: self.controls['ccw'] = False elif evt.key == K_PAGEDOWN: self.controls['cw'] = False self._parseControls() if evt.type == KEYDOWN: if evt.key == K_UP: self.controls['up'] = True elif evt.key == K_DOWN: self.controls['down'] = True elif evt.key == K_LEFT: self.controls['left'] = True elif evt.key == K_RIGHT: self.controls['right'] = True elif evt.key == K_PAGEUP: self.controls['ccw'] = True elif evt.key == K_PAGEDOWN: self.controls['cw'] = True elif evt.key == K_r: # Run mode print '=== Set mode to Run ===\n\n' self.opState = DoritoState.RUNNING elif evt.key == K_s: # E-stop self.opState = DoritoState.STOPPED if(self.servoErrorFlag == False): self.drive.stopAll() elif evt.key == K_m: # Manual drive self.opState = DoritoState.MANUAL self._parseControls() return JoyApp.onEvent(self,evt)
def onEvent(self, evt): # periodically, show the sensor reading we got from the waypointServer if self.timeForStatus(): self.showSensors() print( "CurrPos(Camera): " + str( self.core.coordinateFrames.convertRealToCamera( self.robSim.pos))) print("CurrPos(Real): " + str(self.robSim.pos)) try: # print("Estimated Pos(Real): " + str(self.core.particleFilter.getState().pos) + "\t" + str(self.core.particleFilter.getState().yaw)) print( "CurrPos(waypts): " + str( self.core.coordinateFrames.convertRealToWaypoint( self.robSim.pos)) + "\t" + str(self.robSim.yaw - self.core.coordinateFrames.getRealToWaypointYaw())) print("EstimatedPos(waypts): " + str(self.core.particleFilter.getWaypointState().pos) + "\t" + str(self.core.particleFilter.getWaypointState().yaw)) except: pass progress(self.robSim.logLaserValue(self.now)) # logging # if (self.core.coordinateFrames.waypoint != None): # rotatedLength = CoordinateFrames.rotateCCW([Constants.tagLength, 0], self.robSim.yaw - self.core.coordinateFrames.getRealToWaypointYaw()) # posWaypoint = self.core.coordinateFrames.convertRealToWaypoint(self.robSim.pos) # frontSensor = posWaypoint[1] + rotatedLength[1] # backSensor = posWaypoint[1] - rotatedLength[1] # if (self.sensor.lastSensor[1] > 5 and self.sensor.lastSensor[2] > 5): # self.logFile.write(str([self.sensor.lastSensor[1], self.sensor.lastSensor[2]])) # self.logFile.write(str([frontSensor, backSensor]) + "\n") # print(rotatedLength) # print(str([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]) + str([frontSensor, backSensor])) # generate simulated laser readings elif self.timeForLaser(): self.robSim.logLaserValue(self.now) # update the robot and simulate the tagStreamer if self.timeForFrame(): self.emitTagMessage() if self.timeForFilter(): self.core.setSensorAndWaypoints( array([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]), self.sensor.lastWaypoints[1]) if self.timeForAuto() and self.auto: self.core.autonomousPlanner.plan(self.sensor.lastWaypoints[1]) if evt.type == KEYDOWN: if evt.key == K_UP: self.core.movePosY() return progress("(say) Up") elif evt.key == K_DOWN: self.core.moveNegY() return progress("(say) Down") elif evt.key == K_LEFT: self.core.moveNegX() return progress("(say) Left") elif evt.key == K_RIGHT: self.core.movePosX() return progress("(say) Right") elif evt.key == K_RETURN: self.startedFilter = not self.startedFilter if (self.startedFilter): self.core.startFilter() return progress("(say) Started Filter") else: self.core.stopFilter() return progress("(say) Stopped Filter") elif evt.key == K_TAB: self.auto = ~self.auto if self.auto: return progress("(say) Autonomous On") else: return progress("(say) Autonomous Off") else: return JoyApp.onEvent(self, evt) return # ignoring non-KEYDOWN events
def onEvent(self, evt): if self.oe(): progress("OE:" + str(self.dt.blobs[-1:])) # Punt keydown events to superclass if evt.type == KEYDOWN: return JoyApp.onEvent(evt)
def onEvent(self, evt): # periodically, show the sensor reading we got from the waypointServer if self.timeForStatus(): self.showSensors() return JoyApp.onEvent(self, evt)
def onEvent( self, evt ): if self.watchdogTime(): self.watchdogTick = self.watchdogTick - 1 if(self.watchdogTick == 0): self._setMotorCommand(self.c.at.D0,0) self._setMotorCommand(self.c.at.D1,0) self._setMotorCommand(self.c.at.D2,0) # TODO: watchdog if evt.type == KEYUP: self.watchdogTick = 2 if evt.key == K_UP: self.controls['up'] = False #self.setSpeed((0,1),0) elif evt.key == K_DOWN: self.controls['down'] = False #self.setSpeed((0,-1),0) elif evt.key == K_LEFT: self.controls['left'] = False #self.setSpeed((-1,0),0) elif evt.key == K_RIGHT: self.controls['right'] = False #self.setSpeed((1,0),0) elif evt.key == K_PAGEUP: self.controls['ccw'] = False #self.setSpeed((0,0),1) elif evt.key == K_PAGEDOWN: self.controls['cw'] = False #self.setSpeed((0,0),-1) self._parseControls() ''' if evt.key == K_UP or \ evt.key == K_DOWN or \ evt.key == K_LEFT or \ evt.key == K_RIGHT or \ evt.key == K_PAGEUP or \ evt.key == K_PAGEDOWN: self.setDriveMotorCommands([0,0,0]) ''' if evt.type == KEYDOWN: self.watchdogTick = 2 if evt.key == K_UP: self.controls['up'] = True #self.setSpeed((0,1),0) elif evt.key == K_DOWN: self.controls['down'] = True #self.setSpeed((0,-1),0) elif evt.key == K_LEFT: self.controls['left'] = True #self.setSpeed((-1,0),0) elif evt.key == K_RIGHT: self.controls['right'] = True #self.setSpeed((1,0),0) elif evt.key == K_PAGEUP: self.controls['ccw'] = True #self.setSpeed((0,0),1) elif evt.key == K_PAGEDOWN: self.controls['cw'] = True #self.setSpeed((0,0),-1) self._parseControls() # Use superclass to show any other events return JoyApp.onEvent(self,evt)
def onEvent( self, evt ): # periodically, show the sensor reading we got from the waypointServer if self.timeForStatus(): self.showSensors() print("CurrPos(Camera): " + str(self.core.coordinateFrames.convertRealToCamera(self.robSim.pos))) print("CurrPos(Real): " + str(self.robSim.pos)) try: # print("Estimated Pos(Real): " + str(self.core.particleFilter.getState().pos) + "\t" + str(self.core.particleFilter.getState().yaw)) print("CurrPos(waypts): " + str(self.core.coordinateFrames.convertRealToWaypoint(self.robSim.pos)) + "\t" + str(self.robSim.yaw - self.core.coordinateFrames.getRealToWaypointYaw())) print("EstimatedPos(waypts): " + str(self.core.particleFilter.getWaypointState().pos) + "\t" + str(self.core.particleFilter.getWaypointState().yaw)) except: pass progress( self.robSim.logLaserValue(self.now) ) # logging # if (self.core.coordinateFrames.waypoint != None): # rotatedLength = CoordinateFrames.rotateCCW([Constants.tagLength, 0], self.robSim.yaw - self.core.coordinateFrames.getRealToWaypointYaw()) # posWaypoint = self.core.coordinateFrames.convertRealToWaypoint(self.robSim.pos) # frontSensor = posWaypoint[1] + rotatedLength[1] # backSensor = posWaypoint[1] - rotatedLength[1] # if (self.sensor.lastSensor[1] > 5 and self.sensor.lastSensor[2] > 5): # self.logFile.write(str([self.sensor.lastSensor[1], self.sensor.lastSensor[2]])) # self.logFile.write(str([frontSensor, backSensor]) + "\n") # print(rotatedLength) # print(str([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]) + str([frontSensor, backSensor])) # generate simulated laser readings elif self.timeForLaser(): self.robSim.logLaserValue(self.now) # update the robot and simulate the tagStreamer if self.timeForFrame(): self.emitTagMessage() if self.timeForFilter(): self.core.setSensorAndWaypoints(array([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]), self.sensor.lastWaypoints[1]) if self.timeForAuto() and self.auto: self.core.autonomousPlanner.plan(self.sensor.lastWaypoints[1]) if evt.type == KEYDOWN: if evt.key == K_UP: self.core.movePosY() return progress("(say) Up") elif evt.key == K_DOWN: self.core.moveNegY() return progress("(say) Down") elif evt.key == K_LEFT: self.core.moveNegX() return progress("(say) Left") elif evt.key == K_RIGHT: self.core.movePosX() return progress("(say) Right") elif evt.key == K_RETURN: self.startedFilter = not self.startedFilter if (self.startedFilter): self.core.startFilter() return progress("(say) Started Filter") else: self.core.stopFilter() return progress("(say) Stopped Filter") elif evt.key == K_TAB: self.auto = ~self.auto if self.auto: return progress("(say) Autonomous On") else: return progress("(say) Autonomous Off") else: return JoyApp.onEvent(self,evt) return # ignoring non-KEYDOWN events