params.addParam('DIST_PER_MOVE', 10.0, 'Centimeters in 1 move unit')
params.addParam('DEG_PER_TURN', 2*math.pi, 'Radians in 1 turn unit')
params.addParam('MOTOR_DEFAULT_PWM_DC', 30, 'Default duty cycle for the motor PWM (0 - 100)')
params.addParam('MOTOR_DEFAULT_PWM_FREQ', 357, 'Default frequency for the motor PWM')
# Create Sensor object
sensors = Sensor()
# Add sensors to sensor dictionary
# 'Name', number, weight (higher is more important), location
sensors.addSensor('HC-SR04', 1, 2, [0, 8.5, 0, math.pi / 2])
sensors.addSensor('HC-SR04', 2, 2, [7.36, 4.25, 0, math.pi / 6])
sensors.addSensor('HC-SR04', 3, 2, [7.36, -4.25, 0, -math.pi / 6])
sensors.addSensor('HC-SR04', 4, 2, [0, -8.5, 0, -math.pi / 2])
sensors.addSensor('HC-SR04', 5, 2, [-7.36, -4.25, 0, -5 * (math.pi / 6)])
sensors.addSensor('HC-SR04', 6, 2, [-7.36, 4.25, 0, 5 * (math.pi / 6)])
sensors.addSensor('TSL2561', 1, 1, [7.36, 4.25, 0, math.pi / 6])
sensors.addSensor('TSL2561', 2, 1, [0, -8.5, 0, -math.pi / 2])
sensors.addSensor('TSL2561', 3, 1, [-7.36, 4.25, 0, 5 * (math.pi / 6)])
# Create PinMaster object
pins = PinMaster()
# Add pins to pin dictionary
pins.addPin('MOTOR1A', 18) # 16)
pins.addPin('MOTOR1B', 23) # 20) # Right Motor
pins.addPin('MOTOR1E', 24) # 21)
pins.addPin('MOTOR2A', 17) # 13) # Left Motor
pins.addPin('MOTOR2B', 27) # 19)
pins.addPin('MOTOR2E', 22) # 26)
pins.addPin('BUZZER', 4) #25) # Piezo buzzer
# Add sensors to sensor dictionary
sensors.addSensor('HC-SR04', 1, 10, [0, 8.5, 0, math.pi / 2])
sensors.addSensor('HC-SR04', 2, 10, [7.36, 4.25, 0, math.pi / 6])
sensors.addSensor('HC-SR04', 3, 10, [7.36, -4.25, 0, -math.pi / 6])
sensors.addSensor('HC-SR04', 4, 10, [0, -8.5, 0, -math.pi / 2])
sensors.addSensor('HC-SR04', 5, 10, [-7.36, -4.25, 0, -5 * (math.pi / 6)])
sensors.addSensor('HC-SR04', 6, 10, [-7.36, 4.25, 0, 5 * (math.pi / 6)])
sensors.addSensor('TSL2561', 1, 5, [7.36, 4.25, 0, math.pi / 6])
sensors.addSensor('TSL2561', 2, 5, [0, -8.5, 0, -math.pi / 2])
sensors.addSensor('TSL2561', 3, 5, [-7.36, 4.25, 0, 5 * (math.pi / 6)])
# Create PinMaster object
pins = PinMaster()
# Add pins to pin dictionary
pins.addPin('MOTOR1A', 18) # 16)
pins.addPin('MOTOR1B', 23) # 20) # Right Motor
pins.addPin('MOTOR1E', 24) # 21)
pins.addPin('MOTOR2A', 17) # 13) # Left Motor
pins.addPin('MOTOR2B', 27) # 19)
pins.addPin('MOTOR2E', 22) # 26)
pins.addPin('SERVO1', 5) # Horizontal (Side to Side) servo
pins.addPin('SERVO2', 13) # Vertical (Up and Down) servo
pins.addPin('BUZZER', 4) #25) # Piezo buzzer
# # Create Map object
# mapa = Mapa()
def main():
'''