from gpiozero import Motor, LED from time import sleep switch = LED(4) switch.on() motora = Motor(forward=22, backward=17) motorb = Motor(forward=18, backward=27) motora.forward() motorb.forward() sleep(3)
from gpiozero import Motor from time import sleep mt = Motor(19, 13) mt.forward(0.5) sleep(3) mt.stop() sleep(1) mt.backward(0.5) sleep(3) mt.stop()
client = mqtt.Client()
client.on_connect = on_connect
client.on_message = on_message
client.connect('192.168.100.235', 1883, 60)
threading.Thread(target=client_loop, daemon=True).start()
while True:
lr_pos = lr_pos_r
fwrev_pos = fwrev_pos_r
lr_pos_r = 0
fwrev_pos_r = 0
time.sleep(0.1)
if lr_pos == 0 and fwrev_pos < 0: #forward
motl.forward(speed=-fwrev_pos)
motr.forward(speed=-fwrev_pos)
l_stop_val = -fwrev_pos
r_stop_val = -fwrev_pos
print('Forward : ', -fwrev_pos)
elif lr_pos == 0 and fwrev_pos > 0: #reverse
if us.get_distance() < 50:
print('[STOP] Obstacle detected')
motor_stop()
else:
print('Reverse : ', fwrev_pos)
motl.backward(speed=fwrev_pos)
motr.backward(speed=fwrev_pos)
from gpiozero import Motor
from time import sleep
# For two wheeled robot.
motor = Motor(forward=17, backward=14)
while True:
motor.forward()
sleep(5)
motor.backward()
sleep(5)
host = “01:23:45:67:89:AB”
port = 1
print("connected. type stuff")
def myThread():
while True:
rdata = sock.recv(1024)
print("received [%s]" % rdata)
th = threading.Thread(target=myThread, args=())
th.start()
motor = Motor(forward=20,backward=21)
while True:
sock=BluetoothSocket( RFCOMM )
if sock.connect((host, port)):
data = raw_input("LED OFF")
sock.send(data+'\n')
motor.forward(speed=0.3)
time.sleep(5)
motor.backward(speed=0.5)
time.sleep(5)
else: break
th.join()
sock.close()
def sstop():
ml.stop()
mr.stop()
except KeyboardInterrupt:
print "cleaning"
finally:
#GPIO.cleanup()
print"check it"
while(True):
print "Entering"
if (raw_input()=='w'):
print "forward"
forward()
elif raw_input()=='s':
print "backward"
backward()
elif raw_input()=='a':
print "left"
ml.backward(0.5)
mr.forward(0.5)
elif raw_input()=='d':
print "right"
mr.backward(0.5)
ml.forward(0.5)
else:
sstop()
print "stopped"
# print("Finish")
# x = input()
else
time.sleep(2)
#выключить двигатели
stop()
if dalnomer.distance < const_l2:
#значит, слева тупик и нужно поворачивать направо
t = time.time()
#задний ход
backward()
sleep(2)
#остановиться
stop()
#питание на левое колесо
motor_l.forward()
sleep(turn)
#поворот завершен
#питание на оба колеса
forward()
sleep(2)
else:
#нужен поворот налево
#задний ход
backward()
sleep(2)
#питание на правое колесо
motor_r.forward()
sleep(turn)
#поворот завершен
#питание на оба колеса
from gpiozero import Motor from time import sleep m1 = Motor(17, 18) m2 = Motor(23, 22) m1.forward() m2.forward() sleep(1) m1.stop() m2.stop() m1.backward(0.5) m2.backward(0.5) sleep(1) m1.stop() m2.stop()
class MecRobot():
def __init__(self):
self.frontLeft = Motor(6, 13)
self.frontRight = Motor(9, 11)
self.rearLeft = Motor(20, 21)
self.rearRight = Motor(17, 27)
# Motors are reversed. If you find your robot going backwards, set this to 1
self.motor_multiplier = -1
self.red = LED(16)
self.red.blink()
self.LineFollowingError = -1
def set_speeds(self, power_left, power_right):
# If one wants to see the 'raw' 0-100 values coming in
# print("source left: {}".format(power_left))
# print("source right: {}".format(power_right))
# Take the 0-100 inputs down to 0-1 and reverse them if necessary
power_left = (self.motor_multiplier * power_left) / 100
power_right = (self.motor_multiplier * power_right) / 100
# Print the converted values out for debug
# print("left: {}".format(power_left))
# print("right: {}".format(power_right))
# If power is less than 0, we want to turn the motor backwards, otherwise turn it forwards
if power_left < 0:
#print("power_left ",power_left)
self.frontLeft.forward(-power_left)
self.rearLeft.forward(-power_left)
else:
#print("power_left ",power_left)
self.frontLeft.backward(power_left)
self.rearLeft.backward(power_left)
if power_right < 0:
self.frontRight.forward(-power_right)
self.rearRight.forward(-power_right)
else:
self.frontRight.backward(power_right)
self.rearRight.backward(power_right)
def stop_motors(self):
"""
As we have an motor hat, stop the motors using their motors call
"""
# Turn both motors off
self.frontLeft.stop()
self.frontRight.stop()
self.rearLeft.stop()
self.rearRight.stop()
def left(self, power):
self.frontLeft.backward(power)
self.rearLeft.forward(power)
self.frontRight.forward(power)
self.rearRight.backward(power)
def right(self, power):
self.frontLeft.forward(power)
self.rearLeft.backward(power)
self.frontRight.backward(power)
self.rearRight.forward(power)
def rightDiagonal(self, power):
self.rearLeft.stop()
self.frontRight.stop()
if power > 0:
self.frontLeft.forward(power)
self.rearRight.forward(power)
else:
self.frontLeft.backward(-power)
self.rearRight.backward(-power)
def leftDiagonal(self, power):
self.frontLeft.stop()
if power > 0:
self.rearLeft.forward(power)
self.frontRight.forward(power)
else:
self.rearLeft.backward(-power)
self.frontRight.backward(-power)
self.rearRight.stop()
# Enable logging of debug messages, by default these aren't shown
# import logzero
# logzero.setup_logger(name='approxeng.input', level=logzero.logging.DEBUG)
def setLineFollowingError(self, error):
self.LineFollowingError = error
def mixer(self, yaw, throttle, max_power=100):
"""
Mix a pair of joystick axes, returning a pair of wheel speeds. This is where the mapping from
joystick positions to wheel powers is defined, so any changes to how the robot drives should
be made here, everything else is really just plumbing.
:param yaw:
Yaw axis value, ranges from -1.0 to 1.0
:param throttle:
Throttle axis value, ranges from -1.0 to 1.0
:param max_power:
Maximum speed that should be returned from the mixer, defaults to 100
:return:
A pair of power_left, power_right integer values to send to the motor driver
"""
left = throttle + yaw
right = throttle - yaw
scale = float(max_power) / max(1, abs(left), abs(right))
return int(left * scale), int(right * scale)
def robotloop(self, time2Go):
allFinished = False
normalMode = True
mecMode = 0
followLine = False
lineFollowSpeed = 1.0
lineFollowGain = 20.0
time2UpdateLineError = time.time()
errorThreshold = 0.02
# Outer try / except catches the RobotStopException we just defined, which we'll raise when we want to
# bail out of the loop cleanly, shutting the motors down. We can raise this in response to a button press
try:
while True:
# Inner try / except is used to wait for a controller to become available, at which point we
# bind to it and enter a loop where we read axis values and send commands to the motors.
try:
# Bind to any available joystick, this will use whatever's connected as long as the library
# supports it.
with ControllerResource(dead_zone=0.1,
hot_zone=0.2) as joystick:
print(
'Controller found, press HOME button to exit, use left stick to drive.'
)
print(joystick.controls)
# Loop until the joystick disconnects, or we deliberately stop by raising a
# RobotStopException
while joystick.connected:
tstart = time.time()
joystick.check_presses()
# Print out any buttons that were pressed, if we had any
if joystick.has_presses:
print(joystick.presses)
# If home was pressed, raise a RobotStopException to bail out of the loop
# Home is generally the PS button for playstation controllers, XBox for XBox etc
if 'home' in joystick.presses:
raise RobotStopException()
elif 'cross' in joystick.presses:
normalMode = not normalMode
# stop motors if switching mode
self.stop_motors()
elif 'dleft' in joystick.presses:
mecMode = 'L'
#print(mecMode)
elif 'dright' in joystick.presses:
mecMode = 'R'
#print(mecMode)
elif 'square' in joystick.presses:
mecMode = 'S'
elif 'start' in joystick.presses:
allFinished = True
raise RobotStopException()
elif 'l1' in joystick.presses:
mecMode = 'DL'
elif 'r1' in joystick.presses:
mecMode = 'DR'
elif 'circle' in joystick.presses:
followLine = not followLine
#print("followline ",followLine)
elif 'triangle' in joystick.presses:
lineFollowSpeed += 0.05
if lineFollowSpeed > 0.99:
lineFollowSpeed = 1.0
print(lineFollowSpeed)
elif 'dup' in joystick.presses:
lineFollowGain += 0.5
print(lineFollowGain)
elif 'ddown' in joystick.presses:
lineFollowGain -= 0.5
if lineFollowGain < 1.0:
lineFollowGain = 1.0
print(lineFollowGain)
# now process command
if normalMode:
# Get joystick values from the left analogue stick
x_axis, y_axis = joystick['lx', 'ly']
# Get power from mixer function
#print(x_axis, y_axis)
power_left, power_right = self.mixer(
yaw=x_axis, throttle=y_axis)
# Set motor speeds
self.set_speeds(power_left, power_right)
# Get a ButtonPresses object containing everything that was pressed since the last
# time around this loop.
else:
#mecanum mode
power = joystick['ry']
#print(power)
if mecMode == 'L':
self.left(abs(power))
elif mecMode == 'R':
self.right(abs(power))
elif mecMode == 'DL':
self.leftDiagonal(power)
elif mecMode == 'DR':
self.rightDiagonal(power)
else:
self.stop_motors()
if followLine:
endpoint = 'http://127.0.0.1:8000/lineError'
lineError = 0.0
if time.time() > time2UpdateLineError:
try:
response = requests.get(endpoint)
lineError = float(response.text)
except:
print("No response")
pass
time2UpdateLineError = time.time(
) + 0.1 # only get line error at 10Hz
if lineError > errorThreshold:
diagSpeed = lineError * lineFollowGain
if diagSpeed > 1.0:
diagSpeed = 1.0
self.rightDiagonal(diagSpeed)
elif lineError < -errorThreshold:
diagSpeed = -lineError * lineFollowGain # speed must be positive
if diagSpeed > 1.0:
diagSpeed = 1.0
self.leftDiagonal(diagSpeed)
else:
# now set speeds wants demand sabetween 0 and 100, lineFollowSpeed is between 0 and 1
# so multiply by 100
#forwardSpeed = lineFollowSpeed * 100
self.set_speeds(100, 100)
print("F")
print(lineError, diagSpeed, lineFollowGain)
loopTime = (time.time() - tstart)
sleepTime = 0.02 - loopTime # run at 50Hz
if sleepTime > 0:
sleep(sleepTime)
else:
sleep(0.02)
#print(loopTime)
except IOError:
# We get an IOError when using the ControllerResource if we don't have a controller yet,
# so in this case we just wait a second and try again after printing a message.
print('No controller found yet')
sleep(1)
except RobotStopException:
# This exception will be raised when the home button is pressed, at which point we should
# stop the motors.
self.stop_motors()
if allFinished:
os.system("sudo shutdown -h now")
print("going down")
else:
print("quit")
time2Go = True
self.red.off()
def rotate():
motor = Motor(26, 19)
motor.forward(0.5)
sleep(4.5)
motor.stop()
def slowMotor():
motorA = Motor(22,18,16)
motorA.forward(0.5)
sleep(2)
motorA.stop()
print "Motor running...."
from approxeng.input.selectbinder import ControllerResource
from gpiozero import Motor
from time import sleep
left_motor = Motor(8, 7)
right_motor = Motor(21, 20)
# Get a joystick
with ControllerResource(dead_zone=0.1, hot_zone=0.2) as joystick:
# Loop until disconnected
while joystick.connected:
left_y = joystick.ly
right_y = joystick.ry
#print(left_y, right_y)
if left_y < 0:
left_motor.backward(abs(left_y))
elif left_y > 0:
left_motor.forward(left_y)
elif left_y == 0:
left_motor.stop()
if right_y < 0:
right_motor.backward(abs(right_y))
elif right_y > 0:
right_motor.forward(right_y)
elif right_y == 0:
right_motor.stop()
class HS_interface():
def __init__(self):
rospy.init_node('hs_interface')
# ------ PARAMETRI ------
# frequenza del loop
self.freq = rospy.get_param('/eod/loop_freq/default')
self.rate = rospy.Rate(self.freq)
# Pin Encoder
self.pin_enc_dx = rospy.get_param('/eod/pin/pin_enc_dx')
self.pin_enc_sx = rospy.get_param('/eod/pin/pin_enc_sx')
# Setup encoder Destro
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.pin_enc_dx, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.add_event_detect(self.pin_enc_dx,
GPIO.RISING,
callback=callback_enc_dx,
bouncetime=3)
# Setup encoder Sinistro
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.pin_enc_sx, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.add_event_detect(self.pin_enc_sx,
GPIO.RISING,
callback=callback_enc_sx,
bouncetime=3)
# Pin Motore
self.pin_motor_dx_f = rospy.get_param('/eod/pin/pin_motor_dx_f')
self.pin_motor_dx_b = rospy.get_param('/eod/pin/pin_motor_dx_b')
self.pin_motor_sx_f = rospy.get_param('/eod/pin/pin_motor_sx_f')
self.pin_motor_sx_b = rospy.get_param('/eod/pin/pin_motor_sx_b')
# Setup motore Destro
self.motor_dx = Motor(self.pin_motor_dx_f, self.pin_motor_dx_b, True)
# Setup motore Sinistro
self.motor_sx = Motor(self.pin_motor_sx_f, self.pin_motor_sx_b, True)
# ------ TOPIC ------
rospy.Subscriber('motor_cmd', Wheels_Vel, self.motor_cmd_callback)
def loop(self):
while not rospy.is_shutdown():
self.rate.sleep()
def motor_cmd_callback(self, msg):
global dir_dx
global dir_sx
global v_input_dx
global v_input_sx
v_input_dx = msg.right
v_input_sx = msg.left
if v_input_dx >= 0:
self.motor_dx.forward(v_input_dx)
dir_dx = 1
else:
self.motor_dx.backward(-v_input_dx)
dir_dx = -1
if v_input_sx >= 0:
self.motor_sx.forward(v_input_sx)
dir_sx = 1
else:
self.motor_sx.backward(-v_input_sx)
dir_sx = -1
from gpiozero import Motor
from time import sleep
import Rpi.GPIO as GPIO
motor = Motor(forward=26, backward=20)
counter = 0
try:
while counter < 10:
motor.forward()
sleep(5)
motor.backward()
sleep(5)
counter +=1
finally:
GPIO.cleanup()
from gpiozero import Motor
import time
motor1 = Motor(9,10)
motor2 = Motor(7,8)
while True:
for i in range(2):
motor1.backward(speed=0.3)
motor2.forward(speed=0.9)
time.sleep(1)
for i in range(2):
motor1.backward(speed=1)
motor2.forward(speed=0.2)
time.sleep(1)
motor1.stop()
motor2.stop()
time.sleep(1)
import time
wheel_FR = Motor(14, 15)
wheel_FL = Motor(4, 17)
wheel_BL = Motor(27, 22)
wheel_BR = Motor(23, 24)
def allFourWheels(n):
wheel_FL.forward(n)
wheel_FR.forward(n)
wheel_BL.forward(n)
wheel_BR.forward(n)
while True:
for i in range(10):
vel = 1.0 - (float(i) / 10.0)
print(vel)
allFourWheels(vel)
#wheel_FL.forward(0.3)
#wheel_FR.forward(0.3)
#wheel_BL.forward(0.3)
#wheel_BR.forward(0.3)
time.sleep(1)
wheel_FL.forward(0)
wheel_FR.forward(0)
wheel_BL.forward(0)
wheel_BR.forward(0)
time.sleep(3)
from gpiozero import Motor from time import sleep m1 = Motor(14,15) speed = 0.5 m1.forward(speed) sleep(1) m1.stop() speed = 1 m1.backward(speed) sleep(1) m1.stop()
elif event.state == 1:
print("D pad Down")
mixer_results = mixer(x_axis, y_axis)
#print (mixer_results)
power_left = (mixer_results[1] / 125.0)
power_right = (mixer_results[0] / 125.0)
print("left: " + str(power_left) + " right: " + str(power_right))
if(power_left > 0):
leftMotor.forward(speed=power_left)
elif(power_left < 0):
leftMotor.backward(speed=-power_left)
else:
leftMotor.stop()
if(power_right > 0):
rightMotor.forward(speed=power_right)
elif(power_right < 0):
rightMotor.backward(speed=-power_right)
else:
rightMotor.stop()
print(event.ev_type, event.code, event.state)
from gpiozero import Motor # Importamos los módulos from time import sleep #Definimos Motor izquierdo (L) conectando a los pines 7 y 8 a la placa L298 motorL = Motor(forward = 7, backward = 8) #Definimos Motor derecho (R) conectando a los pines 9 y 9 a la placa L298 motorR = Motor(forward = 9, backward = 10) while True: # Bucle para siempre motorL.forward() # Motor L hacia adelante motorR.forward() # Motor R hacia adelante sleep(5) # Esperamos 5 segundos motorL.backward() # Motor L hacia atrás motorR.backward() # Motor R hacia atrás sleep(5) # Esperamos 5 segundos
if y < -500:
y = -500
self.last_p = p
self.last_d = d
return 0.5 + y / 1000
if __name__ == "__main__":
try:
# c_left.when_pressed = signal_get_left
# c_right.when_pressed = signal_get_right
g.setup(2, g.IN, pull_up_down=g.PUD_UP) # physicall no7
g.add_event_detect(2, g.FALLING, callback=signal_get)
g.setup(3, g.IN, pull_up_down=g.PUD_UP) # physicall no7
g.add_event_detect(3, g.FALLING, callback=signal_get)
motor_left.forward(0.5)
motor_right.forward(0.5)
m_left = PIDController()
while True:
ret1 = m_left.pid_follow(cn1, cn2)
motor_left.forward(ret1)
if time.perf_counter() - t >= 1:
t = time.perf_counter()
print(f"l {cn1:8d} r {cn2:8d} {cn1-cn2:8d} {ret1:.2f}")
time.sleep(0.05)
except KeyboardInterrupt:
motor_left.stop()
motor_right.stop()
print("user stop")
def push(t):
m.forward()
sleep(t)
m.stop()
def pull(t):
m.backward()
sleep(t)
m.stop()
m = Motor(In1, In2, En, False)
"""
m.forward()
print(m.is_active)
sleep(3)
m.reverse()
sleep(1)
m.stop()
"""
st()
"""
environ 20" pour faire un aller simple de l'actionneur.
Q? : comment faire tourner l'actionneur pendant 20"
puis 10" pour l'amener au milieu ?
course de l'actionneur : 208 - 11 = 197mm /2 = 98,5mm
après 10s : 12mm
class Robby:
def __init__(self):
self.left_motor = Motor(8, 7)
self.right_motor = Motor(9, 10)
def handle_command(self, message):
# convert to object
message = json.loads(message)
x_axis = message['x_axis']
y_axis = message['y_axis']
self.move(x_axis, y_axis)
def stop(self):
self.move(0, 0)
def move(self, x_axis, y_axis):
# forward
if y_axis > 0:
# right
if x_axis > 0:
self.left_motor.forward(1)
self.right_motor.forward(.5)
# straight
elif x_axis == 0:
self.left_motor.forward(1)
self.right_motor.forward(1)
# left
elif x_axis < 0:
self.left_motor.forward(.5)
self.right_motor.forward(1)
# sharp turn
elif y_axis == 0:
# right turn
if x_axis > 0:
strength = x_axis
self.left_motor.forward(strength)
self.right_motor.backward(strength)
# stop
elif x_axis == 0:
self.left_motor.stop()
self.right_motor.stop()
# left turn
elif x_axis < 0:
strength = abs(x_axis)
self.left_motor.backward(strength)
self.right_motor.forward(strength)
# backwards
elif y_axis < 0:
# right
if x_axis > 0:
self.left_motor.backward(1)
self.right_motor.backward(.5)
# straight
elif x_axis == 0:
self.left_motor.backward(1)
self.right_motor.backward(1)
# left
elif x_axis < 0:
self.left_motor.backward(.5)
self.right_motor.backward(1)
def start_js_poller():
motor1 = Motor(17, 18)
motor2 = Motor(27, 22)
motor3 = Motor(23, 24)
motor4 = Motor(6, 12)
kit = ServoKit(channels=16)
kit.servo[0].angle = 90
kit.servo[4].angle = 90
left_angle = 90
# right_angle = 90
while True:
lock.acquire()
# pprint(vars(controller_state))
if controller_state.ls_y > 0:
motor1.forward(controller_state.ls_y)
elif controller_state.ls_y < 0:
motor1.backward(-controller_state.ls_y)
else:
motor1.stop()
if controller_state.rs_y > 0:
motor2.forward(controller_state.rs_y)
elif controller_state.rs_y < 0:
motor2.backward(-controller_state.rs_y)
else:
motor2.stop()
if controller_state.rs_x > 0:
motor3.forward(controller_state.rs_x)
elif controller_state.rs_x < 0:
motor3.backward(-controller_state.rs_x)
else:
motor3.stop()
if controller_state.ls_x > 0:
motor4.forward(controller_state.ls_x)
elif controller_state.ls_x < 0:
motor4.backward(-controller_state.ls_x)
else:
motor4.stop()
if controller_state.a > 0:
kit.servo[0].angle = 180
kit.servo[4].angle = 0
if controller_state.b > 0:
kit.servo[0].angle = 0
kit.servo[4].angle = 180
if (controller_state.rt > -.8):
print(controller_state.rt)
left_angle = ((controller_state.rt + 1) / 2) + left_angle
if left_angle > 180:
left_angle = 180
elif left_angle < 0:
left_angle = 0
kit.servo[0].angle = left_angle
kit.servo[4].angle = 180 - left_angle
if (controller_state.lt > 0):
print(controller_state.lt)
left_angle = left_angle - ((controller_state.lt + 1) / 2)
if left_angle > 180:
left_angle = 180
elif left_angle < 0:
left_angle = 0
kit.servo[0].angle = left_angle
kit.servo[4].angle = 180 - left_angle
# print("s0 ", kit.servo[0].angle,"\ns4 ", kit.servo[4].angle)
# kit.servo[0].angle = 90
# kit.servo[4].angle = 90
lock.release()
sleep(1.0 / 60.0)
from gpiozero import Motor from time import sleep motor = Motor(17, 27) motor.forward(1) sleep(5) motor.stop() sleep(1) motor.backward(1) sleep(3) motor.stop()
#[email protected] from gpiozero import Motor, OutputDevice import time motor1 = Motor(24, 27) motor1_enable = OutputDevice(5, initial_value=1) motor2 = Motor(6, 22) motor2_enable = OutputDevice(17, initial_value=1) motor3 = Motor(23, 16) motor3_enable = OutputDevice(12, initial_value=1) motor4 = Motor(13, 18) motor4_enable = OutputDevice(25, initial_value=1) print "Motor 1, Right Front Forward..." motor1.forward() time.sleep(5) print "Motor 1 Right Front Backward..." motor1.backward() time.sleep(5) motor1.stop() print "Motor 2 Left Front Forward..." motor2.forward() time.sleep(5) motor2.stop() print "Motor 2 Left Front Backward..." motor2.backward() time.sleep(5) motor2.stop() print "Motor 3 Left Rear Forward..." motor3.forward()
udM.forward()
uStop.wait_for_press()
udM.stop()
#Home LR
returnToHome()
initProcedure()
musicNo = 0
while True:
pygame.mixer.music.load(moveMusic[musicNo])
fBl.on()
rBl.on()
fBut.wait_for_press()
pygame.mixer.music.play()
fbM.forward()
sleep(0.2)
while (fBut.is_pressed) and (fbStop.is_pressed == False):
sleep(0.01)
fbM.stop()
fBl.off()
rBut.wait_for_press()
lrM.forward()
sleep(0.2)
while (rBut.is_pressed) and (lrStop.is_pressed == False):
sleep(0.01)
lrM.stop()
rBl.off()
strength = calculateStrength()
grabProcedure(strength)
pygame.mixer.music.load(moveMusic[musicNo])
while wiimote_accel.read_one() != None:
pass
while wiimote_buttons.read_one() != None:
pass
### Wheel Control ###
if buttonHeld[4] and y>-90 and x<20 and x>-40 and direction != "forward":
print("Going forward")
direction = "forward"
if not debugMode:
leftWheelsInh.off()
rightWheelsInh.off()
time.sleep(delay)
leftWheels.forward()
rightWheels.backward()
leftWheelsInh.on()
rightWheelsInh.on()
else:
time.sleep(delay)
elif buttonHeld[4] and y>-90 and x>=35 and direction != "left":
print("Going left")
direction = "left"
if not debugMode:
leftWheelsInh.off()
rightWheelsInh.off()
time.sleep(delay)
leftWheels.backward()
rightWheels.forward()
lrcalc2xm1 = lrcalcx2 - 1
print("lrcalcx2m1 = ", lrcalc2xm1)
print("Full Left")
mannacalc = lrcalcc - (1 - lrcalcc)
print("Mannacalc =", mannacalc)
servo.value = lrcalc2xm1
if event.code == 01:
print("01")
print(event)
if event.code == 02:
print("02")
print(event)
if event.code == 9:
# this is for controlling the forward movement
print("09 - right trigger")
print(event.value)
if event.value > 1:
print("ON")
speedcalc = event.value / 1023.0
speedclean = round(speedcalc, 4)
print("speedclean ", speedclean)
motor.forward(speed=speedclean)
sleep(0.0001)
if event.value < 1:
print("OFF")
motor.stop()
# servo.min()
sleep(0.0001)
# led.off()
from gpiozero import Motor, AngularServo
#then import the 'sleep' class from the 'time' library (so we can add pauses in our program)
from time import sleep
gun_motor = Motor(13, 19)
laser = Motor(5, 6)
servo = AngularServo(26, min_angle=-40, max_angle=50)
retract = -40
fire = 50
print('laser on')
laser.forward()
print('servo back')
servo.angle = retract
sleep(0.5)
print('motors on')
gun_motor.forward(1)
sleep(5)
for x in range(5):
print('fire ', x + 1)
servo.angle = fire
sleep(0.5)
print('retract')
servo.angle = retract
sleep(3)
gun_motor.stop()
def izquierda():
global angulo
if angulo <= -26:
angulo = -26
print("No da mas a izquierda")
elif angulo > -26:
angulo -= 1
while True:
sleep(0.2)
desacelerar()
motor.forward(velocidad)
for event in pygame.event.get():
if event.type == pygame.QUIT:
sys.exit()
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_w:
acelerar()
motor.forward(velocidad)
elif event.key == pygame.K_s:
frenar()
traseraDer.blink(0.5, 0, 1, True)
traseraIzq.blink(0.5, 0, 1, True)
motor.forward(velocidad)
elif event.key == pygame.K_a:
izquierda()
class Py_Driver(): #Node):
def __init__(self, **kwargs):
#super().__init__('py_driver')
#self.subscription = self.create_subscription(Twist,'cmd_vel',self.listener_callback,0) # Store 10 messages, drop the oldest one
#self.subscription # prevent unused variable warning
self.kwargs = kwargs
# print(self.kwargs)
self._left_pwm = PWMOutputDevice(kwargs['left_pin'],
kwargs['left_active_high'])
self._left_motor = Motor(kwargs['left_forward'],
kwargs['left_backward'], kwargs['left_pwm'])
self._right_pwm = PWMOutputDevice(kwargs['right_pin'],
kwargs['right_active_high'])
self._right_motor = Motor(kwargs['right_forward'],
kwargs['right_backward'],
kwargs['right_pwm'])
print("Py Pi driver initialiased")
def listener_callback(self, msg):
# self.get_logger().info('Setting left & right wheel speed: %d %d' % (msg.linear.x, msg.angular.z))
_move_dict = {'linear': msg.linear.x, 'angular': msg.angular.z}
self.move = _move_dict
def test_motors(self):
# for i in range(0,6,1):
# i=(i+5)/10
# print(i)
i = 0.6
self._left_pwm.value = i
self._left_motor.forward()
self._right_pwm.value = i
self._right_motor.forward()
time.sleep(1.0)
self._left_motor.stop()
self._right_motor.stop()
# Code relevant for tuning the left and right wheel movement
@property
def move(self): #, linear_speed, angular_speed): # Use cmd_vel
print("Getting movement")
return self._move
# Check that movement is consistent for both wheels, maybe with a scalar in the PWM.
@move.setter
def move(self,
_move_dict): # Speed is with respect to the front of the car
if (abs(_move_dict['linear']) > 0.5):
_move_dict['linear'] = np.sign(_move_dict['linear']) * 0.5
print("Movement constrained")
if (abs(_move_dict['angular']) > 1.0):
_move_dict['angular'] = np.sign(_move_dict['angular']) * 1.0
print("Movement constrained")
if (abs(_move_dict['linear']) <= 0.5 and _move_dict['angular'] == 0.0):
print("Change")
self._left_pwm.value = abs(_move_dict['linear'] / 0.5)
self._right_pwm.value = abs(_move_dict['linear'] / 0.5)
if np.sign(_move_dict['linear']) == 1:
self._left_motor.forward()
self._right_motor.forward()
elif np.sign(_move_dict['linear']) == -1:
self._left_motor.backward()
self._right_motor.backward()
print("Acts")
print("Linear movement")
elif (abs(_move_dict['linear']) == 0.0
and abs(_move_dict['angular']) >= 1.0):
self._left_pwm.value = abs(_move_dict['angular'] / 1.0)
self._right_pwm.value = abs(_move_dict['angular'] / 1.0)
if np.sign(_move_dict['angular']) == 1:
self._left_motor.backward()
self._right_motor.forward()
print("Left wheel back, right wheel front")
elif np.sign(_move_dict['angular']) == -1:
self._left_motor.forward()
self._right_motor.backward()
print("Right wheel back, left wheel front")
print("This bucket", (np.sign(_move_dict['angular']) == False))
elif (abs(_move_dict['linear']) >= 0.5
and abs(_move_dict['angular']) >= 1.0):
if (np.sign(_move_dict['angular']) == 1
and np.sign(_move_dict['linear']) == 1):
self._left_pwm.value = 0.5
self._right_pwm.value = 1.0
self._left_motor.forward()
self._right_motor.forward()
elif (np.sign(_move_dict['angular']) == -1
and np.sign(_move_dict['linear']) == 1):
self._left_pwm.value = 1.0
self._right_pwm.value = 0.5
self._left_motor.forward()
self._right_motor.forward()
if (np.sign(_move_dict['angular']) == 1
and np.sign(_move_dict['linear']) == -1):
self._left_pwm.value = 1.0
self._right_pwm.value = 0.5
self._left_motor.backward()
self._right_motor.backward()
elif (np.sign(_move_dict['angular']) == -1
and np.sign(_move_dict['linear']) == -1):
self._left_pwm.value = 0.5
self._right_pwm.value = 1.0
self._left_motor.backward()
self._right_motor.backward()
print("Last bucket")
else:
self._left_pwm.value = abs(
(_move_dict['linear'] / 0.5 - _move_dict['angular'] / 1.0) /
2.0)
self._right_pwm.value = abs(
(_move_dict['linear'] / 0.5 + _move_dict['angular'] / 1.0) /
2.0)
if (np.sign(_move_dict['linear']) == 1):
self._left_motor.forward()
self._right_motor.forward()
elif (np.sign(_move_dict['linear']) == -1):
self._left_motor.forward()
self._right_motor.backward()
print("Really the last bucket now")
time.sleep(0.1)
self._left_motor.stop()
self._right_motor.stop()
#!/usr/bin/env python3
################################################################################
# 01.py
#
# Ex. 01: Motor spin
#
# 04.06.2019 Created by: rada
################################################################################
from gpiozero import Motor
from time import sleep
motor_right = Motor(forward=12, backward=6)
while(True):
try:
motor_right.forward(0.7)
sleep(3)
motor_right.backward(0.3)
sleep(3)
except:
print('Job has been aborted')
motor_right.stop()
break
import explorerhat
from gpiozero import Button, LED, Motor
from time import sleep
button = Button(9)
led1 = LED(10)
motor1 = Motor(forward=19 , backward=20)
motor2 = Motor(forward=21 , backward=26)
while True:
if button.is_pressed:
print("Button pressed")
led1.on()
motor1.forward()
motor2.forward()
else:
print("Button is not pressed")
led1.off()
motor1.stop()
motor2.stop()
PLF = 17 PLB = 22 PRF = 18 PRB = 23 pwmD = 2 #pin33 pwmG = 3 #pin35 #motorD = Motor(17, 18) #pin11,12 #motorG = Motor(22, 23) #pin13,15 motorD = Motor(PRF, PRB) #pin11,12 motorG = Motor(PLF, PLB) #pin13,15 GPIO.setup(pwmD, GPIO.OUT) GPIO.setup(pwmG, GPIO.OUT) pD = GPIO.PWM(pwmD, 100) pD.start(25) pG = GPIO.PWM(pwmG, 100) pG.start(25) motorD.forward(1) motorD.ChangeDutyCycle(12.5) #motorG.forward() time.sleep(3) #GPIO.cleanup()
forward()
backward()
left_turn()
backward()
left_turn()
backward()
left_turn()
backward()
#
if __name__ == '__main__':
cmd=''
while cmd is not 'e':
cmd=raw_input('Cmd:')
if cmd=='w':
print "forword 2 sec"
motor2.forward() # full speed forwards
motor3.forward() # full speed forwards
sleep(2)
elif cmd=='s':
print "backword 2 sec"
motor2.backward() # full speed backwards
motor3.backward() # full speed backwards
sleep(2)
elif cmd=='a':
print "left turn"
left_turn()
elif cmd=='d':
print "right turn"
right_turn()
elif cmd=='8':
print "run 8"
from gpiozero import Motor import time motorr = Motor(forward=22, backward=23) motorl = Motor(forward=18, backward=17) while True: print "Left motor forward" motorl.forward(1) time.sleep(0.5) motorl.forward(0) time.sleep(2) print "Right motor forward" motorr.forward(1) time.sleep(0.5) motorr.forward(0) time.sleep(2) print "Left motor backwards" motorl.backward(1) time.sleep(0.5) motorl.backward(0) time.sleep(2) print "Right motor backwards" motorr.backward(1) time.sleep(0.5) motorr.backward(0) time.sleep(2)
