def main():
count = 40
print(
"Motor with stepper sensor example. Moves {0} steps forwards, then {0} steps backwards and stops."
.format(count))
#20210218 DPM: The code of this example is configured for the NUCLEO-L746RG board.
# Please, adapt according to the actual configuration.
PID_KP = 250.0
PID_KI = 0.0
PID_KD = 0.0
mpu = Mpu6050(1)
mpu.start()
motorLeft = Motor(Pin.board.D10, 4, 1, Pin.board.D11)
motorRight = Motor(Pin.board.D9, 8, 2, Pin.board.D8)
motorDriver = Driver(motorLeft, motorRight)
mc = MotionController(mpu, motorDriver, PID_KP, PID_KI,
PID_KD).setStepper(Stepper(Pin.board.D7,
Pin.PULL_UP))
try:
uasyncio_run(mainTask(mc, count))
finally:
mpu.cleanup()
motorDriver.cleanup()
def main():
print("Turn_to test")
#20210218 DPM: The code of this example is configured for the NUCLEO-L746RG board.
# Please, adapt according to the actual configuration.
PID_KP = 250.0
PID_KI = 0.0
PID_KD = 0.0
mpu = Mpu6050(1)
mpu.start()
motorLeft = Motor(Pin.board.D10, 4, 1, Pin.board.D11)
motorRight = Motor(Pin.board.D9, 8, 2, Pin.board.D8)
motorDriver = Driver(motorLeft, motorRight)
mc = MotionController(mpu, motorDriver, PID_KP, PID_KI,
PID_KD).setStepper(Stepper(Pin.board.D7,
Pin.PULL_UP))
try:
uasyncio_run(mainTask(mc))
finally:
mpu.cleanup()
motorDriver.cleanup()
def main():
print("Turn test")
#20210618 DPM: The code of this example is configured for the NUCLEO-L746RG board.
# Please, adapt according to the actual configuration.
PID_KP = 250.0
PID_KI = 0.0
PID_KD = 0.0
print("initializing MPU")
mpu=Mpu6050(1)
mpu.start()
print("MPU initialized")
motorLeft = Motor(Pin.board.D10, 4, 1, Pin.board.D11)
motorRight = Motor(Pin.board.D9, 8, 2, Pin.board.D8)
motorDriver = Driver(motorLeft, motorRight)
motion = MotionController(mpu, motorDriver, PID_KP, PID_KI, PID_KD)
try:
uasyncio_run(mainTask(motion))
finally:
motion.stop()
mpu.cleanup()
motorDriver.cleanup()
def __init__(self, leftMotor, leftIcTimerId, leftIcPin, rightMotor,
rightIcTimerId, rightIcPin):
'''
Constructor
@param leftMotor: The left motor
@param leftIcTimerId: The id-number of the timer for input-capturing on the left motor
@param leftIcPin: The pin where the capture of pulses on the left motor will be performed
@param rightIcTimerId: The id-number of the timer for input-capturing on the right motor
@param rightIcPin: The pin where the capture of pulses on the right motor will be performed
'''
Driver.__init__(self, leftMotor, rightMotor)
self._lpfAlpha = SmartDriver.DEFAULT_LPF_ALPHA
self._leftThrottle = 0.0
self._rightThrottle = 0.0
self._leftTimer = InputCapture(leftIcTimerId, leftIcPin)
self._rightTimer = InputCapture(rightIcTimerId, rightIcPin)
self._pidInputValues = [0.0] * SmartDriver.PID_RANGE
self._pid = PidCoroutine(SmartDriver.PID_RANGE, self._readPidInput,
self._setPidOutput, "SmartDriver-PID")
self._pid.setProportionalConstants([0.0] * SmartDriver.PID_RANGE)
self._pid.setIntegralConstants([0.0] * SmartDriver.PID_RANGE)
self._pid.setDerivativeConstants([0.0] * SmartDriver.PID_RANGE)
self._pid.setModulus([0.0, 0.0, 2 * pi])
self._mpu = None
def start(self):
if self._mpu != None:
self._mpu.start()
Driver.start(self)
self._pid.init(SmartDriver.PID_FREQ)
self._pid.setTargets([0.0] * SmartDriver.PID_RANGE)
def setMode(self, mode):
Driver.setMode(self, mode)
if self._mpu != None:
if mode == Driver.MODE_DRIVE:
self._pid.unlockIntegral(2)
self._pid.setTarget(2, self._mpu.readAngles()[2])
else:
self._pid.lockIntegral(2)
def cleanup(self):
'''
Releases and finishes all used resources
'''
self._pid.stop()
self._leftTimer.cleanup()
self._rightTimer.cleanup()
if self._mpu != None:
self._mpu.cleanup()
Driver.cleanup(self)
def main():
'''
Main application function
Initializes the resources, launches the activity and performs a heart-beat led running
'''
#TODO: 20200917 DPM Move the ultra-sound initialization to the robot-class
distanceSensor = Ultrasound(Pin.board.D2, Pin.board.D4)
motorLeft = Motor(Pin.board.D10, 4, 1, Pin.board.D11)
motorRight = Motor(Pin.board.D9, 8, 2, Pin.board.D8)
motorDriver = Driver(motorLeft, motorRight)
#TODO: 20200918 DPM Move the MPU initialization to the robot-class
mpu=Mpu6050(1)
mpu.start()
#TODO: 20200918 DPM Move the motion-controller initialization to the robot-class
motion = MotionController(mpu, motorDriver, PID_KP, PID_KI, PID_KD).setStepper(Stepper(Pin.board.D7, Pin.PULL_UP))
#TODO: 20200918 DPM Move the wifi-module initialization to the robot-class
esp = Esp8266(3, Pin.board.D3, 115200, debug=True)
esp.start()
robot = Robot()
# Add activities here:
robot.addActivity(RandomMotionActivity(motion, distanceSensor)) #.setObstacleLed(pyb.LED(3))
robot.addActivity(RemoteControlledActivity(motion, esp, distanceSensor))
robot.addActivity(MusicalInstrumentActivity(distanceSensor))
try:
robot.run()
finally:
robot.cleanup()
#TODO: 20200918 DPM move devices' cleanup into robot's
esp.cleanup()
mpu.cleanup()
motorDriver.cleanup()
distanceSensor.cleanup()
def main():
print("Initializing")
ml = Motor(Pin.board.D10, 4, 1, Pin.board.D11)
mr = Motor(Pin.board.D9, 8, 2, Pin.board.D8)
d = Driver(ml, mr)
mpu = Mpu6050(1)
mpu.start()
mc = MotionController(mpu, d, PID_KP, PID_KI, PID_KD)
mc.setThrottle(80.0)
mc.setRotation(60.0)
loop = get_event_loop()
loop.run_until_complete(_comain(mc))
loop.close()
mpu.cleanup()
d.cleanup()
print("Finished")
def main():
ml = Motor(pyb.Pin.board.D10, 4, 1, pyb.Pin.board.D11)
mr = Motor(pyb.Pin.board.D9, 8, 2, pyb.Pin.board.D8)
d = Driver(ml, mr)
d.setMode(Driver.MODE_DRIVE)
try:
d.setMotionVector(80, 0)
utime.sleep(3)
d.stop()
utime.sleep(1)
d.setMotionVector(-80, 0)
utime.sleep(3)
d.stop()
finally:
d.cleanup()