def main():
interface = brickpi.Interface()
#interface.startLogging('logfile.txt')
interface.initialize()
# Setup motors
motors = [0, 3]
speed = 5
interface.motorEnable(motors[0])
interface.motorEnable(motors[1])
motorParams = interface.MotorAngleControllerParameters()
motor_util.set_pid(motorParams)
interface.setMotorAngleControllerParameters(motors[0], motorParams)
interface.setMotorAngleControllerParameters(motors[1], motorParams)
# Setup initial state of particles
numberOfParticles = 100
particles = [((startPos, startPos, 0), 1 / float(numberOfParticles))
for i in range(numberOfParticles)]
# Waypoint navigation:
ms.drawParticles(particles)
while (True):
w_x = float(input("Enter your desired Wx position: "))
w_y = float(input("Enter your desired Wy position: "))
ms.navigateToWaypoint(w_x + startPos, w_y + startPos, particles,
interface, motors)
ms.drawParticles(particles)
print "Destination reached!"
#interface.stopLogging('logfile.txt')
interface.terminate()
def __init__(self):
self.motors = [0, 1]
self.sensorPos = 0.0
self.interface = brickpi.Interface()
self.interface.initialize()
setupMotors(self.interface, self.motors)
self.interface.sensorEnable(2, brickpi.SensorType.SENSOR_ULTRASONIC)
def __init__(self):
self.x = 0.0
self.y = 0.0
self.theta = 0.0
self.particles = Particles()
# Set up interfaces for sonar and motors
self.interface = brickpi.Interface()
self.interface.initialize()
self.interface.sensorEnable(SON_PORT,brickpi.SensorType.SENSOR_ULTRASONIC);
self.interface.motorEnable(MOTORS[0])
self.interface.motorEnable(MOTORS[1])
# motor parameters
self.motorParams = self.interface.MotorAngleControllerParameters()
self.motorParams.maxRotationAcceleration = 6.0
self.motorParams.maxRotationSpeed = 12.0
self.motorParams.feedForwardGain = 255/20.0
self.motorParams.minPWM = 18.0
self.motorParams.pidParameters.minOutput = -255
self.motorParams.pidParameters.maxOutput = 255
#PID
self.motorParams.pidParameters.k_p = 270.0
self.motorParams.pidParameters.k_i = 300.0
self.motorParams.pidParameters.k_d = 13.5
self.interface.setMotorAngleControllerParameters(MOTORS[0],self.motorParams)
self.interface.setMotorAngleControllerParameters(MOTORS[1],self.motorParams)
def main(ku, motor=0, distance=20, timeout=5):
interface = brickpi.Interface()
interface.initialize()
motors = [0, 1]
interface.motorEnable(motors[0])
interface.motorEnable(motors[1])
motorParams = []
motor0_ku = ku if motor == 0 else 0
motor1_ku = ku if motor == 1 else 0
# set params for motor 0 (left)
motorParams.append(interface.MotorAngleControllerParameters())
motorParams[0].maxRotationAcceleration = 6.0
motorParams[0].maxRotationSpeed = 12.0
motorParams[0].feedForwardGain = 255 / 20.0
motorParams[0].minPWM = 18.0
motorParams[0].pidParameters.minOutput = -255
motorParams[0].pidParameters.maxOutput = 255
motorParams[0].pidParameters.k_p = motor0_ku
motorParams[0].pidParameters.k_i = 0
motorParams[0].pidParameters.k_d = 0
# set params for motor 1 (right)
motorParams.append(interface.MotorAngleControllerParameters())
motorParams[1].maxRotationAcceleration = 6.0
motorParams[1].maxRotationSpeed = 12.0
motorParams[1].feedForwardGain = 255 / 20.0
motorParams[1].minPWM = 18.0
motorParams[1].pidParameters.minOutput = -255
motorParams[1].pidParameters.maxOutput = 255
motorParams[1].pidParameters.k_p = motor1_ku
motorParams[1].pidParameters.k_i = 0
motorParams[1].pidParameters.k_d = 0
interface.setMotorAngleControllerParameters(motors[0], motorParams[0])
interface.setMotorAngleControllerParameters(motors[1], motorParams[1])
logfile_name = str(ku) + "c"
interface.startLogging("../Logfiles/" + logfile_name + ".log")
paramsFile = open('../Logfiles/Params/' + logfile_name + "_params.txt",
'w')
paramsFile.write(str(motor0_ku) + '\n' + str(motor1_ku))
paramsFile.close
values = [0, 0]
values[motor] = distance
interface.increaseMotorAngleReferences(motors, values)
while not (interface.motorAngleReferencesReached(motors) or timeout < 0):
time.sleep(0.1)
timeout = timeout - 0.1
print "Test Complete."
interface.stopLogging()
interface.terminate()
def __init__(self, x, y, theta, draw=False, record=False):
self.interface = brickpi.Interface()
self.initialize()
self.motorEnable(robot.wheel_motors[0])
self.motorEnable(robot.wheel_motors[1])
self.motorEnable(robot.sonar_motor)
motorParams = self.interface.MotorAngleControllerParameters()
motorParams.maxRotationAcceleration = 7.0
motorParams.maxRotationSpeed = 12.0
motorParams.feedForwardGain = 255 / 20.0
motorParams.minPWM = 25
motorParams.pidParameters.minOutput = -255
motorParams.pidParameters.maxOutput = 255
# proportional gain, reduces error
motorParams.pidParameters.k_p = 270.0
# integral gain, removes steady_state error
motorParams.pidParameters.k_i = 400
# differential gain, reduce settling time
motorParams.pidParameters.k_d = 160
self.setMotorAngleControllerParameters(robot.wheel_motors[0], motorParams)
self.setMotorAngleControllerParameters(robot.wheel_motors[1], motorParams)
motorParams = self.interface.MotorAngleControllerParameters()
motorParams.maxRotationAcceleration = 2.0
motorParams.maxRotationSpeed = 3
motorParams.feedForwardGain = 255 / 20.0
motorParams.minPWM = 27
motorParams.pidParameters.minOutput = -255
motorParams.pidParameters.maxOutput = 255
# proportional gain, reduces error
motorParams.pidParameters.k_p = 270.0
# integral gain, removes steady_state error
motorParams.pidParameters.k_i = 200
# differential gain, reduce settling time
motorParams.pidParameters.k_d = 160
self.setMotorAngleControllerParameters(robot.sonar_motor, motorParams)
# initialise localisation
#self.loc = localisation(x,y,theta,draw, record)
# sonar rotation offset, 0 is facing forward, positive is left/anticlockwise
self.sonar_rotation_offset = 0 #initialise to 0
def __init__(self):
interface = brickpi.Interface()
interface.initialize()
self.ACCEPT_THRESHOLD = 0.25
self.WHEEL_RADIUS = 2.85
self.ROBOT_RADIUS = 5.5
self.ROBOT_LENGTH = 13.5
self.PERC_MOD_WHEEL = 1.025
self.PERC_MOD_BOOST = 1.15
self.ANGLE_BOOST = 1.141
self.walls = Map()
self.particles = [Particle() for i in range(Particle.num_particles)
] # (x, y, theta)
self.sigma = 0.0125
self.error_theta = 0.5 * math.pi / 180
self.x = 0.0
self.y = 0.0
self.theta = 0.0
self.sonar_sigma = 0.4
self.interface = interface
self.motorParams = params.setup(interface)
self.motors = [
params.MOTOR_LEFT, params.MOTOR_RIGHT, params.MOTOR_HEAD
]
for motor in self.motors:
interface.motorEnable(motor)
interface.setMotorAngleControllerParameters(
motor, self.motorParams[motor])
interface.sensorEnable(params.PORT_LEFT_TOUCH,
brickpi.SensorType.SENSOR_TOUCH)
interface.sensorEnable(params.PORT_RIGHT_TOUCH,
brickpi.SensorType.SENSOR_TOUCH)
interface.sensorEnable(params.PORT_ULTRASONIC,
brickpi.SensorType.SENSOR_ULTRASONIC)
self.HEAD_ANGLE_CENTRE = self.interface.getMotorAngle(
params.MOTOR_HEAD)[0]
self.HEAD_ANGLE_LEFT = self.HEAD_ANGLE_CENTRE - math.pi / 2
self.HEAD_ANGLE_RIGHT = self.HEAD_ANGLE_CENTRE + math.pi / 2
self.HEAD_STEP = 30 * math.pi / 180
self.HEAD_WAIT = 0.25
def __init__(self, motors, wheelradius, robotwidth):
#Initialization of robot Interface
self.interface = brickpi.Interface()
self.interface.initialize()
self.wheelradius = wheelradius
self.robotwidth = robotwidth
self.motors = motors
self.interface.motorEnable(motors[0])
self.interface.motorEnable(motors[1])
#TOUCH SENSOR
self.touch_port = [1, 3] #port 1 = left, port 3 = right
self.interface.sensorEnable(self.touch_port[0], brickpi.SensorType.SENSOR_TOUCH)
self.interface.sensorEnable(self.touch_port[1], brickpi.SensorType.SENSOR_TOUCH)
#PID's Parameters motor 0
self.motor0Params = self.interface.MotorAngleControllerParameters()
self.motor0Params.maxRotationAcceleration = 6.0
self.motor0Params.maxRotationSpeed = 12.0
self.motor0Params.feedForwardGain = 255/20.0
self.motor0Params.minPWM = 18.0
self.motor0Params.pidParameters.minOutput = -255
self.motor0Params.pidParameters.maxOutput = 255
self.motor0Params.pidParameters.k_p = 700.0 #250.0
self.motor0Params.pidParameters.k_i = 950.0 #200.0
self.motor0Params.pidParameters.K_d = 20.0 #100.0
self.interface.setMotorAngleControllerParameters(motors[0],self.motor0Params)
#PID's Parameters motor 1
self.motor1Params = self.interface.MotorAngleControllerParameters()
self.motor1Params.maxRotationAcceleration = 6.0
self.motor1Params.maxRotationSpeed = 12.0
self.motor1Params.feedForwardGain = 255/20.0
self.motor1Params.minPWM = 18.0
self.motor1Params.pidParameters.minOutput = -255
self.motor1Params.pidParameters.maxOutput = 255
self.motor1Params.pidParameters.k_p = 700.0 #250.0
self.motor1Params.pidParameters.k_i = 950.0 #200.0
self.motor1Params.pidParameters.K_d = 20.0 #100.0
self.interface.setMotorAngleControllerParameters(motors[1],self.motor1Params)
def __init__(self, use_spinning_sonar=False):
self.interface = brickpi.Interface()
self.interface.initialize()
self.motors = {'R': {'port': Robot.MD, 'K_u': 750.0, 'P_u': 0.25, 'instance': None},
'L': {'port': Robot.MA, 'K_u': 750.0, 'P_u': 0.25, 'instance': None}}
for _, motor in self.motors.items():
motor['instance'] = Motor(self, motor)
# setup bumper
self.right_bumper = Bumper(self, Robot.S1)
self.left_bumper = Bumper(self, Robot.S4)
print('Bumpers: [{}, {}]'.format(self.left_bumper, self.right_bumper))
# setup sonar
if use_spinning_sonar:
self.sonar = SpinningSonar(self, Robot.S3, Robot.MC)
else:
self.sonar = Sonar(self, Robot.S3)
print('Sonar: {}'.format(self.sonar))
def main():
interface = brickpi.Interface()
#interface.startLogging('logfile.txt')
interface.initialize()
# Setup motors
motors = [0, 3]
speed = 5
interface.motorEnable(motors[0])
interface.motorEnable(motors[1])
motorParams = interface.MotorAngleControllerParameters()
motor_util.set_pid(motorParams)
interface.setMotorAngleControllerParameters(motors[0], motorParams)
interface.setMotorAngleControllerParameters(motors[1], motorParams)
# Setup initial state of particles
numberOfParticles = 100
particles = [((startPos, startPos, 0), 1 / float(numberOfParticles))
for i in range(numberOfParticles)]
# Go in squares
for i in range(4):
for j in range(4):
motor_util.forward(10, interface, motors)
drawParticles(particles)
time.sleep(0.25)
particles = updateParticlesForward(particles, 20)
drawParticles(particles)
time.sleep(0.25)
motor_util.rotateRight90deg(interface, motors)
particles = updateParticlesRotate(particles, -math.pi / 2)
drawParticles(particles)
time.sleep(0.25)
print "Destination reached!"
#interface.stopLogging('logfile.txt')
interface.terminate()
import brickpi import time interface = brickpi.Interface() interface.initialize() motors = [0, 1] interface.motorEnable(motors[0]) interface.motorEnable(motors[1]) motorParams = interface.MotorAngleControllerParameters() motorParams.maxRotationAcceleration = 6.0 motorParams.maxRotationSpeed = 12.0 motorParams.feedForwardGain = 255 / 20.0 motorParams.minPWM = 18.0 motorParams.pidParameters.minOutput = -255 motorParams.pidParameters.maxOutput = 255 motorParams.pidParameters.k_p = 340 motorParams.pidParameters.k_i = 300 motorParams.pidParameters.k_d = 300 motorParamsRight = motorParams motorParamsLeft = motorParams interface.setMotorAngleControllerParameters(motors[0], motorParamsLeft) interface.setMotorAngleControllerParameters(motors[1], motorParamsRight) THRESHOLD = 1.05
def main(ku,
pu,
percentage_modifier_kd=1,
percentage_modifier_ki=1,
motor=0,
distance=20,
timeout=5):
interface = brickpi.Interface()
interface.initialize()
motors = [0, 1]
interface.motorEnable(motors[0])
interface.motorEnable(motors[1])
motorParams = []
motor0_ku = ku if motor == 0 else 0
motor1_ku = ku if motor == 1 else 0
# set params for motor 0 (left)
motorParams.append(interface.MotorAngleControllerParameters())
motorParams[0].maxRotationAcceleration = 6.0
motorParams[0].maxRotationSpeed = 12.0
motorParams[0].feedForwardGain = 255 / 20.0
motorParams[0].minPWM = 18.0
motorParams[0].pidParameters.minOutput = -255
motorParams[0].pidParameters.maxOutput = 255
motorParams[0].pidParameters.k_p = 0.6 * motor0_ku
motorParams[
0].pidParameters.k_i = percentage_modifier_ki * 2 * 0.6 * motor0_ku / pu
motorParams[
0].pidParameters.k_d = percentage_modifier_kd * 0.6 * motor0_ku * pu / 8
# set params for motor 1 (right)
motorParams.append(interface.MotorAngleControllerParameters())
motorParams[1].maxRotationAcceleration = 6.0
motorParams[1].maxRotationSpeed = 12.0
motorParams[1].feedForwardGain = 255 / 20.0
motorParams[1].minPWM = 18.0
motorParams[1].pidParameters.minOutput = -255
motorParams[1].pidParameters.maxOutput = 255
motorParams[1].pidParameters.k_p = 0.6 * motor1_ku
motorParams[
1].pidParameters.k_i = percentage_modifier_ki * 2 * 0.6 * motor1_ku / pu
motorParams[
1].pidParameters.k_d = percentage_modifier_kd * 0.6 * motor1_ku * pu / 8
interface.setMotorAngleControllerParameters(motors[0], motorParams[0])
interface.setMotorAngleControllerParameters(motors[1], motorParams[1])
logfile_name = "KI_" + str(percentage_modifier_ki) + "KD_" + str(
percentage_modifier_kd) + "_MOTOR_" + str(motor) + "_DISTANCE_" + str(
distance)
interface.startLogging("../Logfiles/" + logfile_name + ".log")
values = [0, 0]
values[motor] = distance
interface.increaseMotorAngleReferences(motors, values)
while not (interface.motorAngleReferencesReached(motors) or timeout < 0):
time.sleep(0.1)
timeout = timeout - 0.1
print "Test Complete."
interface.stopLogging()
interface.terminate()
def __init__(self):
self.interface = brickpi.Interface()
self.interface.initialize()
self.sonar = rocommon.Sonar(self, 2)
print('Sonar: {}'.format(self.sonar))
def __init__(self):
self.interface = brickpi.Interface()
self.interface.initialize()
#0 and 1 are for wheels, 3 is for sonar sensor on the top
self.motors = [0,1,3]
self.angle_ratio_left = 1.1 / 90
self.angle_ratio_right = 1.0 / 90
self.sensor_angle_ratio = 0.53 / 90
self.move_ratio = 0.3
#current status (positon, angle)
self.global_x = 0
self.global_y = 0
self.global_theta = 0
self.global_sensor_theta = 0
self.interface.motorEnable(self.motors[0])
self.interface.motorEnable(self.motors[1])
self.interface.motorEnable(self.motors[2])
#left wheel
motorParams0 = self.interface.MotorAngleControllerParameters()
motorParams0.maxRotationAcceleration = 6.0
motorParams0.maxRotationSpeed = 12.0
motorParams0.feedForwardGain = 255/20.0 #319/20
motorParams0.minPWM = 18.0
motorParams0.pidParameters.minOutput = -255
motorParams0.pidParameters.maxOutput = 255
motorParams0.pidParameters.k_p = 675
motorParams0.pidParameters.k_i = 400
motorParams0.pidParameters.k_d = 200
#right wheel
motorParams1 = self.interface.MotorAngleControllerParameters()
motorParams1.maxRotationAcceleration = 6.0
motorParams1.maxRotationSpeed = 16.3
motorParams1.feedForwardGain = 300/20.0
motorParams1.minPWM = 18.0
motorParams1.pidParameters.minOutput = -255
motorParams1.pidParameters.maxOutput = 255
motorParams1.pidParameters.k_p = 900
motorParams1.pidParameters.k_i = 400
motorParams1.pidParameters.k_d = 200
#sonar sensor on top
sensorParam = self.interface.MotorAngleControllerParameters()
sensorParam.maxRotationAcceleration = 3.0
sensorParam.maxRotationSpeed = 7.5
sensorParam.feedForwardGain = 255/20.0
sensorParam.minPWM = 18.0
sensorParam.pidParameters.minOutput = -255
sensorParam.pidParameters.maxOutput = 255
sensorParam.pidParameters.k_p = 300
sensorParam.pidParameters.k_i = 200
sensorParam.pidParameters.k_d = 100
self.interface.setMotorAngleControllerParameters(self.motors[0],motorParams0)
self.interface.setMotorAngleControllerParameters(self.motors[1],motorParams1)
self.interface.setMotorAngleControllerParameters(self.motors[2],sensorParam)
self.sensor_port = 3
self.interface.sensorEnable(self.sensor_port, brickpi.SensorType.SENSOR_ULTRASONIC)
'''
