**NXT Automation Alternatives & Practical Implementations**
\\
**Author: **Hashim H., Email: hameeh1@unlv.nevada.edu
\\
**Date: **Last modified on [11/23/24]
\\
**Keywords: **NXT, Teleoperation, Automation
**Motivation and Audience**
This page serves to document the practical applications of NXT automation as outlined by the [[nxt_automation_implementation|"NXT Automation Implementation"]] tutorial. This includes alternative means of communication which would not be primarily utilized for the original tutorial. Though certain avenues remain incomplete, the foundation found herein may be assistive in furthering such or even be applied to unrelated projects. This information assumes:
*BricxCC (Bricx Command Center) application setup
*A basic understanding of C+ + (or general object oriented programming)
*Familiarity with [[https://opencv.org/|OpenCV]]
**Parts List and Sources**
\\
*LEGO NXT
*[[http://www.mindsensors.com/ev3-and-nxt/29-sony-playstation-2-controller-interface-for-nxt-or-ev3|Sony Playstation 2 Controller Interface for NXT or EV3]]
*[[https://www.brookaccessory.com/products/wingmanps2/index.html|Brook Wingman PS2]]
*[[https://www.amazon.com/Logitech-Wireless-Nano-Receiver-Controller-Vibration/dp/B0041RR0TW?crid=3CA24GQASRV0V&dib=eyJ2IjoiMSJ9.Dpo5bvFXpJM3oQEDxlrPqmP4TRDfE4f8jZYDWWs0-zfZulhTOqzvztLb4u8wiH0X2r79i_fAdbJJ-opjjFkpMtZRn3qE7NZqZnjjiRi0fwl2wtIU_GJQY-lK4HgTHY4mfNYxzU-dBCF_oezbP9Mxws493giWpE0urfRldViaHgJXUCIHs3cmi9vqJ5Nh5iNuHJY29zX02arsvn9J8lZ_IkWjwVKOXrOLA_Y3buJ804s.3u1z3FfpARLiWgLPpZ2KYgU6wYiFBY9DuZRkZyNyv9o&dib_tag=se&keywords=logitech%2Bf710&qid=1732413989&sprefix=logitech%2Bf%2Caps%2C144&sr=8-1&th=1|Logitech F710]]
*[[https://www.amazon.com/AITRIP-ESP-WROOM-32-Development-Microcontroller-Integrated/dp/B0DF2YJSHN?dib=eyJ2IjoiMSJ9.J8fl2PuZsBFTQRqqz9O9mEmyW62U4_MpGp5foNJRgelWypISvNbbm65DtPAI9DzhViCB-x3lbNUDgupbZhCbxSDVwBBMwtxW_bgXqq9RyaIYGRk49OAKmhrFw_OKJdupc5G0u4I1gW7rkdlTuLLVACDXdiQFeOYfhHiV8AOgwIMv8TQfESYUxBC8714aMXMEUBk-H-kpkaVbEzudR8qffCO6tua6TvQSKq7ggfZuV88.U3LiQYOUYERI_is6ZEJiMS6iwDWPcd1npboG-NrB1Io&dib_tag=se&keywords=esp32%2Busbc&qid=1732415679&sr=8-3&th=1|ESP32 USB-C]]
*[[https://www.amazon.com/Arduino-A000066-ARDUINO-UNO-R3/dp/B008GRTSV6?th=1|Arduino Uno]]
*[[https://www.pjrc.com/store/teensy.html|Teensy 2.0]]
**Software**
\\
*[[https://www.arduino.cc/en/software|Arduino IDE]]
* [[https://pypi.org/project/pyserial/|Pyserial]]
* [[https://www.pjrc.com/teensy/loader.html|Teensy Loader]]
* [[http://sourceforge.net/projects/bricxcc/files/bricxcc|BricxCC]]
* (If needed) [[https://www.daslhub.org/unlv/courses/me425-625/lesson-B-simpleMachines1-leversShaftsAndCranks/lab/NXT_USB_Driver_120.zip|NXT USB Driver]]
* (If needed) [[https://www.daslhub.org/unlv/courses/me425-625/lesson-B-simpleMachines1-leversShaftsAndCranks/lab/lms_arm_nbcnxc_131.rfw|NXC Firmware]]
**Arduino-I2C Alternative**
The I2C communication protocol is the means through which LEGO NXT peripherals are able to communicate with the module, meaning that responses may be programmed in correspondence with particular inputs from a microcontroller. An arduino uno has been used for this example.
**Setup**
**Step 1 [Theory/Reading]**
Prior to implementation, it is important to understand the means through which the I2C protocol operates. The following documents will be greatly assistive outside of the scope of this tutorial:
*[[https://www.daslhub.org/unlv/courses/me425-625/lesson-K-robotCommunications/lab/labLegoCommunications092616.pdf|PDF tutorial]]
*[[https://engmuhannadalkhudari.wordpress.com/2016/02/11/nxtev3-arduino-i2c-ultimate-guide/|"Ultimate Guide"]]
I2C is a communication protocol combining feature of SPI and UARTs. Multiple slaves are able to connect to a master unit, or even multiple masters. Serial Data (SDA) and Serial Clock (SCL) are lines for data transmission and the clock signal respectively, with bit output being synchronized to the sampling of bits by the clock which is controlled by the master.
I2C uses "messages" broken up into frames:
*Starting condition (SDA switches from high to low voltage before SCL switches from high to low)
*Address frame (slave's binary address [7-10 bits])
*Data frame(s) with acknowledgement sent in between
*Stopping condition (Inversion of starting [low to high])
{{::introduction-to-i2c-message-frame.png?400|}}
Acknowledge bits are returned to the sender from the receiving device if an address or data frame is successfully received.
The read/write bit at the end of the address frame determines whether data is being sent to or received from the master.
**Step 2 [Hardware Setup]**
In order to communicate via I2C, a standard NXT connector wire may be stripped down to the individual wires it is comprised of, with each of the wires being correspondent to a particular functionality as detailed in the reading provided. Solder on connector pins to the utilized wires, after which point the NXT may be plugged directly from port 1 into the arduino uno.
{{::nxt_uno_wiring.png?400|}}
Alternatives exist in the form of breakout boards and shields, though the latter may be limited to a certain number of motors in some cases and will requires research and implementation outside the scope of this tutorial.
*[[https://store.wayneandlayne.com/products/bricktronics-breakout-board.html|Breakout]]
*[[https://forum.arduino.cc/t/nxt-motor-shield/64710|Community Made Shield]]
*[[https://store.wayneandlayne.com/products/bricktronics-shield-kit.html|Bricktronics]]
**Programming**
The most consistent means of I2C communication encountered due to both ease of abstraction and time dedicated to troubleshooting utilizes the RobotC IDE rather than BricxCC. It is certainly possible and indeed recommended to utilize the latter considering the limited license of the former, though code for RobotC is documented to provide guidance:
#pragma config(Sensor, S1,TIR,sensorI2CCustom)
// Define the Arduino Address
// Address is 0x0A on the Arduino: (Binary) 1010
// Bit shifted out with one 0: (Binary) 10100 = 0x14
#define ARDUINO_ADDRESS 0x14
#define ARDUINO_PORT S1
ubyte I2Cmessage[22];
char I2Creply[20];
void i2c_read_registers_text(ubyte register_2_read, int message_size, int return_size){
memset(I2Creply, 0, sizeof(I2Creply));
message_size = message_size+3;
I2Cmessage[0] = message_size; // Messsage Size
I2Cmessage[1] = ARDUINO_ADDRESS;
I2Cmessage[2] = register_2_read; // Register
sendI2CMsg(S1, &I2Cmessage[0], return_size);
wait1Msec(20);
readI2CReply(ARDUINO_PORT, &I2Creply[0], return_size);
int i = 0;
while(true){
writeDebugStream("%c", I2Creply[i]);
i++;
if(I2Creply[i] == 0) break;
}
writeDebugStreamLine(" ");
}
task main()
{
while(true){
// i2c_write_registers(0x01, 0x01, 0x00, 0x0A, 0, 0, 0);
i2c_read_registers_text(0x01, 0, 10);
motor[motorB] = I2Creply[0];
i2c_read_registers_text(0x02, 0, 10);
motor[motorC] = -I2Creply[0];
}
}
**Teensy-PS2 Alternative [*Dubious Functionality]**
This alternative sees direct connection of a PS2 receiver module to a Teensy microcontroller so as to send signals corresponding to particular inputs. It must be noted that this alternative in its current state has not been followed through on completely, but is documented considering its potential to function if a greater deal deal of time dedicated.
**Setup**
**Step 1 [Theory/Reading]**
Prior to any form of implementation, it is best to understand the Playstation2's controller communication protocol to some extent. The following webpages document the protocol (mostly reception rather than transmission) and practical examples of utilization therein:
*[[https://store.curiousinventor.com/guides/PS2/|PS2 Protocol Reception Documentation]]
*[[https://gamesx.com/controldata/psxcont/psxcont.htm|IC Protocol Emulation]]
*[[https://www.ps2-home.com/forum/viewtopic.php?f=52&t=135&p=101&hilit=Teensy#p101|More Recent Example]]
*[[https://procrastineering.blogspot.com/2010/12/simulated-ps2-controller-for.html|Older Example]]
**Step 2 [Hardware Setup]**
As detailed in the practical examples listed, a Playstation2 wired controller (in this case wireless receiver) may be stripped and its wires soldered particular pins on the Teensy 2.0 microcontroller. It is important to note that there are instances wherein aftermarket controllers may not hold the same colors as listed in either tutorial, and so connections must be checked relative to the pinouts contained within the given tutorials.
**Step 3 [Teensy Setup]**
Following soldering, the Teensy 2.0 must be connected to compute unit with the [[https://www.pjrc.com/teensy/loader.html|Teensy Loader]] installed in order to load programs onto the microcontroller. It is recommended that you start off with an existing hex file from either of the practical examples listed to troubleshoot and verify connections. If at no point an attached motor spins on a properly powered NXT with correspondent programming loaded, it may be best to use a multimeter to test connectivity or double check every wire is connected to the correct PS2 pinout.
{{::teensy_wiring.jpg?400|}}{{::reciever_teensy_wiring.png?400|}}
**Programming**
Though __it is recommended that you attempt to work of the files from the existing projects listed above__, the following is a code example able to spin a motor at a consistent rate, but cannot be updated in real time, likely due to timing issues:
*{{ ::teensycomms.7z | Custom Teensy Communication}}
**Additional Scripts**
Following is a python script intended to inform motor speed in a manner built off a more primitive finite case version of the [[nxt_automation_implementation|"NXT Automation Implementation"]] tutorial using visual data with the [[https://opencv.org/|OpenCV]] computer vision library:
#import necessary packages
import numpy as np
import cv2
import serial
import time
#set constant variables
ball_detected = False
video = cv2.VideoCapture(1)
width = 960
height = 540
num = "3"
positionX = 0
#intialize microcontroller serial communication
arduino = serial.Serial(port='COM12', baudrate=115200, timeout=.1)
#function defining serial communication
def write_read(x):
arduino.write(bytes(x, 'utf-8'))
time.sleep(0.05)
data = arduino.readline()
return data
#program loop
while True:
(grabbed, frame) = video.read()
if not grabbed:
break
frame = cv2.resize(frame, (width, height))
#apply blur to average local values and use HSV for color detection [red]
blur = cv2.GaussianBlur(frame, (21, 21), 0)
hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)
lower = [149, 163, 150]
upper = [255, 255, 255]
lower = np.array(lower, dtype="uint8")
upper = np.array(upper, dtype="uint8")
mask = cv2.inRange(hsv, lower, upper)
#outline detected color/object with circular contour
contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
detections = []
for cnt in contours:
area = cv2.contourArea(cnt)
if area > 100:
(x, y), radius = cv2.minEnclosingCircle(cnt)
center = (int(x), int(y))
radius = int(radius)
positionX = x
cv2.circle(frame, center, radius, (0,255,0), 2)
#create visual mask and count number of pixels in mask
output = cv2.bitwise_and(frame, hsv, mask=mask)
no_red = cv2.countNonZero(mask)
print(no_red)
#send serial messages based on number of pixels detected
if int(no_red) > 1000:
ball_detected = True
else:
ball_detected = False
cv2.imshow("output", output)
if ball_detected:
if positionX < (width/2)-40:
num = "3"
elif positionX > (width/2)+40:
num = "4"
else:
if int(no_red) < 45250:
num = "1"
else:
num = "2"
write_read(num)
time.sleep(1)
num = "5"
write_read(num)
else:
value = write_read("3")
#close program
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
video.release()
Motion capture data would be the primary means of informing motor speed in practice of the project's development. The following is a ROSPY based script utilizing motion capture data alongside a PID controller to hone a bearing angle relative to a ball and move toward the ball in question when this bearing angle is matched:
#!/usr/bin/env python
#importing necessary classes
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped
import serial
import time
import math
from scipy.interpolate import interp1d
#instantiate both dynamic and constant variables
roll = pitch = yaw = 0.0
Pi = math.pi
radToDegMult = 180/Pi
xTransformCar = 0.0
yTransformCar = 0.0
xTransformBall = 0.0
yTransformBall = 0.0
angleRelativeDeg = 0.0
speed = 0
angleRangeConst = 3.2
mapConst = 1.2
dt = 0
lowBound = 15
highBound = 60
newLine = "/n"
#initialize serial communication with microcontroller
arduino = serial.Serial(port='/dev/ttyUSB0', baudrate=115200, timeout=.1)
#instantiate publishers for both "car" (mobile robot) and ball
carPub = rospy.Publisher('car_transform_data', TransformStamped, queue_size = 10)
ballPub = rospy.Publisher('ball_transform_data', TransformStamped, queue_size = 10)
#PID class definition
class PID:
def __init__(self, Kp, Ki, Kd):
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.last_error = 0
self.integral = 0
def update(self, error, dt):
derivative = (error - self.last_error) / dt
self.integral += error * dt
output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
self.last_error = error
return output
#Function definitions
#Motion capture angle output is in quaternions, translated to euler angles for ease of use
#Ensure understanding of gimbal lock if you desire to change this conversion
def euler_from_quaternion(x, y, z, w):
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll_x = math.atan2(t0, t1)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch_y = math.asin(t2)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
yaw_z = math.atan2(t3, t4)
return roll_x, pitch_y, yaw_z # in radians
#project a range of values onto another for ease of use in serial communication
def mapValue(value, leftMin, leftMax, rightMin, rightMax):
# Figure out how 'wide' each range is
leftSpan = leftMax - leftMin
rightSpan = rightMax - rightMin
# Convert the left range into a 0-1 range (float)
valueScaled = float(value - leftMin) / float(leftSpan)
# Convert the 0-1 range into a value in the right range.
return rightMin + (valueScaled * rightSpan)
#serial communication function
def write_read(x):
input = str(x)
arduino.write(bytes(input, 'utf-8'))
#time.sleep(0.005)
data = arduino.readline()
return data
#callback instantiation for publishers
def carCb (msg):
global roll, pitch, yaw
new_msg = TransformStamped()
orientation_q = msg.transform.rotation
orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list[0], orientation_list[1],orientation_list[2],orientation_list[3])
global xTransformCar
xTransformCar = msg.transform.translation.x * 1000
new_msg.transform.translation.x = xTransformCar
global yTransformCar
yTransformCar = msg.transform.translation.y * -1000
new_msg.transform.translation.y = yTransformCar
new_msg.transform.translation.z = msg.transform.translation.z * 1000
global angleRelativeDeg
roll = roll * radToDegMult
pitch = pitch * radToDegMult
yaw = yaw * radToDegMult
new_msg.transform.rotation.x = roll
new_msg.transform.rotation.y = pitch
new_msg.transform.rotation.z = yaw
new_msg.transform.rotation.w = angleRelativeDeg
carPub.publish(new_msg)
def ballCb (msg):
new_msg = TransformStamped()
global xTransformBall
xTransformBall = msg.transform.translation.x * 1000
new_msg.transform.translation.x = xTransformBall
global yTransformBall
yTransformBall = msg.transform.translation.y * -1000
new_msg.transform.translation.y = yTransformBall
new_msg.transform.translation.z = msg.transform.translation.z * 1000
ballPub.publish(new_msg)
#begin script
rospy.init_node('continuous_change')
#instance of PID class
pid = PID(Kp = 1.0, Ki = 0.05, Kd = 0.25)
#main loop
while not rospy.is_shutdown():
#time variable used to measure change in time between each loop instance for PID control
startTime = time.time()
#collect and store data from ROS subscription into variables
carSub = rospy.Subscriber ('/car1_vrpn/estimated_transform', TransformStamped, carCb)
ballSub = rospy.Subscriber ('/ball_vrpn/estimated_transform', TransformStamped, ballCb)
xDelta = yTransformBall - yTransformCar
yDelta = xTransformBall - xTransformCar
distance = math.sqrt(pow(xDelta, 2) + pow(yDelta, 2))
#obtain and translate bearing angle from radians to degrees for ease of use
angleRelativeRad = math.atan2(yDelta, xDelta)
angleRelativeDeg = angleRelativeRad * radToDegMult
#print statements for troubleshooting
print("yaw: " + str(yaw))
print("angle: " + str(angleRelativeDeg))
print("distance: " + str(distance))
#obtain difference in current car angle to bearing angle
yawPos = yaw >= 0
relAnglePos = angleRelativeDeg >= 0
angleErr = (angleRelativeDeg) - (yaw)
overShoot = abs(angleErr) >180
print("angle diff: " + str(angleErr))
#PID speed control
dt = time.time() - startTime
control_signal = pid.update(angleErr, dt)
speed += control_signal * dt
#logic defining value mapping to ensure proper translation to serial as seen in previous tutorial
#communicates speed value(s) or action value depending on if car angle matches bearing angle
if(yaw > angleRelativeDeg + 1):
print("Greater")
if(not overShoot):
speed = mapValue(speed, 0, 180, highBound, lowBound)/mapConst
else:
speed = -mapValue(speed, 180, 360, lowBound, highBound)/mapConst
value = write_read(int(speed))
print(value)
elif(yaw < angleRelativeDeg - 1):
print("Lesser")
if(not overShoot):
speed = -mapValue(abs(speed), 0, 180, lowBound, highBound)/mapConst
else:
speed = mapValue(abs(speed), 180, 360, lowBound, highBound)/mapConst
value = write_read(int(speed))
print(value)
elif(angleRelativeDeg -angleRangeConst < yaw < angleRelativeDeg + angleRangeConst):
print("In Range")
if(distance > 150):
value = write_read(200)
time.sleep(0.4)
print(value)
else:
value = write_read(300)
print(value)
time.sleep(3)
print("Speed PID: " + str(speed))
r = rospy.Rate(100)
r.sleep()
**Final Words**