Python Motor.setSpeed方法代碼示例

# 需要導入模塊: from motor import Motor [as 別名]
# 或者: from motor.Motor import setSpeed [as 別名]
class TantrumMotors:
    """
    This class is designed to control the motors of a Tyco Tantrum
    RC car. The Tantrum has a front motor controlling direction and
    a rear motor for moving forwards and backwards.
    """

    _front = 0  # Motor
    _rear = 0  # Motor
    _frontDriver = MotorDriver("P9_11", "P9_14")
    _rearDriver = MotorDriver("P9_12", "P9_16")

    # <public>
    # Initialize motors to forward, stopped
    def __init__(self):
        self._front = Motor(self._frontDriver)
        self._rear = Motor(self._rearDriver)
        self._front.setDirCw()
        self._front.setSpeed(0)
        self._rear.setDirCcw()
        self._rear.setSpeed(0)

    # Set the speed for all motors, if a speed is negative then the
    # motor is put in reverse.
    def setSpeed(self, front, rear):
        front = int(front)
        rear = int(rear)

        self._front.setSpeed(front)
        if (front < 0):
            self._front.setDirCcw()
        else:
            self._front.setDirCw()

        self._rear.setSpeed(rear)
        if (rear < 0):
            self._rear.setDirCcw()
        else:
            self._rear.setDirCw()

    def getFrontSpeed(self):
        return self._front.getSpeed()

    def getRearSpeed(self):
        return self._rear.getSpeed()

    def getFrontDirection(self):
        return self._front.getDirection()

    def getRearDirection(self):
        return self._rear.getDirection()

    def updateMotorState(self):
        self._frontDriver.updateState()
        self._rearDriver.updateState()

    def __exit__(self, type, value, traceback):
        self._frontDriver.shutdown()
        self._rearDriver.shutdown()

# 需要導入模塊: from motor import Motor [as 別名]
# 或者: from motor.Motor import setSpeed [as 別名]

#.........這裏部分代碼省略.........

	def __exit__(self, exc_type, exc_value, traceback):
		self.interface.terminate()

	#####################
	### Motor control ###
	#####################
	
	"""
	Sets PID values for all motors
	"""
	def setPID(self, p, i, d):
		self.motorL.setPID(p, i, d)
		self.motorR.setPID(p, i, d)
	
	"""
	Whether any of the motors are rotating
	"""
	def isMoving(self):
		return self.motorL.isRotating() or self.motorR.isRotating()

	"""
	Moves the given distance forwards or backwards
	"""
	def move(self, distance):
		# distance specified in metres
		wheel = distance * self.movementCoeff
		self.startLogging(['move', distance])
		self.motorL.rotate(self.powerL * wheel)
		self.motorR.rotate(self.powerR * wheel)
		self.stopLogging()
	
	"""
	Rotates the whole robot by the given angle clockwise
	"""
	def rotate(self, angle):
		# angle specified in degrees
		wheel = self.rotatePower * angle * (math.pi/180) * self.botRadius * self.movementCoeff
		self.startLogging(['turn', angle])
		self.motorL.rotate( self.powerL * wheel)
		self.motorR.rotate(-self.powerR * wheel)
		self.stopLogging()

	def drive(self, speedL, speedR):
		self.motorL.setSpeed(speedL)
		self.motorR.setSpeed(speedR)

	def arcPath(self, theta):
		# theta = 0 returns to straight path
		# Positive theta => too far to the right => turn left
		if theta <= 0 :
			self.powerL = 1 
			self.powerR = 1 + theta
		else: 		
			self.powerL = 1 - theta
			self.powerR = 1 
	
	"""
	Blocks the program until the robot stops moving
	"""
	def wait(self):
		while self.isMoving():
			time.sleep(self.deltaTime)

	######################
	### Sensor control ###
	######################

	"""
	Starts the main loop, checking sensors and responding to events until
	`running` is false
	"""
	def mainLoop(self):
		self.running = True
		while self.running:
			self.touchSensorL.check()
			self.touchSensorR.check()
			self.ultraSonic.check()
			self.motorL.check()
			self.motorR.check()
			time.sleep(self.deltaTime)

	def sensorAction(self, params):
		if(params['position'] != 'either' or not params['down']):
			return
		self.wait()
		# function triggered by the event handler when the touchsensor values are changed.
		if (self.touchSensorL.getState() == EventState.SENSOR_TOUCH_DOWN) \
			and (self.touchSensorR.getState() == EventState.SENSOR_TOUCH_DOWN):
			self.move(-20)
			self.wait()
			self.rotate(self, 90)
		elif self.touchSensorL.getState() == EventState.SENSOR_TOUCH_DOWN:
			self.move(-10)
			self.wait()
			self.rotate(self, 45)
		elif self.touchSensorL.getState() == EventState.SENSOR_TOUCH_DOWN:
			self.move(-10)
			self.wait()
			self.rotate(self, -45)