-
-
Save artpoz/d66f72a6092b580b9903d088dd6d1262 to your computer and use it in GitHub Desktop.
| #!/usr/bin/env python3 | |
| __author__ = 'Artur Poznanski' | |
| import evdev | |
| import ev3dev.auto as ev3 | |
| import threading | |
| import time | |
| ## Some helpers ## | |
| def clamp(n, minn, maxn): | |
| return max(min(maxn, n), minn) | |
| def scale(val, src, dst): | |
| return (float(val - src[0]) / (src[1] - src[0])) * (dst[1] - dst[0]) + dst[0] | |
| def scale_stick(value): | |
| return scale(value,(0,255),(-1000,1000)) | |
| def dc_clamp(value): | |
| return clamp(value,-1000,1000) | |
| ## Initializing ## | |
| print("Finding ps4 controller...") | |
| devices = [evdev.InputDevice(fn) for fn in evdev.list_devices()] | |
| ps4dev = devices[0].fn | |
| gamepad = evdev.InputDevice(ps4dev) | |
| forward_speed = 0 | |
| side_speed = 0 | |
| running = True | |
| class MotorThread(threading.Thread): | |
| def __init__(self): | |
| self.right_motor = ev3.LargeMotor(ev3.OUTPUT_C) | |
| self.left_motor = ev3.LargeMotor(ev3.OUTPUT_B) | |
| threading.Thread.__init__(self) | |
| def run(self): | |
| print("Engine running!") | |
| while running: | |
| self.right_motor.run_forever(speed_sp=dc_clamp(forward_speed+side_speed)) | |
| self.left_motor.run_forever(speed_sp=dc_clamp(-forward_speed+side_speed)) | |
| self.right_motor.stop() | |
| self.left_motor.stop() | |
| motor_thread = MotorThread() | |
| motor_thread.setDaemon(True) | |
| motor_thread.start() | |
| for event in gamepad.read_loop(): #this loops infinitely | |
| if event.type == 3: #A stick is moved | |
| if event.code == 0: #X axis on left stick | |
| forward_speed = -scale_stick(event.value) | |
| if event.code == 1: #Y axis on left stick | |
| side_speed = -scale_stick(event.value) | |
| if side_speed < 100 and side_speed > -100: | |
| side_speed = 0 | |
| if forward_speed < 100 and forward_speed > -100: | |
| forward_speed = 0 | |
| if event.type == 1 and event.code == 305 and event.value == 1: | |
| print("X button is pressed. Stopping.") | |
| running = False | |
| time.sleep(0.5) # Wait for the motor thread to finish | |
| break |
I am using your code with these motor changes:
def init(self):
self.right_motor = ev3.LargeMotor(ev3.OUTPUT_C)
self.left_motor = ev3.LargeMotor(ev3.OUTPUT_B)
self.steering_motor = ev3.MediumMotor(ev3.OUTPUT_A)
self.steering_motor.reset()
threading.Thread.init(self)
The two LargeMotors directly drive the two rear wheels. The MediumMotor operates the steering through two knob gears. The change in the knob gears is very little. I would say about 30 degrees from all the way on the left to all the way on the right. The original code had the left joystick operate the drive wheels. So I thought I could program the right joystick to operate the steering motor. In Daniel’s book he creates a steering block using EVG. The steering block uses a +/- 30 degree value with zero being center or straight ahead.
I would also try these changes:
In the motor thread replace the old set speed lines with this:
self.right_motor.run_forever(speed_sp=dc_clamp(magnitude))
self.left_motor.run_forever(speed_sp=dc_clamp(magnitude))
And this replaces the old theta and magnitude calculation:
# calculate theta if outside of deadzone
if abs(rx) > .2 or abs(rx) > .2:
theta = rx * 30 # calculating angle
# calculate magnitude
magnitude = ly * 700
This should make ly control forward and backwards on the drive motors, and rx controls the steering, and has a min/max of +/- 30 degrees. If this doesn't work the steering_motor might be outputting radians, in this case make sure that you are still outputting the position with this (assuming that use have a terminal to print to. I'm using PuTTY):
print("currentPos:", self.steering_motor.position)
If the amount is a lot lower than it should be then it is in radians. If it is in radians replace the old error calculation with this:
error = (theta - math.degrees(self.steering_motor.position))
Thank you the changes worked. I did have to change this line:
theta = rx * 30 # calculating angle
to
theta = rx * -30 # calculating angle
The right joystick was backwards. When I changed 30 to -30 when I move the joystick to the left than the steering wheels also moved to the left. The only thing I have noticed is that the drive wheels respond very quickly to joystick moves, but the steering is not as responsive. I am not sure why. I thought maybe is was too lower a power setting??? I tried changing the 700 to 900 in the power setting lines. But, that did not have any thing.
Mike
I downloaded this to my brick, connected controller, and ran the file, but the screen goes blank and the controller doesn't do anything. What did I do wrong, is there new code?
One of the things that I noticed is it takes the program a bit of time before it starts running on the EV3. When the program is ready to receive commands from the controller the display will start showing very small text.
@hobojoe972
Do you have "ps4explor3r.py.err.log" file in /home/robot/ on brick? Can you copy this file to PC and paste it here?
Hello, I'm trying to add to your design a claw with a Medium Motor to the code, but no success so far. My Python knowledge is incipient.
Can you please hint me what I do wrong? I attempted several possible ways to link the right stick or some of the buttons of the PS4 controller to move the claw engine.
However, the program starts, I can ride the drive train, but as soon as I touch the controls for the servo motor to activate the claw, the drive train loses power and the wheel engines do not work anymore.
I'm attaching the modified code:
#!/usr/bin/env python3
author = 'Artur Poznanski'
import evdev
import ev3dev.auto as ev3
import threading
import time
Some helpers
def clamp(n, minn, maxn):
return max(min(maxn, n), minn)
def scale(val, src, dst):
return (float(val - src[0]) / (src[1] - src[0])) * (dst[1] - dst[0]) + dst[0]
def scale_stick(value):
return scale(value,(0,255),(-1000,1000))
def dc_clamp(value):
return clamp(value,-1000,1000)
Initializing
print("Finding ps4 controller...")
devices = [evdev.InputDevice(fn) for fn in evdev.list_devices()]
ps4dev = devices[0].fn
gamepad = evdev.InputDevice(ps4dev)
forward_speed = 0
side_speed = 0
grab_speed = 0
running = True
class MotorThread(threading.Thread):
def init(self):
self.right_motor = ev3.LargeMotor(ev3.OUTPUT_C)
self.left_motor = ev3.LargeMotor(ev3.OUTPUT_B)
self.claw_motor = ev3.MediumMotor(ev3.OUTPUT_A)
threading.Thread.init(self)
def run(self):
print("Engine running! for Stefan")
while running:
self.right_motor.run_forever(speed_sp=dc_clamp(forward_speed+side_speed))
self.left_motor.run_forever(speed_sp=dc_clamp(-forward_speed+side_speed))
self.claw_motor.run_direct(duty_cycle_sp=grab_speed)
self.right_motor.stop()
self.left_motor.stop()
self.claw_motor.stop()
motor_thread = MotorThread()
motor_thread.setDaemon(True)
motor_thread.start()
for event in gamepad.read_loop(): #this loops infinitely
# map the controller left analog stick to the two driving motors
if event.type == 3: #left stick is moved
if event.code == 0: #X axis on left stick
forward_speed = -scale_stick(event.value)
if event.code == 1: #Y axis on left stick
side_speed = -scale_stick(event.value)
if side_speed < 100 and side_speed > -100:
side_speed = 0
if forward_speed < 100 and forward_speed > -100:
forward_speed = 0
# map the grab claw/MediumMotor
if event.type == 3:
if event.code == 2:
grab_speed = -scale_stick(event.value)
if grab_speed < 100 and grab_speed > -100:
grab_speed = 0
# button O exits
if event.type == 1 and event.code == 305 and event.value == 1:
print("X button is pressed. Stopping.")
running = False
time.sleep(0.5) # Wait for the motor thread to finish
break
well, thanks Quantum357,
With my previous setup, the drivetrain, and, in fact, almost all functions stopped after moving the right stick, seemingly like the defective activation of the claw engine interfered and hindered the rest of the functions. Just the O button continued to work, so I still could get out of the loop by simply exiting the program.
Before touching the right stick, the left stick worked normally, and the drive train was active. However, touching the right stick suspended the functionality irreversibly (well, until a program restart, at least).
meanwhile, the error log showed this:
Exception in thread Thread-1:
Traceback (most recent call last):
File "/usr/lib/python3.5/threading.py", line 914, in _bootstrap_inner
self.run()
File "/home/robot/ps44r.py", line 46, in run
self.claw_motor.run_direct(duty_cycle_sp=grab_speed)
File "/usr/lib/python3/dist-packages/ev3dev/core.py", line 834, in run_direct
setattr(self, key, kwargs[key])
File "/usr/lib/python3/dist-packages/ev3dev/core.py", line 461, in duty_cycle_sp
self._duty_cycle_sp = self.set_attr_int(self._duty_cycle_sp, 'duty_cycle_sp', value)
File "/usr/lib/python3/dist-packages/ev3dev/core.py", line 223, in set_attr_int
return self._set_attribute(attribute, name, str(int(value)))
File "/usr/lib/python3/dist-packages/ev3dev/core.py", line 211, in _set_attribute
attribute.write(value.encode())
OSError: [Errno 22] Invalid argument
Back to the Present Tense:
After I applied the change you suggested, the faulty behaviour indeed disappeared, but still, the Medium Motor does not activate. It indeed does not move at all. It is not a connection problem, because the Brickman does detects the engine. There is also no error log after running the program, suggesting that the program itself has no syntax errors, just problems of logic.
The claw code should answer some functionality:
What the program should achieve is to apply two directions of movement: one for opening the claw with a limited rotation of the engine to the right for about 720-980 degrees and a reverse rotation to close the claw.
The reverse rotation to close the claw should have a maximal reversed range of the opening rotation, but it is not necessarily complete, since the claw could grab an object of variable size.
Stopping the claw closing motion may occur either by depressing the Right Stick, or any other button was used to open the claw, or perhaps auto-stop when the motor stall or overload is reached when the claw has grabbed the object.
A new interesting development: I changed the run command from "direct" to "forever"
self.claw_motor.run_direct(speed_sp=dc_clamp(grab_speed))
changed to
self.claw_motor.run_forever(speed_sp=dc_clamp(grab_speed))
for mapping R2 and L2 buttons got activated with those lines
map the grab claw/MediumMotor
if event.type == 3:
if event.code == 2:
grab_speed = -scale_stick(event.value)
if grab_speed < 100 and grab_speed > -100:
grab_speed = 0
I was thrilled to see that the Medium Motor works both directions now.
However, the functionality is not completely what it should be.
When I start the program, the engines are all waiting.
The left stick activates the drivetrain as expected. Leaving the stick in rest position stops the drivetrain, as expected.
The claw is now activated with R2 and L2, although the codes seem wrong.
Pressing the R2 (should be code 311) activates the right turning of the engine closing the claw, and it does not stop when the button is not pressed anymore. The claw gets ripped apart if not stopped.
Pressing L2 (should be code 309) activates turning in reverse and overrides the R2. However, as soon as I take the finger off the L2 the claw goes back to turning without stop in the other direction (closing the claw again without stop)
The event.code == 2 seems to activate both L2 and R2
One direction works as expected, the other direction has no stopping when the button is no more pressed.
I guess I have to include a stopping routine to the functionality of R2.
So I modified the claw mapping again:
map the grab claw/MediumMotor
if event.type == 3:
if event.code == 2:
grab_speed = scale_stick(event.value)
if grab_speed < 100 and -grab_speed > -100:
grab_speed = 0
if event.type == 3:
if event.code == 4:
grab_speed = -scale_stick(event.value)
if grab_speed < 100 and grab_speed > -100:
grab_speed = 0
This resulted in an almost perfect functionality.
Now the Right Stick closes and opens the claw.
Moving the RStick Forward (Positive Y) closes the claw (reversed polarity rotate MediumMotor).
Moving the RStick Backward (Negative Y) opens the claw
Both movements stop when the RStick is in middle position, as expected.
However, there are two problems: the L2 button still opens the claw although it should not.
The Backward (negY) RStick does not work smoothly. It needs slow small movements to activate. If pulled all the way, the stick does not work.
It needs slow handling.
Perhaps the code from the L2 interferes with the RStick
Thanks for the help. It got me unstuck.
Here it is:
https://github.com/usebast/ev3Dev_py_apps
well the joysticks are both working, you can see the robot running here:
[https://youtu.be/tAn0mSNIIuc]
still, the errors persist. the right joystick is sluggish especially when closing. and the R1 and L1 should not trigger the claw, according to the codes, but they do.
where should I enter the "println("grab_speed") line? after line 50 or later where the controller code is around line 84?
How to detect if the d-pad buttons (up, down, left, right) are pressed? They don't seem to register when event.type == 1 like the rest of the buttons. Does anyone know if there is a reference somewhere that maps out all the event types, codes and values? I would like to find out too if the trackpad can also be used. Thanks!
Figured out that connecting the PS4 controller registers as 3 input devices. Using this code:
devices = [evdev.InputDevice(fn) for fn in evdev.list_devices()] for device in devices: print(device.name)
It will output:
Wireless Controller
Wireless Controller Motion Sensors
Wireless Controller Touchpad
So the first is the gamepad (the buttons and the joysticks), the second is the accelerometer, and the third is the touchpad (touch and click).
What I haven't figured out still is how to capture the events for when the D-pad buttons (up,down,left,right) are pressed. They seem to be not registering from any of these three inputs of the PS4 controller connected with Bluetooth to the EV3. Anyone figured this out yet?
@Quantum357 if you can already confirm, let us know what is the exact event type, code, and value for the d-pad buttons. thanks!
Sorry for a dumb question but, I am using your code with the ev3dev os with a Pistorms-V2 module. I have everything set up correctly and can run the OS and I have paired the ps4 controller successfully but I am not getting any response when I run your program. Any suggestions? The error log is below if that is helpful.
_**Exception in thread Thread-1:
Traceback (most recent call last):
File "/usr/lib/python3.5/threading.py", line 914, in _bootstrap_inner
self.run()
File "/home/robot/ps4explor3r.py", line 42, in run
self.right_motor.run_forever(speed_sp=dc_clamp(forward_speed+side_speed))
File "/usr/lib/python3/dist-packages/ev3dev/core.py", line 799, in run_forever
setattr(self, key, kwargs[key])
File "/usr/lib/python3/dist-packages/ev3dev/core.py", line 589, in speed_sp
self._speed_sp = self.set_attr_int(self._speed_sp, 'speed_sp', value)
File "/usr/lib/python3/dist-packages/ev3dev/core.py", line 223, in set_attr_int
return self._set_attribute(attribute, name, str(int(value)))
File "/usr/lib/python3/dist-packages/ev3dev/core.py", line 216, in _set_attribute
raise Exception("%s is not connected" % self)
Exception: LargeMotor(outC) is not connected
Traceback (most recent call last):
File "/home/robot/ps4explor3r.py", line 51, in
for event in gamepad.read_loop(): #this loops infinitely
File "/usr/lib/python3/dist-packages/evdev/device.py", line 254, in read_loop
r, w, x = select([self.fd], [], [])
KeyboardInterrupt**_
There's a little problem in the stopping code. 305 is the O button, not the X button.
Is there a micropython version of this code? I wanted to the ps4 controller for a robot competition and waiting 20 whole seconds is not great. Can anyone help? (it will be great if i can control the motor separately with each joystick on my ps4)
If I run your code I get motor mis-match errors. So I changed the two MediumMotor to LargeMotor and the single LargeMotor to MediumMotor and also changed which inputs they are connected to -- the code runs. The only problem is that both the left (both horizontal and vertical) and right (horizontal works while the vertical is not very fast and only after the horizontal is used) joysticks work the drive motors. Also, the steering motor is a bit sluggish. Below is the alteration I made to your code.
I also changed the PS3dev to PS4dev for clarity.
Mike,