chipbot/chipbot.py at master · signalnine/chipbot · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
#!/usr/bin/env python
import time
import pygame
import CHIP_IO.GPIO as GPIO
import CHIP_IO.SOFTPWM as SPWM

# Set which GPIO pins the drive outputs are connected to
drive_0 = "GPIO6"
drive_1 = "GPIO4"
drive_2 = "GPIO2"
drive_3 = "XIO-P1"
PWM_0 = "LCD-VSYNC"
PWM_1 = "LCD-HSYNC"

# Set all of the drive pins as output pins
GPIO.setup(drive_0, GPIO.OUT)
GPIO.setup(drive_1, GPIO.OUT)
GPIO.setup(drive_2, GPIO.OUT)
GPIO.setup(drive_3, GPIO.OUT)

# SoftPWM setup
SPWM.start(PWM_0, 50)
SPWM.start(PWM_1, 50)

# Settings
leftdrive = drive_0                     # drive number for left motor
rightdrive = drive_3                    # drive number for right motor
leftback = drive_1                      # drive number for left motor, backwards
rightback = drive_2                     # drive number for right motor, backwards
axisleftmotor = 1                       # Joystick axis to read for up / down position
axisleftmotorinverted = True            # Set this to True if up and down appear to be swapped
axisrightmotor = 3                      # Joystick axis to read for left / right position
axisrightmotorinverted = True           # Set this to True if left and right appear to be swapped
interval = 0.05                         # Time between keyboard updates in seconds, smaller responds faster but uses more processor time

drive0 = rightdrive
drive1 = leftdrive
off0 = rightback
off1 = leftback
global leftmotor
global rightmotor
leftmotor = 0
rightmotor = 0

# Setup pygame
pygame.init()

# Wait for joystick to become available
joystick_count = pygame.joystick.get_count()
while joystick_count == 0:
    print "joystick disconnected, waiting.."
    pygame.joystick.quit()
    time.sleep(5)
    pygame.init()
    joystick_count = pygame.joystick.get_count()

# Initialize joystick
joystick = pygame.joystick.Joystick(0)
joystick.init()

# Set all drives to off
def motoroff():
    GPIO.output(drive_0, GPIO.LOW)
    GPIO.output(drive_1, GPIO.LOW)
    GPIO.output(drive_2, GPIO.LOW)
    GPIO.output(drive_3, GPIO.LOW)
    SPWM.set_duty_cycle(PWM_0, 0)
    SPWM.set_duty_cycle(PWM_1, 0)

# Function to handle pygame events
def pygamehandler(events):
    global leftmotor
    global rightmotor
    # Handle each event individually
    for event in events:
        if event.type == pygame.JOYAXISMOTION:
            # A joystick has been moved, read axis positions (-1 to +1)
            hadevent = True
            leftmotor = joystick.get_axis(axisleftmotor)
            rightmotor = joystick.get_axis(axisrightmotor)
            # Invert any axes which are incorrect
            if axisleftmotorinverted:
                leftmotor = -leftmotor
            if axisrightmotorinverted:
                rightmotor = -rightmotor
    return leftmotor, rightmotor

try:
    print 'Press ctrl+c to quit'
    # Loop indefinitely
    while True:
        leftmotor, rightmotor = pygamehandler(pygame.event.get())
        if rightmotor > 0:
            drive0 = rightdrive
            off0 = rightback
        if rightmotor < 0:
            drive0 = rightback
            off0 = rightdrive
        if leftmotor > 0:
            drive1 = leftdrive
            off1 = leftback
        if leftmotor < 0:
            drive1 = leftback
            off1 = leftdrive
        GPIO.output(drive0, GPIO.HIGH)
        GPIO.output(drive1, GPIO.HIGH)
        GPIO.output(off0, GPIO.LOW)
        GPIO.output(off1, GPIO.LOW)
        SPWM.set_duty_cycle(PWM_0, abs(leftmotor*100))
        SPWM.set_duty_cycle(PWM_1, abs(rightmotor*100))
        time.sleep(interval)
    motoroff()

except KeyboardInterrupt:
    # CTRL+C exit, disable all drives
    motoroff()
    SPWM.cleanup()
    GPIO.cleanup()