Copyright 2018 - Capnetix, LLC

Fast forward to first working PiBot

Well, all went well with soldering up the MotorHAT.  In fact, we downloaded the software from Adafruit, fired it up in Python, and went through a sequence of first moving a single motor, then moving two motors just using SSH and Python locally on the Pi, with the device on the bench.

 


What I needed to do next was find a python program that would initialize a networktables server, and listen for a connection from a client, and print out the values, so I could see what was happening at the driverstation.

 

I started with just a listener, but here is what I'm running now.  This code listens to any changes in network tables, and then filters out anything that has to do with the forward/backward axes on the two joysticks.

This is NOT production code BTW.   It's a hack to get things to work.

#!/usr/bin/env python3
#
# This is a NetworkTables client (eg, the DriverStation/coprocessor side).
# You need to tell it the IP address of the NetworkTables server (the
# robot or simulator).
#
# This shows how to use a listener to listen for changes in NetworkTables
# values. This will print out any changes detected on the SmartDashboard
# table.
#

# Make this a server/listener
'''

import time
from networktables import NetworkTables

# To see messages from networktables, you must setup logging
import logging

logging.basicConfig(level=logging.DEBUG)

NetworkTables.initialize()
sd = NetworkTables.getTable("SmartDashboard")

i = 0
while True:
    print('dsTime:', sd.getNumber('dsTime', 'N/A'))

    sd.putNumber('robotTime', i)
    time.sleep(1)
    i += 1

'''


import sys
import time
import atexit

from networktables import NetworkTables
from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor

# To see messages from networktables, you must setup logging
import logging
logging.basicConfig(level=logging.DEBUG)

'''if len(sys.argv) != 2:
    print("Error: specify an IP to connect to!")
    exit(0)

ip = sys.argv[1]

NetworkTables.initialize(server=ip)

'''
# create a default object, no changes to I2C address or frequency
mh = Adafruit_MotorHAT(addr=0x60)

# recommended for auto-disabling motors on shutdown!


def turnOffMotors():
    mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE)
    mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE)
    mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE)
    mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE)


atexit.register(turnOffMotors)

motorLeft = mh.getMotor(3)
motorRight = mh.getMotor(4)

motorLeftSpeed = 0                           # should be from 0-255
motorLeftDirection = Adafruit_MotorHAT.RELEASE   # Start in a forward direction

motorGain = .5				# 1.0 is full speed, needed to turn them down


def getSpeedFromAxisValue(axisvalue: float) -> int:
    if abs(axisvalue) < .001:
        axisvalue = 0.0
    return abs(int(axisvalue*motorGain*255.0))


def getDirectionFromAxisValue(axisvalue: float, reverse: bool) -> int:
    if abs(axisvalue) < .001:
        direction = Adafruit_MotorHAT.RELEASE
    elif axisvalue < 0.0:
        direction = Adafruit_MotorHAT.FORWARD
        if reverse:
            direction = Adafruit_MotorHAT.BACKWARD
    else:
        direction = Adafruit_MotorHAT.BACKWARD
        if reverse:
            direction = Adafruit_MotorHAT.FORWARD
    return direction


nt = "driver_station//joystick-0"

print("Listening to table:"+nt)
NetworkTables.initialize()
sd = NetworkTables.getTable(nt)


def valueChanged(table, key, value, isNew):
    print("valueChanged: key: '%s'; value: %s; isNew: %s" % (key, value, isNew))
    if key == "axis-3":
        print("Axis 3 changed to %f" % value)
        motorLeft.run(getDirectionFromAxisValue(value, True))
        motorLeft.setSpeed(getSpeedFromAxisValue(value))
    elif key == "axis-1":
        print("Axis 1 changed to %f" % value)
        motorRight.run(getDirectionFromAxisValue(value, True))
        motorRight.setSpeed(getSpeedFromAxisValue(value))


def connectionListener(connected, info):
    print(info, '; Connected=%s' % connected)


NetworkTables.addConnectionListener(connectionListener, immediateNotify=True)

sd = NetworkTables.getTable(nt)
sd.addEntryListener(valueChanged)

# set the speed to start, from 0 (off) to 255 (max speed)
motorLeft.setSpeed(150)
motorLeft.run(Adafruit_MotorHAT.FORWARD);
# turn off motor
motorLeft.run(Adafruit_MotorHAT.RELEASE);

motorRight.setSpeed(150)
motorRight.run(Adafruit_MotorHAT.FORWARD);
# turn off motor
motorRight.run(Adafruit_MotorHAT.RELEASE);

while True:
    time.sleep(1)
 

 

Now, I can connect the player console to the Windows laptop, and startup the pydriver station.  Run the code above on the pi, and Voila!  You can move the motors with the joysticks.  Teleoperation!

 

 

 
More in this category: « Wiring in the motor control board

Leave a comment

Make sure you enter all the required information, indicated by an asterisk (*). HTML code is not allowed.

f t g m