# 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"],