def test_location(self):
r = Robot()
self.assertRaises(LocationError, r.move)
self.assertRaises(LocationError, r.left)
self.assertRaises(LocationError, r.right)
self.assertRaises(LocationError, r.report)
from robot import Robot
from pid_controller import PIController
import time
bot = Robot()
stop_at_time = time.time() + 60
speed = 80
bot.set_left(speed)
bot.set_right(speed)
pid = PIController(proportional_constant=4, integral_constant=0.2)
while time.time() < stop_at_time:
time.sleep(0.02)
# Calculate the error
left = bot.left_encoder.pulse_count
right = bot.right_encoder.pulse_count
error = left - right
# Get the speed
adjustment = pid.get_value(error)
right_speed = int(speed + adjustment)
print "left", left, \
"right", right, \
"right_speed:", right_speed, \
"error:", error, \
"adjustment: %.2f" % adjustment
# Appy it to the right motor
bot.set_right(right_speed)
(motor_torque(robot, t, x[2], x[3]) +
robot.m_cylinder() * robot.g *
(x[0] - robot.r_max()) * math.sin(x[2])) /
robot.I_flywheel(x[0])
]
return [
x[1], -robot.g * math.cos(x[2]) + x[0] * x[3] * x[3], x[3],
(motor_torque(robot, t, x[2], x[3]) +
robot.m_cylinder() * robot.g *
(x[0] - robot.r_max()) * math.sin(x[2])) / robot.I_flywheel(x[0])
]
return lambda t, x: aux_function(t, x)
my_robot = Robot()
my_robot.set_r_flywheel_r_wheel_w_N(.086, .10, .04, 2)
system = system_function(my_robot)
def r_max_event(t, x):
if (x[1] <= 0):
return -1
return x[0] - my_robot.r_max()
def r_min_event(t, x):
if (x[1] >= 0):
return 1
return x[0] - my_robot.r_min()
def __init__(self):
# create the physics simulator
self.physicsClient = p.connect(p.GUI)
p.setGravity(0, 0, -9.81)
self.max_communication_distance = 2.0
# We will integrate every 4ms (250Hz update)
self.dt = 1. / 250.
p.setPhysicsEngineParameter(self.dt, numSubSteps=1)
# Create the plane.
self.planeId = p.loadURDF("../models/plane.urdf")
p.changeDynamics(self.planeId,
-1,
lateralFriction=5.,
rollingFriction=0)
self.goalId = p.loadURDF("../models/goal.urdf")
p.resetDebugVisualizerCamera(7.0, 50.0, -35.0, (1., 1., 0.0))
# Add objects
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [0., -1., 0],
(0., 0., 0.5, 0.5))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [0., 1., 0],
(0., 0., 0.5, 0.5))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [3., -1., 0],
(0., 0., 0.5, 0.5))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [3., 1., 0],
(0., 0., 0.5, 0.5))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [1., 2., 0],
(0., 0., 0., 1.))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [2., -2., 0],
(0., 0., 0., 1.))
# tube
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [-1., 5., 0],
(0., 0., 0., 1.))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [-1., 6., 0],
(0., 0., 0., 1.))
#arena
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [-2, 4., 0],
(0., 0., 0.5, 0.5))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [-2., 7., 0],
(0., 0., 0.5, 0.5))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [-2., 9., 0],
(0., 0., 0.5, 0.5))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [-2., 11., 0],
(0., 0., 0.5, 0.5))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [-2., 13., 0],
(0., 0., 0.5, 0.5))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [-3., 3., 0],
(0., 0., 0., 1.))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [-5., 3., 0],
(0., 0., 0., 1.))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [-7., 3., 0],
(0., 0., 0., 1.))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [-8, 4., 0],
(0., 0., 0.5, 0.5))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [-8., 6., 0],
(0., 0., 0.5, 0.5))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [-8., 8., 0],
(0., 0., 0.5, 0.5))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [-8., 10., 0],
(0., 0., 0.5, 0.5))
wallId = p.loadSDF("../models/walls.sdf")[0]
p.resetBasePositionAndOrientation(wallId, [-8., 12., 0],
(0., 0., 0.5, 0.5))
# create 6 robots
self.robots = []
for (i, j) in itertools.product(range(3), range(2)):
self.robots.append(
Robot([1. * i + 0.5, 1. * j - 0.5, 0.3], 2 * i + j, self.dt))
p.stepSimulation()
self.time = 0.0
self.stepSimulation()
self.stepSimulation()
#!/usr/bin/env python3
from robot import Robot
rb = Robot()
while rb.update():
print(rb.pos_x, rb.pos_y, rb.pos_t, rb.range)
def setUp(self):
self.mega_man = Robot("Mega man", battery=50)
def main():
robot = Robot(Piso(int(sys.argv[1]), int(sys.argv[2])))
robot.excavar()
def compute_fitness(genome, env):
sim = new_sim(pb=False)
robot = Robot(sim, genome)
robot.evaluate(sim, env)
fitness, distance = robot.eval_fitness(sim, env)
return fitness, distance
def test_calculate_viewzone_for_corner(self):
field = Map(7, 7)
robot = Robot(0, 0)
coord = simulator.calculate_viewzone(field, robot, 3, 3, 3, 3)
self.assertEqual(coord, (3 - 3, 3, 3 - 3, 3))
def __call__(self, *args):
return Robot(*self.params)
joint_dict['4 lower'].enable()
joint_dict['4 shoulder'].enable()
for controller in odrive_controllers:
controller.send_packet()
for controller in odrive_controllers:
controller.block_for_response()
print('running loop')
#start gui worker - needs arm object
#process_list.append(Process(target=gui_worker, args=(None, )))
#process_list[-1].start()
#------------------------------------arjun do stuff here ---------------------------------------------
robot = Robot(arm_dict)
robot.boot()
# main program
try:
while True:
robot.loop()
for controller in odrive_controllers:
controller.send_packet()
for controller in odrive_controllers:
controller.block_for_response()
except(SystemExit):
print('i die')
cv2.rectangle(frame, (x, y), (x + w, y + w), [255, 0, 0])
self.make_display(frame)
return x, y, w, h
def run(self):
camera = camera_stream.setup_camera()
time.sleep(0.1)
print("Setup Complete")
for frame in camera_stream.start_stream(camera):
(x, y, w, h) = self.process_frame(frame)
self.process_control()
if self.running and h > self.min_size:
pan_error = self.center_x - (x + (w / 2))
pan_value = self.pan_pid.get_value(pan_error)
self.robot.set_pan(int(pan_value))
tilt_error = self.center_y - (y + (h / 2))
tilt_value = self.tilt_pid.get_value(tilt_error)
self.robot.set_tilt(int(tilt_value))
print(
f"x: {x}, y: {y}, pan_error: {pan_error}, tilt_error: {tilt_error}, pan_value: {pan_value:.2f}, tilt_value: {tilt_value:.2f}"
)
print("Setting up")
behavior = FaceTrackBehavior(Robot())
process = start_server_process('color_track_behavior.html')
try:
behavior.run()
finally:
process.terminate()
def main():
r = Robot()
try:
r.drive_triple(0, 100, 80, 15, 15, 5, 0, "run", -1, -1, 5)
# r.turn(-90)
# time.sleep(3)
# r.turn(90)
# r.get_direction_drive(60, 40, 2, 0, "run", 100)
# r.drive_triple(0, 100, 50, 10, 20, 5, -2, "run", 50, 0)
# r.drive(50, 0, 9.5, -1, "brake")
# r.turn(90)
# print("TESTING")
# r.beep(True)
# while True:
# print("left: " + str(r.col_l.light_reflected()) + "right: " + str(r.col_r.light_reflected()))
# r.lifter.move_to_top_position()
# positions = [0, 0, 0]
# pt_3.drop_food(r, positions)
# time.sleep(2)
# r.drive_triple(0, 50, 0, 10, 10, 10, 0, "brake")
# r.drive_triple(0, 50, 0, 10, 10, 10, 0, "brake")
# r.drive(0, 60, 5, 0, "run", 50, 50)
# r.align_driving(60, 0, 3, 5, "brake")
# r.drive(30, 30, 4, 0, "brake")
#
# r.lifter.move_up()
# r.lifter.move_up()
# r.lifter.move_up()
#
# time.sleep(1)
#
# r.drive(30, 30, 4, 0, "brake")
#
# r.lifter.move_down()
# r.lifter.move_down()
# r.lifter.move_down()
# r.drive(0, 20, 5)
# r.get_direction(20)
# r.pivot(-90, True)
# r.drive_triple(0, 100, 0, 10, 5, 15, 0, "brake")
# r.pivot(90, False)
# r.drive_triple(-20, -100, -20, 20, 5, 20, 0, "hold")
# r.drive_triple(0, 80, 0, 10, 1, 10, 0, "brake")
# r.pivot(90, True)
# r.pivot(-90, False)
# r.pivot(-90, False)
# r.lifter.move_up()
# time.sleep(2)
# r.lifter.move_up()
# time.sleep(2)
# r.lifter.move_up()
# time.sleep(2)
# r.lifter.move_down()
# time.sleep(2)
# r.lifter.move_down()
# time.sleep(2)
# r.lifter.move_down()
# time.sleep(2)
# r.ht_middle.mode = 'WHITE'
# while True:
# print(str(r.ht_middle.light_reflected()))
# a = r.ht_middle.value(0)
# b = r.ht_middle.value(1)
# c = r.ht_middle.value(2)
# d = r.ht_middle.value(3)
# print(str(a) + ' ' + str(b) + ' ' + str(c) + ' ' + str(d))
# r.turn(360)
# r.turn(-90)
# print("l: " + str(r._lMot.position) + "; r: " + str(r._rMot.position))
# while True:
# color = r.ht_middle.get_color()
# r.speak(color.to_text())
# r.wait_until_button()
# print(str(r.lifter.position))
#
# r.lifter.move_to_top_position(False)
# print(str(r.lifter.position))
# time.sleep(3)
# r.lifter.move_to_first_position(False)
# time.sleep(3)
# direction = r.get_direction_drive(80, 20, 8.5, 10, "brake") # Calculate error
# r.turn(-90 - direction)
# r.ht_middle.mode = 'WHITE'
# r.line_follow(30, 30, 100, 50, "run")
# r.ht_side.mode = 'COLOR'
# while True:
# print(r.ht_side.get_color().to_text())
# print(r.ht_side.value())
# r.drive_triple(0, -100, 0, 5, 5, 5, 0, "brake")
# time.sleep(2)
# print(str(r._lMot.position) + ' ' + str(r._rMot.position))
# r.reset_motor_pos()
# r.turn(90)
#
# r.drive(0, 60, 5, 0, "run", 50, 50)
# r.align()
# pt_3.drop_off(r, 0, 0)
#
# r.ht_middle.mode = 'WHITE'
# while True:
# print(str(r.ht_middle.value()) + ' ' + str(r.ht_middle.light_reflected()))
# while True:
# r.beep(True)
# r.wait_until_button()
# r.slider.close(True, 100, 5)
# r.slider.open_for_ships()
finally:
print("RESET")
r.reset()
time.sleep(0.5)
def test_report(self):
r = Robot()
r.place()
self.assertDictEqual(r.report(), dict(X=0, Y=0, F='NORTH'))
#!/usr/bin/env python
import rospy
from robot import Robot
rospy.init_node("move_base")
youbot = Robot()
r = rospy.Rate(1)
while not rospy.is_shutdown():
print youbot.position
r.sleep()
p.setRealTimeSimulation(0)
# reset cam
p.resetDebugVisualizerCamera(cameraDistance=8,
cameraYaw=-90,
cameraPitch=-30,
cameraTargetPosition=[0, 0, 0])
# create workspace where geometry will be printed
geoSpaceShape = p.createCollisionShape(shapeType=p.GEOM_BOX,
halfExtents=[2, 2, 0])
geoSpaceBody = p.createMultiBody(baseCollisionShapeIndex=geoSpaceShape,
basePosition=[0, 0, 0])
# initialize the robot and draw a bounding box
kuka = Robot(p, "kuka_iiwa/model.urdf", [0, 3, 0])
# starting base position of robot
basePos = kuka.baseInfo[0]
# show the desired geometry
# print('printing the geom')
# h.drawContDF(p, path_df, [1,1,1])
# robot will only be working in one hemisphere
posPathDF = path_df.loc[path_df['y'] >= 0]
negPathDF = path_df.loc[path_df['y'] < 0]
# show the hemisphere where robot will be working
h.drawContDF(p, posPathDF, [0, 0.5, 0])
# only want to consider points at z = 0 now
def robot():
return Robot()
print("Setup Complete")
print('Radius, Radius error, speed value, direction error, direction value')
for frame in camera_stream.start_stream(camera):
(x, y), radius = self.process_frame(frame)
self.process_control()
if self.running and radius > 20:
radius_error = self.correct_radius - radius
speed_value = speed_pid.get_value(radius_error)
direction_error = self.center - x
direction_value = direction_pid.get_value(direction_error)
print(f"{radius}, {radius_error}, {speed_value:.2f}, {direction_error}, {direction_value:.2f}")
# Now produce left and right motor speeds
self.robot.set_left(speed_value - direction_value)
self.robot.set_right(speed_value + direction_value)
else:
self.robot.stop_motors()
if not self.running:
speed_pid.reset()
direction_pid.reset()
print("Setting up")
behavior = ColorTrackingBehavior(Robot())
process = start_server_process('color_track_behavior.html')
try:
behavior.run()
finally:
process.terminate()
# To Be Done
# [ ] FastSlam2.0
# [ ] ICP
# [ ] Graph Based SLAM
# [ ] Circle Fitting for Robot Detection
# pylint: disable=invalid-name
pygame.init()
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT))
clock = pygame.time.Clock()
# environment = Environment()
field = Field(10, 10)
robot = Robot(480, 480)
running = True
# pylint: enable=invalid-name
tick = 0
while running:
robot.update()
# environment.update(robot)
field.update(robot)
tick += 1
clock.tick(2000 * 1)
if tick % 20 == 0:
def get_robot_move(self):
rb = Robot(self._game)
res = rb.get_a_move()
return QPoint(res[0],res[1])
import test_motors
import test_power
import test_servo
TEST_MOTOR_BOARD = True
TEST_POWER_BOARD = True
TEST_SERVO_ASSEMBLY = True
def test_board(board, test_function):
print("Testing board {}".format(board.serial))
test_function(board)
print("Testing board {} Complete!".format(board.serial))
print("\n\n")
if __name__ == '__main__':
r = Robot()
if TEST_SERVO_ASSEMBLY:
for servo_assembly in r.servo_boards:
test_board(servo_assembly, test_servo.test_servo_assembly)
if TEST_MOTOR_BOARD:
for motor_board in r.motor_boards:
test_board(motor_board, test_motors.test_motor_board)
if TEST_POWER_BOARD:
for power_board in r.power_boards:
test_board(power_board, test_power.test_power_board)
cv2.namedWindow('map', cv2.WINDOW_AUTOSIZE)
# Load configs
env_config = utils.load_config('../config/environment.yaml')
agent_config = utils.load_config('../config/agent.yaml')
grid_map_config = utils.load_config('../config/grid_map.yaml')
# Initialize 2D Environment
env = utils.load_env_from_img(env_config['img_src'])
env = cv2.resize(env, (round(env_config['scale'] * env.shape[1]),
round(env_config['scale'] * env.shape[0])),
interpolation=cv2.INTER_LINEAR)
# Initialize Agent
robot_pos = np.array([150.0, 100.0, 0.0])
robot = Robot(robot_pos, agent_config['robot'], agent_config['sensor'])
# Initialize Grid Map
m = OccupancyGridMap(grid_map_config, grid_size=1.0)
sensor_data = robot.measure(env)
SensorMapping(m, robot.pose, robot.sensor.config, sensor_data)
# Initialize Particle
pf = ParticleFilter(robot_pos.copy(), agent_config['robot'],
agent_config['sensor'], copy.deepcopy(m), 10)
img = Draw(env, 1, robot.pose, sensor_data, robot.sensor.config)
mimg = AdaptiveGetMap(m)
cv2.imshow('view', img)
cv2.imshow('map', mimg)
def __init__(self, parent, height, width, initial_data):
super().__init__(parent)
self.height = int(height / 3)
self.width = int(width)
self["height"] = self.height
self["width"] = self.width
self.parent = parent
self.color = initial_data[0]["team_color"]
print(self.color)
# self.columnconfigure(1, weight=1)
# self.rowconfigure(0, weight=1)
BACKGROUND_PATH = "Assets/team1_background.jpg" if initial_data[0][
"team_number"] == "team1" else "Assets/team2_background.jpg"
background_image = ImageTk.PhotoImage(
Image.open(BACKGROUND_PATH).resize((self.width, self.height),
Image.ANTIALIAS))
background_label = tk.Label(self, image=background_image)
background_label.place(x=0, y=0, relwidth=1, relheight=1)
background_label.image = background_image
# self.color_picker_btn = ttk.Button(
# self,
# text="",
# #command=self.start_timer,
# cursor="hand2",
# width=self.width/18
# )
# self.color_picker_btn.grid(row=0, column=0, sticky="NS", pady=(15, 15))
self.color_bar = tk.Label(self, bg=self.color)
self.color_bar.grid(row=0, column=0, sticky="NS", pady=(15, 15))
self.robot1 = Robot(self,
height=height,
width=width,
initial_data=initial_data[0])
self.robot1.grid(
row=0, column=1, padx=(20, 20), pady=(15, 15),
sticky="NSEW") #, sticky="NSEW", padx=(10, 0), pady=(10, 0))
self.robot2 = Robot(self,
height=height,
width=width,
initial_data=initial_data[1])
self.robot2.grid(
row=0, column=2, padx=(20, 20), pady=(15, 15),
sticky="NSEW") #, sticky="NSEW", padx=(10, 0), pady=(10, 0))
self.robot3 = Robot(self,
height=height,
width=width,
initial_data=initial_data[2])
self.robot3.grid(
row=0, column=3, padx=(20, 20), pady=(15, 15),
sticky="NSEW") #, sticky="NSEW", padx=(10, 0), pady=(10, 0))
from robot import Robot
from intprogram import IntProgram
from queue import Queue
from threading import Thread
with open("input") as file:
program_orig = list(map(int, file.read().split(',')))
q1 = Queue()
q2 = Queue()
instance = IntProgram(program_orig, q1, q2)
robot = Robot(q2, q1)
instance.start()
robot.start()
instance.join()
robot.join()
robot.print_result()
self.humans.append(human)
def remove_human(self, human):
self.humans.remove(human)
def add_robot(self, robot): #Methods
self.robots.append(robot)
def remove_robot(self, robot):
self.humans.remove(robot)
def __repr__(self): #Magic methods
return f"planet(humans- {len(self.humans)}, robots- {len(self.robots)})"
def __str__(self):
return f'This planet has: humans- {len(self.humans)} and robots- {len(self.robots)}'
if (__name__ == "__main__"): #Used to display the methods
planet = Planet()
print(planet)
print(repr(planet))
jed = Human("Jed") #Adding a human entity to Human class
planet.add_human(jed)
rtx3060 = Robot("rtx3060") #Adding a robot entity to Robot class
planet.add_robot(rtx3060)
print(repr(planet))
print(planet)
def read_file():
file = open('action.txt', "r")
c = []
lines = file.readlines()
for i, line in enumerate(lines):
a=float(line.split(',')[0])
b=float(line.split(',')[-1])
c.append((a,b))
return c
mymap = Map()
mymap.GetUserNodes()
# Construct the robot
robot = Robot(mymap)
# c=read_file()
# print('\nFrom file: ')
# print(c)
robot.A_Star()
if robot.foundGoal:
robot.backtrack_path()
c=read_file()
print('\nFrom file: ')
print(c)
else:
# test and score parameters
max_time = 1000
train_score_mult = 1 / 30.
if __name__ == '__main__':
'''
This script tests a robot based on the code in robot.py on a maze given
as an argument when running the script.
'''
# Create a maze based on input argument on command line.
testmaze = Maze(str(sys.argv[1]))
# Intitialize a robot; robot receives info about maze dimensions.
testrobot = Robot(testmaze.dim)
# Record robot performance over two runs.
runtimes = []
total_time = 0
for run in range(2):
print "Starting run {}.".format(run)
# Set the robot in the start position. Note that robot position
# parameters are independent of the robot itself.
robot_pos = {'location': [0, 0], 'heading': 'up'}
run_active = True
hit_goal = False
while run_active:
# check for end of time
from mapa import Mapa
from moneda import Moneda
from utilidades import cargar_mapa, cargar_instrucciones
import time
mi_mapa = cargar_mapa("mapas/mapa1.txt")
reglas = cargar_instrucciones("instrucciones/programa1.txt")
mapa = Mapa(4, 8)
for y in range(len(mi_mapa)):
fila = mi_mapa[y]
for x in range(len(fila)):
casilla = mi_mapa[y][x]
if casilla == "*":
robot = Robot(x, y)
mapa.asignar_robot(robot)
robot.asignar_mapa(mapa)
else:
cantidad = int(casilla)
for i in range(cantidad):
moneda = Moneda(x, y)
mapa.agregar_moneda(moneda)
mon = 0
for i in reglas:
if i == 'PICK':
robot.recoger()
mon += 1
if i == 'MOVE':
robot.move()
if i == 'ROTATE':
def __init__(self):
self.verbosity = 10
##############################################
#field_width=230*in_*3
#field_height=133*in_*3
self.field_width = 54 * ft_
self.field_height = 27 * ft_
self.hab_line_x = 94.3 * in_
# Call this function so the Pygame library can initialize itself
pygame.init()
pygame.joystick.init()
self.joysticks = [
pygame.joystick.Joystick(x)
for x in range(pygame.joystick.get_count())
]
for joystick in self.joysticks:
joystick.init()
# Create an 800x600 sized screen
#screen_size=[800,600]
screen_size = [self.field_width, int(self.field_height * 1.20)]
self.screen = pygame.display.set_mode(screen_size, pygame.RESIZABLE)
# Set the title of the window
pygame.display.set_caption('Test')
max_x = self.field_width
max_y = self.field_height
min_x = 0
min_y = 0
mid_x = max_x / 2.0
mid_y = max_y / 2.0
wall_thickness = 1 * in_
wall_1 = Wall(min_x,
min_y,
width=self.field_width,
height=wall_thickness,
color=BLACK)
wall_2 = Wall(min_x,
max_y,
width=self.field_width,
height=wall_thickness,
color=BLACK)
wall_3 = Wall(min_x,
min_y,
width=wall_thickness,
height=self.field_height,
color=BLACK)
wall_4 = Wall(max_x,
min_y,
width=wall_thickness,
height=self.field_height,
color=BLACK)
############################################
# Robot starting points
#
blue_x = min_x + 5 * ft_
blue_y1 = mid_y
blue_y2 = blue_y1 + 5 * ft_
blue_y3 = blue_y1 - 5 * ft_
red_x = max_x - 5 * ft_
red_y1 = blue_y1
red_y2 = blue_y2
red_y3 = blue_y3
# Create the robot object
#
#
#
self.robot1 = Robot(x=blue_x,
y=blue_y1,
color=BLUE1,
angle=-90,
keymap=key_map_1,
joystick=joystick_1,
is_mecanum=True,
mecanum_control_is_in_field_frame=False,
team_name=5115,
width=27 * in_,
length=45 * in_)
self.robot2 = Robot(x=blue_x,
y=blue_y2,
color=BLUE2,
angle=0,
keymap=key_map_2,
joystick=joystick_2,
is_mecanum=False,
team_name=493,
width=27 * in_,
length=55 * in_)
self.robot3 = Robot(x=blue_x,
y=blue_y3,
color=BLUE3,
angle=180,
keymap=key_map_3,
joystick=joystick_3,
is_mecanum=False,
team_name=503,
width=45 * in_,
length=45 * in_)
self.robot4 = Robot(x=red_x,
y=red_y1,
color=RED1,
angle=90,
keymap=key_map_4,
joystick=joystick_4,
is_mecanum=True,
team_name=3361,
width=27 * in_,
length=45 * in_)
self.robot5 = Robot(x=red_x,
y=red_y2,
color=RED2,
angle=90,
keymap=key_map_5,
joystick=joystick_5,
is_mecanum=False,
team_name=3258,
width=27 * in_,
length=45 * in_)
self.robot6 = Robot(x=red_x,
y=red_y3,
color=RED3,
angle=90,
keymap=key_map_6,
joystick=joystick_6,
is_mecanum=False,
team_name=2106,
width=27 * in_,
length=45 * in_)
# self.all_sprites_list = pygame.sprite.Group()
self.all_sprites_list = pygame.sprite.OrderedUpdates()
self.all_sprites_list.add(wall_1)
self.all_sprites_list.add(wall_2)
self.all_sprites_list.add(wall_3)
self.all_sprites_list.add(wall_4)
self.all_sprites_list.add(self.robot1)
self.all_sprites_list.add(self.robot2)
self.all_sprites_list.add(self.robot3)
self.all_sprites_list.add(self.robot4)
self.all_sprites_list.add(self.robot5)
self.all_sprites_list.add(self.robot6)
self.solid_sprites_list = pygame.sprite.Group()
# self.solid_sprites_list.add(wall_1)
# self.solid_sprites_list.add(wall_2)
# self.solid_sprites_list.add(wall_3)
# self.solid_sprites_list.add(wall_4)
# self.solid_sprites_list.add(self.robot1)
# self.solid_sprites_list.add(self.robot2)
# self.solid_sprites_list.add(self.robot3)
# self.solid_sprites_list.add(self.robot4)
# self.solid_sprites_list.add(self.robot5)
# self.solid_sprites_list.add(self.robot6)
self.robots_list = pygame.sprite.Group()
self.robots_list.add(self.robot1)
self.robots_list.add(self.robot2)
self.robots_list.add(self.robot3)
self.robots_list.add(self.robot4)
self.robots_list.add(self.robot5)
self.robots_list.add(self.robot6)
self.clock = pygame.time.Clock()
from qr import QR_class from acq_camera import VideoCam from tracker import Tracker_class from robot import Robot # Creo oggetti CAM = VideoCam() QR_OBJ = QR_class() TRACK_OBJ = Tracker_class() ROBOT_OBJ = Robot() #inizializzo QR_OBJ.set_CAM_obj(CAM) TRACK_OBJ.set_CAM_obj(CAM) TRACK_OBJ.set_QR_obj(QR_OBJ) ROBOT_OBJ.set_obj_cam (CAM) # Avvio thread CAM.start() QR_OBJ.start() TRACK_OBJ.start() ROBOT_OBJ.start() #Robot_def = Robot() # Inizializzo oggetti #self.Tracker_obj.set_obj(Robot_def) # Avvio i Thread # Avvio la macchina principale
