Last active
May 21, 2019 03:34
-
-
Save danjperron/d9ec8a32e0007db4c3c8fb3caee9b154 to your computer and use it in GitHub Desktop.
RaspberryPi_Sabertooth_test
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
from pysabertooth import Sabertooth | |
import os, struct | |
import time | |
#classe joystick | |
#classe joystick | |
class MyJoystick: | |
def __init__(self, port): | |
self.port = port | |
self.jsdev = os.open(port,os.O_RDONLY | os.O_NONBLOCK) | |
def Get(self): | |
try: | |
info = os.read(self.jsdev,8) | |
except BlockingIOError: | |
return None | |
return struct.unpack('IhBB',info) | |
#sabertooth | |
saber = Sabertooth('/dev/serial0', baudrate=9600, address=128, timeout=0.1) | |
# drive(number, speed) | |
# number: 1-2 | |
# speed: -100 - 100 | |
#initialisation moteur | |
MoteurG = 0 | |
MoteurD = 0 | |
runFlag = False | |
saber.stop() | |
def moteurs(): | |
# print("moteurs G:{} D:{} f:{}".format(MoteurG,MoteurD,runFlag)) | |
if runFlag: | |
saber.drive(1,MoteurG) | |
saber.drive(2,MoteurD) | |
else: | |
saber.drive(1,0) | |
saber.drive(2,0) | |
# creer gamepad | |
gamepad = MyJoystick('/dev/input/js0') | |
StickGauche=1 | |
StickDroite=4 | |
# Main loop | |
while True: | |
#lire info gamepad | |
info = gamepad.Get() | |
if info is None: | |
moteurs() | |
else: | |
#ok nous avons des données décrytont la structure | |
temps, valeur , type , id = info | |
#est-ce un boutton | |
if type & 0x01: | |
if id == 0: | |
#ok boutton A | |
if valeur : | |
runFlag = True | |
elif id == 0x7: | |
if valeur : | |
#print("set baudrate") | |
saber.setBaudrate(saber.saber.baudrate) | |
else : | |
#autre boutton | |
if valeur: | |
runFlag = False | |
continue | |
#est-ce un joystick | |
if type & 0x02: | |
if id == StickGauche: | |
MoteurG = -(valeur // 327) | |
if MoteurG > 100: | |
MoteurG = 100 | |
if MoteurG < (-100): | |
MoteurG = (-100) | |
continue | |
if id == StickDroite: | |
MoteurD = -(valeur // 327) | |
if MoteurD > 100: | |
MoteurD = 100 | |
if MoteurD < (-100): | |
MoteurD = (-100) | |
continue |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment