DASL Wiki

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
<fs xx-large>**NXT Automation Alternatives & Practical Implementations**</fs>
\\
**Author: **Hashim H., Email: hameeh1@unlv.nevada.edu
\\
**Date: **Last modified on [11/23/24]
\\
**Keywords: **NXT, Teleoperation, Automation

<fs x-large>**Motivation and Audience**</fs>

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]]

<fs x-large>**Parts List and Sources**</fs>
\\
  *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]]


<fs x-large>**Software**</fs>
\\
  *[[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]]

<fs x-large>**Arduino-I2C Alternative**</fs>

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.

<fs large>**Setup**</fs>

**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]]

<fs large>**Programming**</fs>

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:

<code c [enable_line_numbers="true"] robotC_I2C.c>
#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];
	}

}

</code>

<fs x-large>**Teensy-PS2 Alternative [*Dubious Functionality]**</fs>

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. 

<fs large>**Setup**</fs>

**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|}}

<fs large>**Programming**</fs>

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}}

**<fs x-large>Additional Scripts</fs>**

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:

<code python [enable_line_numbers="true"] nxt_basic_vision.py>
#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()

</code>


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:

<code python [enable_line_numbers="true"] nxt_mocap.py>
#!/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()

</code>

<fs x-large>**Final Words**</fs>