class MotorSkin(DualHBridge):
""" Specifics for 2 bidirectional motor driving (with speed control via PWM) """
MOT1_PINS = (pyb.Pin.board.X6, pyb.Pin.board.X5)
MOT1_PWM = {'pin': pyb.Pin.board.X3, 'timer': 5, 'channel': 3}
MOT2_PINS = (pyb.Pin.board.X7, pyb.Pin.board.X8)
MOT2_PWM = {'pin': pyb.Pin.board.X4, 'timer': 5, 'channel': 4}
(TRIGGER_PIN, ECHO_PIN) = (pyb.Pin.board.Y5, pyb.Pin.board.Y6)
ultrason = None
_switches = []
def __init__(self,
reverse_mot1=False,
reverse_mot2=False,
fix_rotate=False,
derivative_fix=0):
""" Initialize the DualHBridge L293D. Allows you to reverse/tune the
spinning commands for the motors when the underlaying plateform does not
move proprely when calling forward() or backward()
:param reverse_mot1: switch the forward/backward commands for motor 1
:param reverse_mot2: switch the forward/backward commands for motor 2
:param fix_rotate: set this True when the robot turn left instead of turning right as requested
:param derivative_fix: use +3 to speedup right motor when forward() does bend the path to the right.
use -3 to slow down the right motor when forward() bend the path to the left."""
mot1 = (self.MOT1_PINS[1],
self.MOT1_PINS[0]) if reverse_mot1 else self.MOT1_PINS
mot2 = (self.MOT2_PINS[1],
self.MOT2_PINS[0]) if reverse_mot2 else self.MOT2_PINS
if not fix_rotate:
super(MotorSkin, self).__init__(mot1, self.MOT1_PWM, mot2,
self.MOT2_PWM, derivative_fix)
else:
# Robot is apparently rotating the wrong way... this is because
# motor1 has been wired in place of the motor 2.
super(MotorSkin, self).__init__(mot2, self.MOT2_PWM, mot1,
self.MOT1_PWM, derivative_fix)
self.ultrason = Ultrasonic(self.TRIGGER_PIN, self.ECHO_PIN)
for pin in ('X18', 'X19', 'X20', 'X21'):
self._switches.append(
pyb.Pin(pin, pyb.Pin.IN, pull=pyb.Pin.PULL_UP))
def distance(self):
""" Read distance in cm from Ultrasonic sensor """
return self.ultrason.distance_in_cm()
def button_pin(self, button_nr):
""" Return Pin for a user button 1 to 4 (SW1..SW4) """
return self._switches[button_nr - 1]
def button_pressed(self, button_nr):
""" Check if a user button is pressed """
return self._switches[button_nr - 1].value() == 0
class MotorSkin( DualHBridge ):
""" Specifics for 2 bidirectional motor driving (with speed control via PWM) """
MOT1_PINS = ( pyb.Pin.board.X6, pyb.Pin.board.X5 )
MOT1_PWM = {'pin' : pyb.Pin.board.X3, 'timer' : 5, 'channel' : 3 }
MOT2_PINS = ( pyb.Pin.board.X7, pyb.Pin.board.X8 )
MOT2_PWM = {'pin' : pyb.Pin.board.X4, 'timer' : 5, 'channel' : 4 }
(TRIGGER_PIN,ECHO_PIN) = (pyb.Pin.board.Y5, pyb.Pin.board.Y6)
ultrason = None
_switches = []
def __init__( self, reverse_mot1 = False, reverse_mot2 = False, fix_rotate = False, derivative_fix = 0 ):
""" Initialize the DualHBridge L293D. Allows you to reverse/tune the
spinning commands for the motors when the underlaying plateform does not
move proprely when calling forward() or backward()
:param reverse_mot1: switch the forward/backward commands for motor 1
:param reverse_mot2: switch the forward/backward commands for motor 2
:param fix_rotate: set this True when the robot turn left instead of turning right as requested
:param derivative_fix: use +3 to speedup right motor when forward() does bend the path to the right.
use -3 to slow down the right motor when forward() bend the path to the left."""
mot1 = ( self.MOT1_PINS[1], self.MOT1_PINS[0] ) if reverse_mot1 else self.MOT1_PINS
mot2 = ( self.MOT2_PINS[1], self.MOT2_PINS[0] ) if reverse_mot2 else self.MOT2_PINS
if not fix_rotate:
super( MotorSkin, self ).__init__( mot1, self.MOT1_PWM, mot2, self.MOT2_PWM, derivative_fix )
else:
# Robot is apparently rotating the wrong way... this is because
# motor1 has been wired in place of the motor 2.
super( MotorSkin, self).__init__( mot2, self.MOT2_PWM, mot1, self.MOT1_PWM, derivative_fix )
self.ultrason = Ultrasonic( self.TRIGGER_PIN, self.ECHO_PIN )
for pin in ('X18','X19','X20','X21'):
self._switches.append( pyb.Pin( pin, pyb.Pin.IN, pull=pyb.Pin.PULL_UP) )
def distance( self ):
""" Read distance in cm from Ultrasonic sensor """
return self.ultrason.distance_in_cm()
def button_pin( self, button_nr ):
""" Return Pin for a user button 1 to 4 (SW1..SW4) """
return self._switches[ button_nr-1 ]
def button_pressed( self, button_nr ):
""" Check if a user button is pressed """
return self._switches[ button_nr-1 ].value() == 0
tone(1760, 300) # La
tone(1760, 150) # La
tone(2093, 150) # Do
tone(2637, 300) # Mi
tone(2349, 150) # Re
tone(2093, 150) # Do
#
tone(1975, 450) # Si
tone(2093, 150) # Do
tone(2349, 300) # Re
tone(2637, 300) # Mi
#
tone(2093, 300) # Do
tone(1760, 300) # La
tone(1760, 300) # La
#tone( 1975, 150) # Si
#tone( 2093, 150) # DO
# === Boucle principale ===========================
# Attendre action sur capteur ultrason
print("Pret")
while True:
if u.distance_in_cm() < 10:
print("Detection")
attente_savon()
print("Lavage")
lavage()
print("Pret")
musique()
def print_distances(trig_pin, echo_pin, count=300, delay=50):
sonar = Ultrasonic(trig_pin, echo_pin)
for i in range(count):
print(sonar.distance_in_cm())
pyb.delay(delay)
# along with this program. If not, see <http://www.gnu.org/licenses/>.
#
##
from pyb import delay
from r2wheel import Robot2Wheel
from ultrasonic import Ultrasonic
# Broche pour déclencher le senseur
TRIGGER_PIN = pyb.Pin.board.Y5
# Broche pour attendre le retour d'echo
ECHO_PIN = pyb.Pin.board.Y6
r2 = Robot2Wheel( reverse_mot2 = True )
u = Ultrasonic( TRIGGER_PIN, ECHO_PIN )
MIN_DISTANCE = 20 # Minimum distance
r2.forward()
while True:
if u.distance_in_cm() < MIN_DISTANCE:
r2.halt()
delay(100)
r2.right()
delay( 2500 )
r2.halt()
delay(100)
# If nothing in front then move
if (r2.state == Robot2Wheel.HALTED) and (u.distance_in_cm() > MIN_DISTANCE):
r2.forward()
#
##
from pyb import delay
from r2wheel import Robot2Wheel
from ultrasonic import Ultrasonic
# Broche pour déclencher le senseur
TRIGGER_PIN = pyb.Pin.board.Y5
# Broche pour attendre le retour d'echo
ECHO_PIN = pyb.Pin.board.Y6
r2 = Robot2Wheel(reverse_mot2=True)
u = Ultrasonic(TRIGGER_PIN, ECHO_PIN)
MIN_DISTANCE = 20 # Minimum distance
r2.forward()
while True:
if u.distance_in_cm() < MIN_DISTANCE:
r2.halt()
delay(100)
r2.right()
delay(2500)
r2.halt()
delay(100)
# If nothing in front then move
if (r2.state
== Robot2Wheel.HALTED) and (u.distance_in_cm() > MIN_DISTANCE):
r2.forward()
sw = Switch()
# Wait user A to be pressed
while not sw.value():
sleep(0.100)
# wait 10 secondes before start
for i in range(9):
LED(1).on()
sleep(0.100)
LED(1).off()
sleep(1)
# Final indicator
LED(1).on()
sleep(1)
LED(1).off()
try:
while True:
# if object detected
while sr04.distance_in_cm() <= 25:
# turn on right (positive speed) or left (negative speed) during a
# random amount of time (400ms to 1.5s) to find an escape.
zumo.right(speed=choice([-50, +50]), time_ms=randint(400, 1500))
if not zumo.is_moving:
zumo.speed(80) # Move forward @ speed
if sw.value():
break
finally:
zumo.stop()
