# FINAL SUMO ROBOT CODE # --------------------------------------------------------------------------------------------------------------------- # IMPORTS # --------------------------------------------------------------------------------------------------------------------- from gpiozero import Button, DigitalOutputDevice, DistanceSensor, PWMOutputDevice, LED from evdev import InputDevice, categorize, ecodes from time import sleep, time # --------------------------------------------------------------------------------------------------------------------- # INITIALIZATION # --------------------------------------------------------------------------------------------------------------------- # COLOR SENSOR INIT pinColorSel0 = 10 # output pin to select the scaling pinColorSel1 = 9 # output pin to select the scaling pinColorSel2 = 25 # output pin to select the color filter pinColorSel3 = 26 # output pin to select the color filter pinColorSig = 21 # input pin for reading length of pulse colorSig = Button(pinColorSig, pull_up=True, hold_time=.001, hold_repeat=True) colorSel0 = DigitalOutputDevice(pinColorSel0) colorSel1 = DigitalOutputDevice(pinColorSel1) colorSel2 = DigitalOutputDevice(pinColorSel2) colorSel3 = DigitalOutputDevice(pinColorSel3) colorSel0.off() # Selects 2% scale colorSel1.on() # Selects 2% scale colorSel2.on() # Selects NO FILTER colorSel3.off() # Selects NO FILTER colorWhiteH = 10 # Set the upper bound of the color white colorCounter = 0 # Set the initial white count to 0 # DISTANCE SENSOR INIT pinDistEcho = 27 # Echo pin for distance sensor pinDistTrig = 22 # Trigger pin for distance sensor distSensor = DistanceSensor(echo=pinDistEcho, trigger=pinDistTrig) distThresh = .72 # The threshold distance to sense an object # MOTOR INIT pinMotorLf = 24 # Set GPIO Pin that the forward input of the left motor is connected to pinMotorLb = 23 # Set GPIO Pin that the backward input of the left motor is connected to pinMotorRf = 11 # Set GPIO Pin that the forward input of the right motor is connected to pinMotorRb = 5 # Set GPIO Pin that the backward input of the right motor is connected to pinMotorRen = 13 # Set GPIO Pin that the enable input of the right motor is connected to (PWM) pinMotorLen = 12 # Set GPIO Pin that the enable input of the left motor is connected to (PWM) motorLen = PWMOutputDevice(pinMotorLen) motorRen = PWMOutputDevice(pinMotorRen) motorLf = DigitalOutputDevice(pinMotorLf) motorLb = DigitalOutputDevice(pinMotorLb) motorRf = DigitalOutputDevice(pinMotorRf) motorRb = DigitalOutputDevice(pinMotorRb) pinDistLED = 8 # Set GPIO Pin that the distance debug LED is connected to pinColorLED = 7 # Set GPIO Pin that the color debug LED is connected to pinAutoLED = 20 # Set GPIO Pin that the auto/manual debug LED is connected to distLED = LED(pinDistLED) colorLED = LED(pinColorLED) autoLED = LED(pinAutoLED) # TIMER INIT time0 = time() # Creates a timer with the start being when the program starts # GAMEPAD INIT stkLX = 0 # Left joystick X-direction code stkLY = 1 # Left joystick Y-direction code btnTri = 307 # Triangle button code gamepad = InputDevice('/dev/input/event2') # Initialize gamepad tmpStickY = 127 # Initialize the temporary speed variable tmpStickX = 127 # Initialize the temporary curve variable auto = False # Initializes the auto/manual flag to auto # --------------------------------------------------------------------------------------------------------------------- # FUNCTION DEFS # --------------------------------------------------------------------------------------------------------------------- def set_auto(): global gamepad, auto, autoLED # Allows the function to access the gamepad and auto flag event = gamepad.read_one() # Reads an input from the gamepad if event is not None \ and event.type == ecodes.EV_KEY \ and event.code == btnTri \ and event.value == 1: # Checks if the Triangle button is pressed... auto = not auto # Toggles the auto/manual flag autoLED.toggle() def stick_to_dec(stick): # Takes in a stick value (0 -- 255) return float((float(stick) - 127.0) / 128.0) # Puts out a ratio of the input between -1 and 1 def read_gamepad(stick_x, stick_y): speed = -stick_to_dec(stick_y) # Perform the ratio calculation curve = stick_to_dec(stick_x) # Perform the ratio calculation move(speed, curve) # Use the values to set the motor values def move(speed, curve): global motorLen, motorLf, motorLb, motorRen, motorRf, motorRb # Allows the function to change the motor values if speed > 0: # If speed is positive... motorLf.on() # Left motor forward motorLb.off() # Left motor forward motorRf.on() # Right motor forward motorRb.off() # Right motor forward elif speed < 0: # If speed is negative... motorLf.off() # Left motor backward motorLb.on() # Left motor backward motorRf.off() # Right motor backward motorRb.on() # Right motor backward else: # If speed is 0... motorRen.value = 0 # Stop the motors motorLen.value = 0 # Stop the motors if curve >= 0: # If curve is to the right... motorRen.value = abs(speed) * (1 - abs(curve)) # Right motor slows by curve value to turn right motorLen.value = abs(speed) # Left motor goes at speed elif curve < 0: # If curve is to the left... motorRen.value = abs(speed) # Right motor goes at speed motorLen.value = abs(speed) * (1 - abs(curve)) # Left motor slows by curve value to turn left # --------------------------------------------------------------------------------------------------------------------- # MAIN LOOP # --------------------------------------------------------------------------------------------------------------------- while True: if auto: if distSensor.distance <= distThresh: # If there is an object within the threshold distance... distLED.on() # Turn on the distance debug LED move(.6, .4) # Move towards it else: # Otherwise... distLED.off() # Turn off the distance debug LED move(.3, -1) # Scan the ring by turning left # COLOR SENSOR colorLen = 0 # Reset the wavelength to 0 colorSig.wait_for_active() # Wait for the sensor to start sending a signal while colorSig.is_pressed: # Loop iterates the wavelength as long as the signal is active colorLen = colorLen + 1 if time() - time0 > 1.5: # Checks if it has been 1.5 seconds since the previous time time0 = time() # Sets the time to the current time colorCounter = 0 # Resets the color counter to remove false positives of white if colorLen < colorWhiteH: # If it senses white... colorLED.on() # Turn on the color debug LED colorCounter = colorCounter + 1 # Increments the color counter for how many times it sees white if colorCounter > 18: # If it receives enough white signals it will... colorCounter = 0 # Reset the counter move(-1, 0) # Move backwards... sleep(1) # for a second move(1, -1) # Turn left to avoid the edge again... sleep(1) # for a second time0 = time() # Sets the timer for resetting the counter to the current time else: # If no white is detected... colorLED.off() # Turn off the color debug LED set_auto() # Check if Triangle is pressed and change the auto flag if needed else: tmpStickY = gamepad.absinfo(stkLY).value # Take in the current value of the left joystick (Y-direction) tmpStickX = gamepad.absinfo(stkLX).value # Take in the current value of the left joystick (X-direction) read_gamepad(tmpStickX, tmpStickY) # Use the values to change the motor values set_auto() # Check if Triangle is pressed and change the auto flag if needed