RaspberryPi_Sabertooth_test

SabertoothTest.py

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