1 files changed, 57 insertions, 0 deletions
diff --git a/controllers/final/setup.py b/controllers/final/setup.py
new file mode 100644
index 0000000..dc3a7b3
--- /dev/null
+++ b/controllers/final/setup.py
@@ -0,0 +1,57 @@
+"""
+All the robot devices as well as the blackboard are defined and initialised
+here for an important reason - imported modules in python are run only once.
+This means that there will be no redundant re-execution and all the variables
+will be accessible from the other files as necessary. Modifications to the
+blackboard will be visible from all files, which is the intended effect.
+"""
+from controller import Robot
+
+robot = Robot()
+timestep = int(robot.getBasicTimeStep())
+blackboard = {} # Defined as a dictionary as it is much simpler.
+
+# Setup the motors and sensors as dictionaries, with the relevant joint names
+# as keys.
+joints = [
+    "gripper_left_finger_joint", "gripper_right_finger_joint", "head_2_joint",
+    "arm_1_joint", "arm_2_joint", "arm_4_joint", "arm_7_joint",
+    "torso_lift_joint",
+]
+finger_sensor_names = {
+    "gripper_left_finger_joint": "gripper_left_sensor_finger_joint",
+    "gripper_right_finger_joint": "gripper_right_sensor_finger_joint",
+}
+sensors = {}
+motors = {}
+for joint in joints:
+    motors[joint] = robot.getDevice(joint)
+    # If the name is in finger_sensor_names, use it. Fallback to %s_sensor.
+    sensor = finger_sensor_names.get(joint, "%s_sensor" % joint)
+    sensor = robot.getDevice(sensor)
+    sensor.enable(timestep)
+    sensors[joint] = sensor
+
+# Enable force feedback for the grippers.
+for finger in finger_sensor_names:
+    motors[finger].enableForceFeedback(timestep)
+
+# Initialise the wheel motors.
+left_motor = robot.getDevice("wheel_left_joint")
+right_motor = robot.getDevice("wheel_right_joint")
+left_motor.setPosition(float("inf"))
+right_motor.setPosition(float("inf"))
+left_motor.setVelocity(.0)
+right_motor.setVelocity(.0)
+
+# Enable the gps and compass to gain better situational awareness.
+gps = robot.getDevice("gps")
+gps.enable(timestep)
+compass = robot.getDevice("compass")
+compass.enable(timestep)
+
+# Setup and enable the camera, including the recognition functionality to
+# obtain the jar locations.
+camera = robot.getDevice("Astra rgb")
+camera.enable(timestep)
+camera.recognitionEnable(timestep)