Raspberry Pi Forums

Code: Select all

int SendPacket(unsigned char  * Packet, int count)
{
if(!emulator)
{
        write(Handle,Packet,count);
//        printf("%s\n",Packet);fflush(stdout);
}
        return(1);
}


void SendMotor(BYTE command,BYTE value)
{
        BYTE controller=131;
        BYTE buffer[8];
        int loop;
        int sum=0;
        buffer[0]=controller;
        buffer[1]= command & 0x7f;
        buffer[2]= value & 0x7f;
        for( loop=0;loop<3;loop++)
                sum+=buffer[loop];
        buffer[3]=  sum & 0x7f;

  SendPacket(buffer,4);usleep(50000);
  printf("SendMotor(command=%d,value=%d)\n",command,value);
  fflush(stdout);       
}

Code: Select all

import RPi.GPIO as GPIO
import time


PIN_MOTOR1=12 #Moteur droite S1
PIN_MOTOR2=19 #Moteur gauche S2
try:
    #configure les pins
    GPIO.setmode(GPIO.BCM) 
    GPIO.setwarnings(False)
    GPIO.setup(PIN_MOTOR1, GPIO.OUT)
    GPIO.setup(PIN_MOTOR2, GPIO.OUT)
    motor1=GPIO.PWM(PIN_MOTOR1,1000)
    motor2=GPIO.PWM(PIN_MOTOR2,1000)
    #Avance
    motor1.start(75)
    motor2.start(75)
    #GPIO.output(PIN_MOTOR1, GPIO.HIGH)
    time.sleep(3) #Continue
    
    #Fais reculer
    motor1.start(25)
    motor2.start(25)
    #GPIO.output(PIN_MOTOR1,GPIO.LOW)
    time.sleep(5)
    #Freine et stoppe le moteur
    motor1.stop()
    motor2.stop()
except KeyboardInterrupt :
    pass
except :
    GPIO.cleanup()
    raise
GPIO.cleanup() 


    
    
    

Code: Select all

sudo pip3 install pysabertooth

Code: Select all

from pysabertooth import Sabertooth
moteurAB = Sabertooth("/dev/serial0",baudrate=9600,address=128)
moteur.drive(1,50)

Code: Select all

# Released by rdb under the Unlicense (unlicense.org)
# Based on information from:
# https://www.kernel.org/doc/Documentation/input/joystick-api.txt

import os, struct, array
from fcntl import ioctl

# Iterate over the joystick devices.
print('Available devices:')

for fn in os.listdir('/dev/input'):
    if fn.startswith('js'):
        print('  /dev/input/%s' % (fn))

# We'll store the states here.
axis_states = {}
button_states = {}

# These constants were borrowed from linux/input.h
axis_names = {
    0x00 : 'x',
    0x01 : 'y',
    0x02 : 'z',
    0x03 : 'rx',
    0x04 : 'ry',
    0x05 : 'rz',
    0x06 : 'trottle',
    0x07 : 'rudder',
    0x08 : 'wheel',
    0x09 : 'gas',
    0x0a : 'brake',
    0x10 : 'hat0x',
    0x11 : 'hat0y',
    0x12 : 'hat1x',
    0x13 : 'hat1y',
    0x14 : 'hat2x',
    0x15 : 'hat2y',
    0x16 : 'hat3x',
    0x17 : 'hat3y',
    0x18 : 'pressure',
    0x19 : 'distance',
    0x1a : 'tilt_x',
    0x1b : 'tilt_y',
    0x1c : 'tool_width',
    0x20 : 'volume',
    0x28 : 'misc',
}

button_names = {
    0x120 : 'trigger',
    0x121 : 'thumb',
    0x122 : 'thumb2',
    0x123 : 'top',
    0x124 : 'top2',
    0x125 : 'pinkie',
    0x126 : 'base',
    0x127 : 'base2',
    0x128 : 'base3',
    0x129 : 'base4',
    0x12a : 'base5',
    0x12b : 'base6',
    0x12f : 'dead',
    0x130 : 'a',
    0x131 : 'b',
    0x132 : 'c',
    0x133 : 'x',
    0x134 : 'y',
    0x135 : 'z',
    0x136 : 'tl',
    0x137 : 'tr',
    0x138 : 'tl2',
    0x139 : 'tr2',
    0x13a : 'select',
    0x13b : 'start',
    0x13c : 'mode',
    0x13d : 'thumbl',
    0x13e : 'thumbr',

    0x220 : 'dpad_up',
    0x221 : 'dpad_down',
    0x222 : 'dpad_left',
    0x223 : 'dpad_right',

    # XBox 360 controller uses these codes.
    0x2c0 : 'dpad_left',
    0x2c1 : 'dpad_right',
    0x2c2 : 'dpad_up',
    0x2c3 : 'dpad_down',
}

axis_map = []
button_map = []
 
# Open the joystick device.
fn = '/dev/input/js0'
print('Opening %s...' % fn)
jsdev = open(fn, 'rb')

# Get the device name.
#buf = bytearray(63)
#buf = array.array(b'c', [b'\0'] * 64)
#ioctl(jsdev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len)
#js_name = buf.tostring()
buf=b'\0'*64
buf = ioctl(jsdev, 0x80006a13 + (0x10000 * len(buf)), buf) # JSIOCGNAME(len)
js_name = buf.decode("utf-8").split('\0')[0]
print('Device name: %s' % js_name)

# Get number of axes and buttons.
buf = b'\0'
#buf = array.array('B', [0])
#ioctl(jsdev, 0x80016a11, buf) # JSIOCGAXES
buf=ioctl(jsdev, 0x80016a11, buf) # JSIOCGAXES

num_axes = struct.unpack('B',buf)[0]
buf = b'\0'
buf= ioctl(jsdev, 0x80016a12, buf) # JSIOCGBUTTONS
num_buttons = struct.unpack('B',buf)[0]

#print("num_axes ",num_axes)
#print("num_buttons ",num_buttons)

#buf = array.array('B', [0])
#ioctl(jsdev, 0x80016a12, buf) # JSIOCGBUTTONS
#num_buttons = buf[0]

# Get the axis map.
buf = b'\0'*64

#buf = array.array('B', [0] * 0x40)
#ioctl(jsdev, 0x80406a32, buf) # JSIOCGAXMAP
buf = ioctl(jsdev, 0x80406a32, buf) # JSIOCGAXMAP

for axis in buf[:num_axes]:
    axis_name = axis_names.get(axis, 'unknown(0x%02x)' % axis)
    axis_map.append(axis_name)
    axis_states[axis_name] = 0.0

# Get the button map.
buf = b'\0' * 400
#buf = array.array('H', [0] * 200)
buf = ioctl(jsdev, 0x80406a34, buf) # JSIOCGBTNMAP
buf = struct.unpack('H'*(len(buf)//2),buf)

for btn in buf[:num_buttons]:
    btn_name = button_names.get(btn, 'unknown(0x%03x)' % btn)
    button_map.append(btn_name)
    button_states[btn_name] = 0

print ('%d axes found: %s' % (num_axes, ', '.join(axis_map)))
print ('%d buttons found: %s' % (num_buttons, ', '.join(button_map)))



# Main event loop
while True:
    evbuf = jsdev.read(8)
    print(evbuf)
    if evbuf:
        time, value, type, number = struct.unpack('IhBB', evbuf)

        if type & 0x80:
             print( "(initial)"),

        if type & 0x01:
            button = button_map[number]
            if button:
                button_states[button] = value
                if value:
                    print( "%s pressed" % (button))
                else:
                    print("%s released" % (button))

        if type & 0x02:
            axis = axis_map[number]
            if axis:
                fvalue = value / 32767.0
                axis_states[axis] = fvalue
                print( "%s: %.3f" % (axis, fvalue))

Code: Select all

#!usr/bin/env python
#*coding:utf-8*

from pysabertooth2 import Sabertooth
import serial
import time

saber = Sabertooth('/dev/ttyS0', baudrate=9600, address=130, timeout=0.1)

# drive(number, speed)
#number : 1-2
#speed: -100 à +100

saber.drive(1,50) 
saber.drive(2,50)
time.sleep(3)

saber.drive(1,-50) 
saber.drive(2,-50)
time.sleep(3) 

#avance pendant 5 secondes
#saber.driveBoth(50,50)
#time.sleep(3)

saber.stop()
saber.close()

Code: Select all

saber = Sabertooth('/dev/ttyS0', baudrate=9600, address=130, timeout=0.1)
time.sleep(0.1)
saber.drive(1,50)
...