# Released under the MIT license (http://choosealicense.com/licenses/mit/).
# For more information, see https://github.com/DexterInd/BrickPi3/blob/master/LICENSE.md
#
# This code is an example for reading an NXT ultrasonic sensor connected to PORT_1 of the BrickPi3
#
# Hardware: Connect an NXT ultrasonic sensor to BrickPi3 Port 1.
#
# Results: When you run this program, you should see the distance in CM.
from __future__ import print_function # use python 3 syntax but make it compatible with python 2
from __future__ import division # ''
import time # import the time library for the sleep function
import brickpi3 # import the BrickPi3 drivers
BP = brickpi3.BrickPi3(
) # Create an instance of the BrickPi3 class. BP will be the BrickPi3 object.
# Configure for an NXT ultrasonic sensor.
# BP.set_sensor_type configures the BrickPi3 for a specific sensor.
# BP.PORT_1 specifies that the sensor will be on sensor port 1.
# BP.SENSOR_TYPE.NXT_ULTRASONIC specifies that the sensor will be an NXT ultrasonic sensor.
BP.set_sensor_type(BP.PORT_1, BP.SENSOR_TYPE.NXT_ULTRASONIC)
try:
while True:
# read and display the sensor value
# BP.get_sensor retrieves a sensor value.
# BP.PORT_1 specifies that we are looking for the value of sensor port 1.
# BP.get_sensor returns a list of two values.
# The first item in the list is the sensor value (what we want to display).
# The second item in the list is the error value (should be equal to BP.SUCCESS if the value was read successfully)
import time
import brickpi3
import math
BP = brickpi3.BrickPi3()
SONAR_PORT = BP.PORT_1
SONAR_INITIALISED = False
PORT_A = BP.PORT_A
PORT_B = BP.PORT_B
PORT_C = BP.PORT_C
RIGHT_WHEEL = PORT_B
LEFT_WHEEL = PORT_C
class ScalingFactors:
movement = 1.11 * 360 / 229
rotation = 1.155 * 360 / 229
# LIMIT 25
# rotation = 1.13 * 360 / 229
# almost perfect
# almost perfect
# rotation = 1.15 * 360 / 229
# overrotation a bit (2cm)
# overrotation a bit (2cm)
# overrotation a bit (1cm)
class BricKuberLib(object):
# create a BrickPi3 instance
BP = brickpi3.BrickPi3()
# define motor ports
MOTOR_GRAB = 0
MOTOR_TURN = 1
MOTOR_PORTS = [BP.PORT_D, BP.PORT_A]
def __init__(self, robot_style, debug = False):
self.debug = debug
if robot_style == "NXT1":
self.TurnTablePinion = 24
self.TurnTableGear = 56
# motor position constants
self.MOTOR_GRAB_POSITION_HOME = 0
self.MOTOR_GRAB_POSITION_REST = -35
self.MOTOR_GRAB_POSITION_FLIP_PUSH = -90
self.MOTOR_GRAB_POSITION_GRAB = -130
self.MOTOR_GRAB_POSITION_FLIP = -240
self.MOTOR_GRAB_SPEED_GRAB = 400
self.MOTOR_GRAB_SPEED_FLIP = 600
self.MOTOR_GRAB_SPEED_REST = 400
elif robot_style == "EV3":
self.TurnTablePinion = 12
self.TurnTableGear = 36
# motor position constants (these likely need to be adjusted)
self.MOTOR_GRAB_POSITION_HOME = 0
self.MOTOR_GRAB_POSITION_REST = -35
self.MOTOR_GRAB_POSITION_FLIP_PUSH = -90
self.MOTOR_GRAB_POSITION_GRAB = -130
self.MOTOR_GRAB_POSITION_FLIP = -240
self.MOTOR_GRAB_SPEED_GRAB = 400
self.MOTOR_GRAB_SPEED_FLIP = 600
self.MOTOR_GRAB_SPEED_REST = 400
else:
raise ValueError("Unsupported robot style")
self.BP.set_motor_limits(self.MOTOR_PORTS[self.MOTOR_TURN], 0, ((500 * self.TurnTableGear) / self.TurnTablePinion))
self.home_all()
# find motor home positions for all motors
def home_all(self):
self.BP.set_motor_power(self.MOTOR_PORTS[self.MOTOR_GRAB], 15)
EncoderLast = self.BP.get_motor_encoder(self.MOTOR_PORTS[self.MOTOR_GRAB])
time.sleep(0.1)
EncoderNow = self.BP.get_motor_encoder(self.MOTOR_PORTS[self.MOTOR_GRAB])
while EncoderNow != EncoderLast:
EncoderLast = EncoderNow
time.sleep(0.1)
EncoderNow = self.BP.get_motor_encoder(self.MOTOR_PORTS[self.MOTOR_GRAB])
self.BP.offset_motor_encoder(self.MOTOR_PORTS[self.MOTOR_GRAB], (EncoderNow - 25))
self.BP.set_motor_position(self.MOTOR_PORTS[self.MOTOR_GRAB], self.MOTOR_GRAB_POSITION_REST)
self.BP.offset_motor_encoder(self.MOTOR_PORTS[self.MOTOR_TURN], self.BP.get_motor_encoder(self.MOTOR_PORTS[self.MOTOR_TURN]))
self.TurnTableTarget = 0
self.spin(0)
# run a motor to the specified position, and wait for it to get there
def run_to_position(self, port, position, tolerance = 3):
self.BP.set_motor_position(self.MOTOR_PORTS[port], position)
encoder = self.BP.get_motor_encoder(self.MOTOR_PORTS[port])
while((encoder > (position + tolerance)) or (encoder < (position - tolerance))):
time.sleep(0.01)
encoder = self.BP.get_motor_encoder(self.MOTOR_PORTS[port])
# spin the cube the specified number of degrees. Opionally overshoot and return (helps with the significant mechanical play while making a face turn).
def spin(self, deg, overshoot = 0):
if deg < 0:
overshoot = -overshoot
self.TurnTableTarget -= (deg + overshoot)
self.run_to_position(self.MOTOR_TURN, ((self.TurnTableTarget * self.TurnTableGear) / self.TurnTablePinion))
if overshoot != 0:
self.TurnTableTarget += overshoot
self.run_to_position(self.MOTOR_TURN, ((self.TurnTableTarget * self.TurnTableGear) / self.TurnTablePinion))
# grab the cube
def grab(self):
self.BP.set_motor_limits(self.MOTOR_PORTS[self.MOTOR_GRAB], 0, self.MOTOR_GRAB_SPEED_GRAB)
self.run_to_position(self.MOTOR_GRAB, self.MOTOR_GRAB_POSITION_GRAB)
time.sleep(0.2)
# release the cube
def release(self):
self.BP.set_motor_limits(self.MOTOR_PORTS[self.MOTOR_GRAB], 0, self.MOTOR_GRAB_SPEED_REST)
self.run_to_position(self.MOTOR_GRAB, self.MOTOR_GRAB_POSITION_REST)
# flip the cube, and optionall release if afterwards
def flip(self, release = False):
self.run_to_position(self.MOTOR_GRAB, self.MOTOR_GRAB_POSITION_FLIP_PUSH)
time.sleep(0.05)
self.grab()
time.sleep(0.2)
self.BP.set_motor_limits(self.MOTOR_PORTS[self.MOTOR_GRAB], 0, self.MOTOR_GRAB_SPEED_FLIP)
self.run_to_position(self.MOTOR_GRAB, self.MOTOR_GRAB_POSITION_FLIP)
self.run_to_position(self.MOTOR_GRAB, self.MOTOR_GRAB_POSITION_FLIP_PUSH)
if release:
self.release()
# Return Opposite Face.
def OF(self, f):
if f < 3:
return f + 3
return f - 3
# Current Cube Orientation. Keeps track of the cube orientation.
CCO = [0, 1, 2] # side U, F, R
# Execute a move
def Move(self, cmd):
DegreesToTurnFace = -90
RecoverFace = 22
if(cmd.find("'") != -1):
DegreesToTurnFace = 90
elif (cmd.find("2") != -1):
DegreesToTurnFace = -180
if(cmd.find("U") != -1):
FaceToTurn = 0
elif(cmd.find("F") != -1):
FaceToTurn = 1
elif(cmd.find("R") != -1):
FaceToTurn = 2
elif(cmd.find("D") != -1):
FaceToTurn = 3
elif(cmd.find("B") != -1):
FaceToTurn = 4
elif(cmd.find("L") != -1):
FaceToTurn = 5
if FaceToTurn == self.CCO[0]:
# target is top
# flip twice
self.flip()
self.flip()
self.CCO[0] = self.OF(self.CCO[0])
self.CCO[1] = self.OF(self.CCO[1])
elif FaceToTurn == self.CCO[1]:
# target is front
# rotate 180 and flip
self.release()
self.spin(180)
self.CCO[1] = self.OF(self.CCO[1])
self.CCO[2] = self.OF(self.CCO[2])
self.flip()
tmp = self.CCO[0]
self.CCO[0] = self.CCO[1]
self.CCO[1] = self.OF(tmp)
elif FaceToTurn == self.CCO[2]:
# target is right
# rotate -90 and flip
self.release()
self.spin(-90)
tmp = self.CCO[2]
self.CCO[2] = self.CCO[1]
self.CCO[1] = self.OF(tmp)
self.flip()
tmp = self.CCO[0]
self.CCO[0] = self.CCO[1]
self.CCO[1] = self.OF(tmp)
elif FaceToTurn == self.OF(self.CCO[0]):
# target is bottom
# don't do anything
pass
elif FaceToTurn == self.OF(self.CCO[1]):
# target is back
# flip
self.flip()
tmp = self.CCO[0]
self.CCO[0] = self.CCO[1]
self.CCO[1] = self.OF(tmp)
elif FaceToTurn == self.OF(self.CCO[2]):
# target is left
# rotate 90 and flip
self.release()
self.spin(90)
tmp = self.CCO[1]
self.CCO[1] = self.CCO[2]
self.CCO[2] = self.OF(tmp)
self.flip()
tmp = self.CCO[0]
self.CCO[0] = self.CCO[1]
self.CCO[1] = self.OF(tmp)
self.grab()
self.spin(DegreesToTurnFace, RecoverFace)
# Execute a string of moves
def Moves(self, cmds):
for cmd in cmds.split():
self.Move(cmd)
self.run_to_position(self.MOTOR_GRAB, self.MOTOR_GRAB_POSITION_REST)
rgb_values = [[0, 0, 0] for c in range(54)]
# Use the camera to read the RGB colors for each of the 9 squares on the face
def CameraReadFaceColors(self, face):
commands.getstatusoutput(('raspistill -w 300 -h 300 -t 1 -o /tmp/BricKuber_%s_face.jpg' % face))
raw_result = commands.getstatusoutput(('rubiks-cube-tracker.py --filename /tmp/BricKuber_%s_face.jpg' % face))[1]
raw_result = raw_result.split("\n{")[1]
raw_result = raw_result.split("}")[0]
raw_results = raw_result.split("[")[1:]
for c in range(9):
raw_results[c] = raw_results[c].split("]")[0]
if face == "front":
numbers = [19, 20, 21, 22, 23, 24, 25, 26, 27]
elif face == "right":
numbers = [36, 35, 34, 33, 32, 31, 30, 29, 28]
elif face == "back":
numbers = [43, 40, 37, 44, 41, 38, 45, 42, 39]
elif face == "left":
numbers = [16, 13, 10, 17, 14, 11, 18, 15, 12]
elif face == "top":
numbers = [1, 2, 3, 4, 5, 6, 7, 8, 9]
elif face == "bottom":
numbers = [46, 47, 48, 49, 50, 51, 52, 53, 54]
for c in range(9):
vals = raw_results[c].split(", ")
for v in range(3):
self.rgb_values[numbers[c] - 1][v] = int(vals[v])
# Read the entire cube, and retun the result as a string that can be fed directly into kociemba.
def ReadCubeColors(self):
self.release()
self.CameraReadFaceColors("top")
self.flip(True)
self.CameraReadFaceColors("front")
self.flip(True)
self.CameraReadFaceColors("bottom")
self.spin(90)
self.flip(True)
self.CameraReadFaceColors("right")
self.spin(-90)
self.flip(True)
self.CameraReadFaceColors("back")
self.flip(True)
self.CameraReadFaceColors("left")
self.CCO = [5, 1, 0]
str = "'{"
for c in range(54):
str += '"%d": [%d, %d, %d]' % ((c + 1), self.rgb_values[c][0], self.rgb_values[c][1], self.rgb_values[c][2])
if c == 53:
str += "}'"
else:
str += ', '
str = 'rubiks-color-resolver.py --rgb %s' % str
if self.debug:
print(str)
raw_result = commands.getstatusoutput(str)[1]
raw_result = raw_result.split("\n")
raw_result = raw_result[-1]
return raw_result
import brickpi3 as bp import time BP = bp.BrickPi3() a = 1 BP.offset_motor_encoder(BP.PORT_A, BP.get_motor_encoder(BP.PORT_A)) BP.set_motor_position_kp(BP.PORT_A, 25) BP.set_motor_position_kd(BP.PORT_A, 70) BP.set_motor_position(BP.PORT_A, 3600) ''' while a == 1: try : curr_mot_enc = BP.get_motor_encoder(BP.PORT_A) print(curr_mot_enc) except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard. BP.set_motor_power(BP.PORT_A, 0) BP.reset_all() # Unconfigure the sensors, disable the motors, and restore the LED to the control of the BrickPi3 firmware. a =0 print(curr_mot_enc) ''' time.sleep(5) last_motor_encoder = BP.get_motor_encoder(BP.PORT_A) print(last_motor_encoder)
#!/usr/bin/env python import rospy import brickpi3 import numpy as np import math import time from std_msgs.msg import String, Int16 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint BP = brickpi3.BrickPi3() # Create an instance of the BrickPi3 class # Number of the Joints of the robot nrOfJoints = 3 qNow=np.array([0.0] * nrOfJoints) # This will be our joints state, global access in this script # This int can be used to check wether there are pending requests to be attended by the planner or not pendantRequest = 0 # Joint order according to the motor connected to BrickPi3 ports jointsOrder = [BP.PORT_C, BP.PORT_A, BP.PORT_B] #Conversion from radians to degree of motors. Calculated considering the gear ratio jointsScale = np.array([7*180/math.pi, 3*180/math.pi, 1.6*180/math.pi]) # callbackPlanner(data) This function is triggered when a ROS message is being received while in ratePendant.sleep() # This function takes what the trajectory planner sends, according to the requests you do from this python code # Also, it uses that information to command the motors, iteratively until they reach the jointPoints given a degrees precision and advances to the next def callbackPlanner(data): global BP # We need to specify the globals to be modified
def __init__(self, cell_size, option, init_position=[0.0, 0.0, 0.0]):
"""
Inicializa los parametros basicos del robot y lo configura en funcion
de los puertos a los cuales estan conectados los sensores y motores
"""
# Robot construction parameters
self.r = 0.027 # Radio de las ruedas, en metros
self.L = 0.135 # Distancia entre centros de ruedas, en metros
self.thd = 0.0
self.thi = 0.0
self.logFile = open("log.txt", "w+") # Fichero de log de odometria
self.img_count = 0
##################################################
# Motors and sensors setup
# Create an instance of the BrickPi3 class. BP will be the BrickPi3 object.
self.BP = brickpi3.BrickPi3()
# Configure sensors
self.BP.set_sensor_type(self.BP.PORT_1,
self.BP.SENSOR_TYPE.EV3_ULTRASONIC_CM)
time.sleep(5)
self.distance_from_obstacles = 20.0 # In centimeters
self.security_distance = 10.0 # In centimeters
self.cell_size = cell_size # In meters
# reset encoder B and C (or all the motors you are using)
self.BP.offset_motor_encoder(self.BP.PORT_B,
self.BP.get_motor_encoder(self.BP.PORT_B))
self.BP.offset_motor_encoder(self.BP.PORT_C,
self.BP.get_motor_encoder(self.BP.PORT_C))
self.BP.offset_motor_encoder(self.BP.PORT_A,
self.BP.get_motor_encoder(self.BP.PORT_A))
# CAMERA SETTINGS
self.CAM_CENTER = 320 / 2.0
self.cam = picamera.PiCamera()
self.cam.resolution = (320, 240)
self.cam.framerate = 32
self.cam_refresh = 1.0 / self.cam.framerate
# EXIT SETTINGS
if option == 'A':
self.cmp_img = cv2.imread("img1.png", cv2.IMREAD_COLOR)
else:
self.cmp_img = cv2.imread("img2.png", cv2.IMREAD_COLOR)
##################################################
# odometry shared memory values
self.x = Value('d', init_position[0])
self.y = Value('d', init_position[1])
self.th = Value('d', init_position[2])
self.finished = Value(
'b', 1) # boolean to show if odometry updates are finished
# if we want to block several instructions to be run together, we may want to use an explicit Lock
self.lock_odometry = Lock()
self.P = 1.0 / 50 # Frecuencia de actualizacion de la odometria, en acts/segundo
except:
pass
print("Scratch Interpreted closed")
##################################################################
# GLOBAL VARIABLES
##################################################################
# Set to 1 to have debugging information printed out
# Set to 0 to go into quiet mode
en_debug = 1
success_code = "SUCCESS"
try:
BP3 = brickpi3.BrickPi3()
bp3ports = [BP3.PORT_1, BP3.PORT_2, BP3.PORT_3, BP3.PORT_4]
bp3motors = [BP3.PORT_A, BP3.PORT_B, BP3.PORT_C, BP3.PORT_D]
sensor_types = {
'NONE': [BP3.SENSOR_TYPE.NONE, "None"],
'EV3US': [BP3.SENSOR_TYPE.EV3_ULTRASONIC_CM, "US cm"],
'EV3USCM': [BP3.SENSOR_TYPE.EV3_ULTRASONIC_CM, "US cm"],
'EV3USIN': [BP3.SENSOR_TYPE.EV3_ULTRASONIC_INCHES, "US Inch"],
'EV3USLISTEN': [BP3.SENSOR_TYPE.EV3_ULTRASONIC_LISTEN, "US Listen"],
'EV3GYRO': [BP3.SENSOR_TYPE.EV3_GYRO_ABS, "Gyro ABS"],
'EV3GYROABS': [BP3.SENSOR_TYPE.EV3_GYRO_ABS, "Gyro ABS"],
'EV3GYRODPS': [BP3.SENSOR_TYPE.EV3_GYRO_DPS, "Gyro DPS"],
'EV3GYROABSDPS':
[BP3.SENSOR_TYPE.EV3_GYRO_ABS_DPS, "Gyro ABS", "Gyro DPS"],
'EV3IR': [BP3.SENSOR_TYPE.EV3_INFRARED_PROXIMITY, "IR Prox"],
