from gpiozero import Motor
import pygame
from pygame.locals import *
motor = Motor(2, 3)
motor.enable()
done = false
while not done:
event = pygame.event.poll()
if event.type == pygame.QUIT:
done = true
elif event.type == pygame.KEYUP:
if event.key == pygame.K_w:
motor.value = 0
if event.key == pygame.K_s:
motor.value = 0
else:
keys = pygame.key.get_pressed()
if keys[pygame.K_w]:
motor.forward()
elif keys[pygame.K_s]:
motor.backwards()
elif keys[pygame.K_SPACE]:
motor.stop()
#map theta (0-360) to number of LEDs (29?).
# MOVE BOT WITH RSTICK.
# TOLERANCE RANGE FOR MOTORS. REDUCE NOISE
motor_1 = 0;
motor_2 = 0;
motor_3 = 0;
motor_4 = 0;
if (Rmag > 0.20 or Rmag < -0.20):
motor_1 = Rmag*np.cos(theta);
motor_2 = Rmag*np.sin(theta);
motor_3 = Rmag*np.cos(theta);
motor_4 = Rmag*np.sin(theta);
else:
motor1.value = 0;
motor2.value = 0;
motor3.value = 0;
motor4.value = 0;
#ROTATE (YAW) BOT WITH LEFT STICK
#PRINT PREV MOTOR1 VALUES
if theta != prev_theta:
print('Rstick Angle: ', theta*(180/np.pi));
prev_theta = theta;
if motor_1 != prev_motor1_val:
print('Motor1 Output: ', motor_1);
prev_motor1_val = motor_1;
if motor_2 != prev_motor2_val:
print('Motor2 Output: ', motor_2);
from time import sleep
motor1 = Motor(27, 24)
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)
my_speed = 1
while (1):
motor1.value = my_speed # speed forwards
motor2.value = my_speed # speed forwards
motor3.value = -my_speed # speed backwards
motor4.value = -my_speed # speed backwards
# Leave the motor on for 3 seconds
sleep(0.2)
motor1.stop() # stop the motor
motor2.stop() # stop the motor
motor3.stop() # stop the motor
motor4.stop() # stop the motor
# Leave the motor off for 2 seconds
sleep(2)
def joystick_loop(j, ):
robot = Robot((27, 4), (6, 5))
fork = Motor(15, 23)
lift = Motor(20, 21)
print('Pins OK')
try:
axes = {}
d_pad = '(0, 0) '
buttons = []
acceleration_mode = False
stick_mode = False
forward = False
d_pad_x, d_pad_y = 0, 0
nom_left, nom_right = 0, 0
while True:
try:
events = pygame.event.get()
for event in events:
if event.type == pygame.JOYBUTTONDOWN:
buttons.append(str(event.button))
if event.button == 1:
# switch soft acceleration mode
acceleration_mode = not acceleration_mode
elif event.button == 2:
try:
# blocking scan function
scan()
try:
buttons.remove(str(event.button))
except ValueError:
pass
except Exception as e:
print(f'\r{e}')
elif event.button == 3:
# switch control map
stick_mode = not stick_mode
elif event.type == pygame.JOYBUTTONUP:
try:
buttons.remove(str(event.button))
except ValueError:
pass
elif event.type == pygame.JOYAXISMOTION:
if abs(event.value) > 0.1:
axes[event.axis] = f'{event.value:+.4f}'
else:
axes[event.axis] = f'{0.0:+.4f}'
elif event.type == pygame.MOUSEMOTION:
pass # print(event.rel, event.buttons)
elif event.type == pygame.MOUSEBUTTONDOWN:
pass
elif event.type == pygame.MOUSEBUTTONUP:
pass
elif event.type == pygame.JOYHATMOTION:
d_pad = f'{event.value} '
d_pad_x, d_pad_y = event.value
else:
print(event)
# logging and writing to motors
buttons.sort(key=lambda x: int(x))
ax = list(axes.items())
ax.sort(key=lambda x: x[0])
left, right = -float(axes.get(1, '+0.0')), -float(
axes.get(4, '+0.0'))
if stick_mode:
left, right = joy_to_motor_best(
-float(axes.get(0, '+0.0')),
-float(axes.get(1, '+0.0')))
if acceleration_mode:
left, right, nom_left, nom_right = calculate_diff(
left, right, nom_left, nom_right, diff=0.005)
if forward:
left, right = 0.1, 0.1
print('\rButtons: ' + d_pad + ' '.join(buttons) +
' Axes: ' + '; '.join([f'{k}: {v}' for k, v in ax]),
end=f' ({left}, {right})')
robot.value = (left, right)
lift.value = -d_pad_y
fork.value = -d_pad_x
except Exception as e:
print(f'\r{e}')
except KeyboardInterrupt:
print("\rEXITING NOW")
j.quit()
def main():
# 初期化
factory = PiGPIOFactory()
motor_left = Motor(forward=dcm_pins["left_forward"],
backward=dcm_pins["left_backward"],
pin_factory=factory)
motor_right = Motor(forward=dcm_pins["right_forward"],
backward=dcm_pins["right_backward"],
pin_factory=factory)
# 前進 -> 停止 -> 後進 -> 停止
try:
# 最高速で前進 - 1秒
print("最高速で前進 - 1秒")
motor_left.value = 1.0
motor_right.value = 1.0
sleep(1)
# 少し遅く前進 - 1秒
print("少し遅く前進 - 1秒")
motor_left.value = 0.75
motor_right.value = 0.75
sleep(1)
# 遅く前進 - 2秒
print("遅く前進 - 1秒")
motor_left.value = 0.5
motor_right.value = 0.5
sleep(1)
# 停止 - 1秒
motor_left.value = 0.0
motor_right.value = 0.0
sleep(1)
# 最高速で後進 - 1秒
print("最高速で後進 - 1秒")
motor_left.value = -1.0
motor_right.value = -1.0
sleep(1)
# 少し遅く後進 - 1秒
print("少し遅く後進 - 1秒")
motor_left.value = -0.75
motor_right.value = -0.75
sleep(1)
# 遅く後進 - 2秒
print("遅く後進 - 1秒")
motor_left.value = -0.5
motor_right.value = -0.5
sleep(1)
# 停止 - 1秒
motor_left.value = 0.0
motor_right.value = 0.0
sleep(1)
except KeyboardInterrupt:
print("stop")
# 停止
motor_left.value = 0.0
motor_right.value = 0.0
return
