class TestMotor(unittest.TestCase):
get_input('Attach a motor on port A then continue')
def __init__(self, *args, **kwargs):
super(TestMotor, self).__init__(*args, **kwargs)
self.d = Motor(port=Motor.PORT.A)
def setUp(self):
self.d.reset()
def test_run(self):
self.d.run_mode = 'forever'
self.d.regulation_mode = True
self.d.pulses_per_second_sp = 200
self.d.start()
time.sleep(5)
self.d.stop()
def test_run_forever(self):
self.d.run_forever(50, regulation_mode=False)
time.sleep(5)
self.d.stop()
self.d.run_forever(200, regulation_mode=True)
time.sleep(5)
self.d.stop()
def test_run_time_limited(self):
self.d.run_time_limited(time_sp=10000,
speed_sp=80,
regulation_mode=False,
stop_mode=Motor.STOP_MODE.COAST,
ramp_up_sp=1000,
ramp_down_sp=1000)
time.sleep(12)
def test_run_position_limited(self):
self.d.position = 0
self.d.run_position_limited(position_sp=360,
speed_sp=800,
stop_mode=Motor.STOP_MODE.BRAKE,
ramp_up_sp=1000,
ramp_down_sp=1000)
time.sleep(5)
def test_run_position_limited_relative(self):
self.d.position_mode = Motor.POSITION_MODE.RELATIVE
self.d.run_position_limited(position_sp=160,
speed_sp=800,
stop_mode=Motor.STOP_MODE.BRAKE,
ramp_up_sp=1000,
ramp_down_sp=1000)
time.sleep(5)
class TestMotor(unittest.TestCase):
get_input('Attach a motor on port A then continue')
def __init__(self,*args, **kwargs):
super(TestMotor, self).__init__(*args, **kwargs)
self.d=Motor(port=Motor.PORT.A)
def setUp(self):
self.d.reset()
def test_run(self):
self.d.run_mode = 'forever'
self.d.regulation_mode = True
self.d.pulses_per_second_sp = 200
self.d.start()
time.sleep(5)
self.d.stop()
def test_run_forever(self):
self.d.run_forever(50, regulation_mode=False)
time.sleep(5)
self.d.stop()
self.d.run_forever(200, regulation_mode=True)
time.sleep(5)
self.d.stop()
def test_run_time_limited(self):
self.d.run_time_limited(time_sp=10000, speed_sp=80, regulation_mode=False,
stop_mode=Motor.STOP_MODE.COAST, ramp_up_sp=1000, ramp_down_sp=1000)
time.sleep(12)
def test_run_position_limited(self):
self.d.position=0
self.d.run_position_limited(position_sp=360, speed_sp=800,
stop_mode=Motor.STOP_MODE.BRAKE , ramp_up_sp=1000, ramp_down_sp=1000)
time.sleep(5)
def test_run_position_limited_relative (self):
self.d.position_mode=Motor.POSITION_MODE.RELATIVE
self.d.run_position_limited(position_sp=160, speed_sp=800,
stop_mode=Motor.STOP_MODE.BRAKE , ramp_up_sp=1000, ramp_down_sp=1000)
time.sleep(5)
class Ev3_Motor_Server:
def __init__(self):
self.PORT_NUMBER = 5004
self.DATA_SIZE = 1024
self.DEFAULT_PULSES_PER_SECOND = 0
self.DEFAULT_HOLD_MODE = "on"
self.DEFAULT_STOP_MODE = "brake"
self.DEFAULT_REGULATION_MODE = True
self.MOTOR_STARTING_POSITION = 0
self.system_on = False
self.YAW_LEFT = "yaw_left "
self.YAW_RIGHT = "yaw_right"
self.socket = None
self.motor_lock = threading.Lock()
self.unpacker = struct.Struct('f f')
self.right_track_motor = Motor(port=Motor.PORT.A)
self.left_track_motor = Motor(port=Motor.PORT.B)
#self.servo_arm_pitch_motor=Motor(port=Motor.PORT.C)
#self.servo_arm_roll_motor=Motor(port=Motor.PORT.D)
self.camera_fov = 640
self.camera_av = self.camera_fov / 2
self.face_x = 0
self.face_y = 0
self.kp = 0.15625 # Constant for the proportional controller
self.ki = 0 # Constant for the integral controller
self.kd = 0 # Constant for the differential controller
self.offset = self.camera_av # Offtset used to calculate error
self.tp = 0 # Turning power of motors when there is no error
self.previous_dt = 0
self.integral = 0
self.previous_err = 0
self.derivative = 0
self.coordinates_time_recv = 0
self.timeout = 0.2 #seconds
self.face_coordinates_lock = threading.Lock()
self.system_on_lock = threading.Lock()
self.update_coordinates_thread = threading.Thread(
target=self.update_face_coordinates)
self.update_coordinates_thread.start()
print "Setting up connection.\n"
self.initialise_connection()
print "Initialising Motors..\n"
self.initialise_motors()
print "Awaiting new connection...\n"
self.update_face_coordinates()
def initialise_connection(self):
hostname = socket.gethostbyname('0.0.0.0')
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.bind((hostname, self.PORT_NUMBER))
self.socket.listen(1)
def initialise_motors(self):
self.set_up_motor(self.right_track_motor)
self.set_up_motor(self.left_track_motor)
#set_up_motor(self.servo_arm_pitch_motor)
#set_up_motor(self.servo_arm_roll_motor)
def set_up_motor(self, motor):
motor.reset()
motor.position = self.MOTOR_STARTING_POSITION
motor.regulation_mode = self.DEFAULT_REGULATION_MODE
motor.pulses_per_second_sp = self.DEFAULT_PULSES_PER_SECOND
motor.hold_mode = self.DEFAULT_HOLD_MODE
motor.stop_mode = self.DEFAULT_STOP_MODE
def current_time(self):
return time.time() / 1000 # In seconds
def set_face_coordinates(self, x, y):
with self.face_coordinates_lock:
self.x = x
self.y = y
def get_face_coordinates(self):
return self.x, self.y
def set_system_on(self, state):
with self.system_on_lock:
self.system_on = state
def get_system_on(self):
return self.system_on
def calculate_x_axis_pid(self):
print "time 1" + str(self.current_time())
face_x, face_y = self.get_face_coordinates()
dt = self.current_time() - self.previous_dt
error = face_x - self.offset
self.integral = self.integral + (error * dt)
self.derivative = (error - self.previous_err) / dt
turning_power = (self.kp * error) + (self.ki * self.integral) + (
self.kd * self.derivative)
left_motor_power = self.tp + turning_power # These depend on the inital
right_motor_power = self.tp - turning_power # orientation of the motors
print str(turning_power)
self.right_track_motor.pulses_per_second_sp = right_motor_power
self.left_track_motor.pulses_per_second_sp = left_motor_power
self.previous_dt = dt
self.previous_err = error
print "time 2" + str(self.current_time())
def stop_x_axis_motors(self):
self.right_track_motor.stop()
self.left_track_motor.stop()
def start_x_axis_motors(self):
self.right_track_motor.start()
self.left_track_motor.start()
def motor_control(self):
self.previous_dt = self.current_time()
try:
if self.coordinates_time_recv > 0:
self.calculate_x_axis_pid()
self.start_x_axis_motors()
else:
self.previous_dt = time.time()
self.stop_x_axis_motors()
except (KeyboardInterrupt, SystemExit):
#self.cleanup()
raise
def update_face_coordinates(self):
conn, addr = self.socket.accept()
print conn, addr
self.coordinates_time_recv = -1
print "Connection address: ", addr
self.set_system_on(True)
while self.get_system_on():
try:
data = conn.recv(self.unpacker.size) #data size
if not data:
if self.current_time() > (
self.coordinates_time_recv * self.timeout) and (
self.coordinates_time_recv > -1):
self.coordinates_time_recv = -1
raise CoordinatesTimeOut()
else:
face_x, face_y = self.unpacker.unpack(data)
print face_x, face_y
self.coordinates_time_recv = self.current_time()
self.set_face_coordinates(face_x, face_y)
print "Face coordinates recieved at: " + str(
self.coordinates_time_recv) + "s, Position: (" + str(
face_x) + "," + str(face_y) + ")\n"
self.motor_control()
except CoordinatesTimeOut, e:
self.coordinates_time_recv = -1
self.stop_x_axis_motors()
print "ERROR: No new coordinates recieved!\nThe current coordinates are no longer valid, stopping motors!\n" + e
except KeyboardInterrupt, SystemExit:
self.cleanup()
conn.close()
class Mover():
# Moving motors
left=None;
right=None;
# Sensors
gyro=None;
# Paras
Rrio=2.15; # 60/28=2.14285
# Configs
angle=240;
rmp_sp=200;
run_sp=500; # running speed
t_sp=400; # turning speed
def __init__(self):
self.left=Motor(port=Motor.PORT.B);
self.right=Motor(port=Motor.PORT.C);
self.left.reset();
self.right.reset();
self.gyro=GyroSensor();
def readAngle(self,RESET=0):
if(RESET==1):
self.gyro.rate;
return self.gyro.ang;
def reset(self):
self.left.reset();
self.right.reset();
def runForward(self):
self.reset();
self.left.setup_forever(self.run_sp,speed_regulation = True);
self.right.setup_forever(self.run_sp,speed_regulation = True);
self.left.start();
self.right.start();
def runBackward(self):
self.reset();
self.left.setup_forever(-self.run_sp,speed_regulation = True);
self.right.setup_forever(-self.run_sp,speed_regulation = True);
self.left.start();
self.right.start();
def stop(self):
self.left.stop();
self.right.stop();
def turnRightbyAngle(self, ang):
angtruth = 0;
angtmp = ang;
# adjust loop
while (abs(angtmp)>3):
angtruth=self.oneAngleTurnRight(angtmp);
angtmp=angtmp-angtruth;
#print "Diff: "+str(angtmp);
def oneAngleTurnRight(self, ang):
before=self.readAngle(RESET=1);
#print "Before right turn: "+str(before);
self.reset();
self.left.position=0;
self.left.setup_position_limited(position_sp=int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp);
self.right.position=0;
self.right.setup_position_limited(position_sp=-int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp);
self.left.start();
self.right.start();
time.sleep(2);
self.stop();
after=self.readAngle();
#print "After right turn: "+str(after);
return after - before;
def turnLeftbyAngle(self, ang):
angtruth = 0;
angtmp = ang;
# adjust loop
while (abs(angtmp)>3):
angtruth=self.oneAngleTurnLeft(angtmp);
angtmp=angtmp+angtruth;
print "Diff: "+str(angtmp);
def oneAngleTurnLeft(self, ang):
before=self.readAngle(RESET=1);
print "Before right turn: "+str(before);
self.reset();
self.right.position=0;
self.right.setup_position_limited(position_sp=int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp);
self.left.position=0;
self.left.setup_position_limited(position_sp=-int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp);
self.right.start();
self.left.start();
time.sleep(2);
self.stop();
after=self.readAngle();
print "After right turn: "+str(after);
return after - before;
class Mover():
# Moving motors
left = None
right = None
# Sensors
gyro = None
# Paras
Rrio = 2.15
# 60/28=2.14285
# Configs
angle = 240
rmp_sp = 200
run_sp = 500
# running speed
t_sp = 400
# turning speed
def __init__(self):
self.left = Motor(port=Motor.PORT.B)
self.right = Motor(port=Motor.PORT.C)
self.left.reset()
self.right.reset()
self.gyro = GyroSensor()
def readAngle(self, RESET=0):
if (RESET == 1):
self.gyro.rate
return self.gyro.ang
def reset(self):
self.left.reset()
self.right.reset()
def runForward(self):
self.reset()
self.left.setup_forever(self.run_sp, speed_regulation=True)
self.right.setup_forever(self.run_sp, speed_regulation=True)
self.left.start()
self.right.start()
def runBackward(self):
self.reset()
self.left.setup_forever(-self.run_sp, speed_regulation=True)
self.right.setup_forever(-self.run_sp, speed_regulation=True)
self.left.start()
self.right.start()
def stop(self):
self.left.stop()
self.right.stop()
def turnRightbyAngle(self, ang):
angtruth = 0
angtmp = ang
# adjust loop
while (abs(angtmp) > 3):
angtruth = self.oneAngleTurnRight(angtmp)
angtmp = angtmp - angtruth
#print "Diff: "+str(angtmp);
def oneAngleTurnRight(self, ang):
before = self.readAngle(RESET=1)
#print "Before right turn: "+str(before);
self.reset()
self.left.position = 0
self.left.setup_position_limited(position_sp=int(ang * self.Rrio),
speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE,
ramp_up_sp=self.rmp_sp,
ramp_down_sp=self.rmp_sp)
self.right.position = 0
self.right.setup_position_limited(position_sp=-int(ang * self.Rrio),
speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE,
ramp_up_sp=self.rmp_sp,
ramp_down_sp=self.rmp_sp)
self.left.start()
self.right.start()
time.sleep(2)
self.stop()
after = self.readAngle()
#print "After right turn: "+str(after);
return after - before
def turnLeftbyAngle(self, ang):
angtruth = 0
angtmp = ang
# adjust loop
while (abs(angtmp) > 3):
angtruth = self.oneAngleTurnLeft(angtmp)
angtmp = angtmp + angtruth
print "Diff: " + str(angtmp)
def oneAngleTurnLeft(self, ang):
before = self.readAngle(RESET=1)
print "Before right turn: " + str(before)
self.reset()
self.right.position = 0
self.right.setup_position_limited(position_sp=int(ang * self.Rrio),
speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE,
ramp_up_sp=self.rmp_sp,
ramp_down_sp=self.rmp_sp)
self.left.position = 0
self.left.setup_position_limited(position_sp=-int(ang * self.Rrio),
speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE,
ramp_up_sp=self.rmp_sp,
ramp_down_sp=self.rmp_sp)
self.right.start()
self.left.start()
time.sleep(2)
self.stop()
after = self.readAngle()
print "After right turn: " + str(after)
return after - before
class Driver():
# Wheel motors
left=None
right=None
# Sensors
gyro=None
# Paras
Rrio=2.15 # R_robot/R_wheel: 60/28=2.14285
Wper=175.93 # perimeter of the powering wheels: 175.92919 mm
# Configs
angle=240
rmp_sp=200
run_sp=500 # running speed
t_sp=400 # turning speed
t_accuracy=2 # turning accuracy for Gyro, in degree
r_accuracy=5 # running accuracy for Motor position, in degree
adjust_sp = [100, 50]
def __init__(self):
self.left=Motor(port=Motor.PORT.B)
self.right=Motor(port=Motor.PORT.C)
self.reset()
self.gyro=Gyro()
def reset(self):
self.left.reset()
self.right.reset()
def runForward(self):
self.reset()
self.left.setup_forever(self.run_sp,speed_regulation = True)
self.right.setup_forever(self.run_sp,speed_regulation = True)
self.left.start()
self.right.start()
def forwardbyDistance(self, dist): # distance must be in cm, and positive
if (dist == 0):
return
angle = int(abs(dist*10)/self.Wper*360)
self.forwardbyAngle(angle)
#self.forwardbySecond()
def forwardbyAngle(self, angle):
angtruth = [0, 0]
angtmp = [angle, angle]
# moving and adjusting loop
loop_counter = 0;
while(abs(angtmp[0])>=self.r_accuracy and abs(angtmp[1]) >=self.r_accuracy):
angtruth = self.oneAngleForward(angtmp[0], angtmp[1])
angtmp[0]=angtmp[0]-angtruth[0]
angtmp[1]=angtmp[1]-angtruth[1]
print "fore New angtmp: "+ str(angtmp)
def oneAngleForward(self, left_ang, right_ang):
# run the main movement
self.reset()
self.left.setup_position_limited(position_sp=left_ang, speed_sp=self.run_sp,
stop_mode=Motor.STOP_MODE.HOLD, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.right.setup_position_limited(position_sp=right_ang, speed_sp=self.run_sp,
stop_mode=Motor.STOP_MODE.HOLD, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.left.start()
self.right.start()
time.sleep(10) # need to figure out how much needed
self.stop()
# adjust using low speed mode
for v_adj in self.adjust_sp:
self.left.setup_position_limited(position_sp=left_ang, speed_sp=v_adj,
stop_mode=Motor.STOP_MODE.HOLD, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.right.setup_position_limited(position_sp=right_ang, speed_sp=v_adj,
stop_mode=Motor.STOP_MODE.HOLD, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.left.start()
self.right.start()
time.sleep(1)
return [self.left.position, self.right.position]
def forwardbySecond(self, sec):
self.reset();
self.left.setup_time_limited(time_sp=sec, speed_sp=self.run_sp, speed_regulation=True)
self.right.setup_time_limited(time_sp=sec, speed_sp=self.run_sp, speed_regulation=True)
self.left.start()
self.right.start()
time.sleep(sec)
self.stop()
def runBackward(self):
self.reset()
self.left.setup_forever(-self.run_sp,speed_regulation=True)
self.right.setup_forever(-self.run_sp,speed_regulation=True)
self.left.start()
self.right.start()
def backwardbyDistance(self, dist): # distance must be in cm, and positive
if (dist == 0):
return
angle = -int(abs(dist*10)/self.Wper*360)
self.backwardbyAngle(angle)
def backwardbyAngle(self, angle):
angtruth = [0, 0]
angtmp = [angle, angle]
# moving and adjusting loop
while(abs(angtmp[0])>=self.r_accuracy and abs(angtmp[1]) >=self.r_accuracy):
angtruth = self.oneAngleForward(angtmp[0], angtmp[1])
angtmp[0] = angtmp[0]+angtruth[0]
angtmp[1] = angtmp[1]+angtruth[1]
print "back New angtmp: "+ str(angtmp)
def oneAngleBackward(self, left_ang, right_ang):
self.reset()
self.left.setup_position_limited(position_sp=-left_ang, speed_sp=self.run_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.right.setup_position_limited(position_sp=-right_ang, speed_sp=self.run_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.left.start()
self.right.start()
time.sleep(10)
self.stop()
# adjust using low speed mode
for v_adj in self.adjust_sp:
self.left.setup_position_limited(position_sp=-left_ang, speed_sp=v_adj,
stop_mode=Motor.STOP_MODE.HOLD, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.right.setup_position_limited(position_sp=-right_ang, speed_sp=v_adj,
stop_mode=Motor.STOP_MODE.HOLD, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.left.start()
self.right.start()
time.sleep(1)
return [self.left.position, self.right.position]
def turnRightbyAngle(self, ang):
angtruth = 0
angtmp = ang
# moving and adjusting loop
while (abs(angtmp)>self.t_accuracy):
angtruth=self.oneAngleTurnRight(angtmp)
angtmp=angtmp-angtruth
#print "Diff: "+str(angtmp)
def oneAngleTurnRight(self, ang):
before=self.gyro.readAngle()
#print "Before right turn: "+str(before)
self.reset()
self.left.setup_position_limited(position_sp=int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.right.setup_position_limited(position_sp=-int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.left.start()
self.right.start()
time.sleep(2)
self.stop()
after=self.gyro.readAngle()
#print "After right turn: "+str(after)
return after - before
def turnLeftbyAngle(self, ang):
angtruth = 0
angtmp = ang
# moving and adjusting loop
while (abs(angtmp)>self.t_accuracy):
angtruth=self.oneAngleTurnLeft(angtmp)
angtmp=angtmp+angtruth
print "Diff: "+str(angtmp)
def oneAngleTurnLeft(self, ang):
before=self.gyro.readAngle()
#print "Before right turn: "+str(before)
self.reset()
self.right.setup_position_limited(position_sp=int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.left.setup_position_limited(position_sp=-int(ang*self.Rrio), speed_sp=self.t_sp,
stop_mode=Motor.STOP_MODE.BRAKE, ramp_up_sp=self.rmp_sp, ramp_down_sp=self.rmp_sp)
self.right.start()
self.left.start()
time.sleep(2)
self.stop()
after=self.gyro.readAngle()
#print "After right turn: "+str(after)
return after - before
def stop(self):
self.left.stop()
self.right.stop()
UDP_IP = "10.42.0.3"
UDP_PORT = 5005
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind((UDP_IP,UDP_PORT))
print "Robot awaiting commands..."
while True:
data, addr = sock.recvfrom(1024)
if(data=="right"):
leftTrackMotor.pulses_per_second_sp = 100
rightTrackMotor.pulses_per_second_sp = -100
leftTrackMotor.start()
rightTrackMotor.start()
elif(data=="left"):
leftTrackMotor.pulses_per_second_sp = -100
rightTrackMotor.pulses_per_second_sp = 100
leftTrackMotor.start()
rightTrackMotor.start()
elif(data=="pitchup"):
servoArmPitchMotor.pulses_per_second_sp = -100
servoArmPitchMotor.start()
elif(data=="pitchdown"):
servoArmPitchMotor.pulses_per_second_sp = 100
servoArmPitchMotor.start()
elif(data=="rollleft"):
servoArmRollMotor.pulses_per_second_sp = -60
servoArmRollMotor.start()
class Ev3_Motor_Server:
def __init__(self):
self.PORT_NUMBER = 5004
self.DATA_SIZE = 1024
self.DEFAULT_PULSES_PER_SECOND = 0
self.DEFAULT_HOLD_MODE = "on"
self.DEFAULT_STOP_MODE = "brake"
self.DEFAULT_REGULATION_MODE = True
self.MOTOR_STARTING_POSITION = 0
self.system_on = False
self.YAW_LEFT = "yaw_left "
self.YAW_RIGHT = "yaw_right"
self.socket = None
self.motor_lock = threading.Lock()
self.unpacker = struct.Struct('f f')
self.right_track_motor=Motor(port=Motor.PORT.A)
self.left_track_motor=Motor(port=Motor.PORT.B)
#self.servo_arm_pitch_motor=Motor(port=Motor.PORT.C)
#self.servo_arm_roll_motor=Motor(port=Motor.PORT.D)
self.camera_fov = 640
self.camera_av = self.camera_fov/2
self.face_x = 0
self.face_y = 0
self.kp = 0.15625 # Constant for the proportional controller
self.ki = 0 # Constant for the integral controller
self.kd = 0 # Constant for the differential controller
self.offset = self.camera_av # Offtset used to calculate error
self.tp = 0 # Turning power of motors when there is no error
self.previous_dt = 0
self.integral = 0
self.previous_err = 0
self.derivative = 0
self.coordinates_time_recv = 0
self.timeout = 0.2 #seconds
self.face_coordinates_lock = threading.Lock()
self.system_on_lock = threading.Lock()
self.update_coordinates_thread = threading.Thread(target = self.update_face_coordinates)
self.update_coordinates_thread.start()
print "Setting up connection.\n"
self.initialise_connection()
print "Initialising Motors..\n"
self.initialise_motors()
print "Awaiting new connection...\n"
self.update_face_coordinates()
def initialise_connection(self):
hostname = socket.gethostbyname( '0.0.0.0' )
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.bind((hostname, self.PORT_NUMBER))
self.socket.listen(1)
def initialise_motors(self):
self.set_up_motor(self.right_track_motor)
self.set_up_motor(self.left_track_motor)
#set_up_motor(self.servo_arm_pitch_motor)
#set_up_motor(self.servo_arm_roll_motor)
def set_up_motor(self, motor):
motor.reset()
motor.position = self.MOTOR_STARTING_POSITION
motor.regulation_mode = self.DEFAULT_REGULATION_MODE
motor.pulses_per_second_sp = self.DEFAULT_PULSES_PER_SECOND
motor.hold_mode = self.DEFAULT_HOLD_MODE
motor.stop_mode = self.DEFAULT_STOP_MODE
def current_time(self):
return time.time()/1000 # In seconds
def set_face_coordinates(self, x, y):
with self.face_coordinates_lock:
self.x = x
self.y = y
def get_face_coordinates(self):
return self.x, self.y
def set_system_on(self, state):
with self.system_on_lock:
self.system_on = state
def get_system_on(self):
return self.system_on
def calculate_x_axis_pid(self):
print "time 1" + str(self.current_time())
face_x, face_y = self.get_face_coordinates()
dt = self.current_time() - self.previous_dt
error = face_x - self.offset
self.integral = self.integral + (error*dt)
self.derivative = (error - self.previous_err)/dt
turning_power = (self.kp*error) + (self.ki*self.integral) + (self.kd*self.derivative)
left_motor_power = self.tp + turning_power # These depend on the inital
right_motor_power = self.tp - turning_power # orientation of the motors
print str(turning_power)
self.right_track_motor.pulses_per_second_sp = right_motor_power
self.left_track_motor.pulses_per_second_sp = left_motor_power
self.previous_dt = dt
self.previous_err = error
print "time 2" + str(self.current_time())
def stop_x_axis_motors(self):
self.right_track_motor.stop()
self.left_track_motor.stop()
def start_x_axis_motors(self):
self.right_track_motor.start()
self.left_track_motor.start()
def motor_control(self):
self.previous_dt = self.current_time()
try:
if self.coordinates_time_recv > 0:
self.calculate_x_axis_pid()
self.start_x_axis_motors()
else:
self.previous_dt = time.time()
self.stop_x_axis_motors()
except (KeyboardInterrupt, SystemExit):
#self.cleanup()
raise
def update_face_coordinates(self):
conn, addr = self.socket.accept()
print conn, addr
self.coordinates_time_recv = -1
print "Connection address: ", addr
self.set_system_on(True)
while self.get_system_on():
try:
data = conn.recv(self.unpacker.size) #data size
if not data:
if self.current_time() > (self.coordinates_time_recv*self.timeout) and (self.coordinates_time_recv > -1):
self.coordinates_time_recv = -1
raise CoordinatesTimeOut()
else:
face_x, face_y = self.unpacker.unpack(data)
print face_x, face_y
self.coordinates_time_recv = self.current_time()
self.set_face_coordinates(face_x, face_y)
print "Face coordinates recieved at: " + str(self.coordinates_time_recv) + "s, Position: (" + str(face_x) + "," + str(face_y) + ")\n"
self.motor_control()
except CoordinatesTimeOut, e:
self.coordinates_time_recv = -1
self.stop_x_axis_motors()
print "ERROR: No new coordinates recieved!\nThe current coordinates are no longer valid, stopping motors!\n" + e
except KeyboardInterrupt, SystemExit:
self.cleanup()
conn.close()
