import ev3dev

target_position = 0
target_heading = 0
channel = 1
def get_value(ir_sensor):
c = (channel - 1) * 2
return (ir_sensor.value(c), ir_sensor.value(c+1))
def pid_distance(distance):
# todo: positivo ou negativo pra ir pra frente? alterar k's de acordo
offset = 0
kp = 5
ki = 0
kd = 0
#erro
e = target_distance - distance
E = E + e
e_dot = e - e_old
duty_cycle = ((kp * e) + (ki * E) + (kd * e_dot)) + offset
if duty_cycle > 100:
duty_cycle = 100
elif duty_cycle < -100:
duty_cycle = -100
e_old = e
return duty_cycle
pid_distance.E = 0
pid_distance.e_old = 0
def pid_heading_left(heading):
offset = 1
kp = 0.04
ki = 0
kd = 0
e = target_heading - heading
E = E + e
e_dot = e - e_old
response = ((kp * e) + (ki * E) + (kd * e_dot)) + offset
if response > 1:
response = 1
elif response < -1:
response = -1
e_old = e
return response
pid_heading_left.E = 0
pid_heading_left.e_old = 0

def pid_heading_right(heading):
offset = 1
kp = -0.04
ki = 0
kd = 0
e = target_heading - heading
E = E + e
e_dot = e - e_old
response = ((kp * e) + (ki * E) + (kd * e_dot)) + offset
if response > 1:
response = 1
elif response < -1:
response = -1
e_old = e
return response
pid_heading_right.E = 0
pid_heading_right.e_old = 0
motor_left = ev3dev.large_motor(ev3dev.OUTPUT_C)
motor_right = ev3dev.large_motor(ev3dev.OUTPUT_B)
ir_sensor = infrared_sensor()
ir_sensor.mode = 'IR-SEEK'
#motor.set(speed_regulation_enabled='on', stop_command='brake')
motor_left.duty_cycle_sp = 0
motor_right.duty_cycle_sp = 0
motor_left.run_direct()
motor_right.run_direct()
while True:
heading, distance = get_value(ir_sensor)
if distance == -128:
print "not found"
continue
duty_cycle = pid_distance(distance)
corr_factor_left = pid_heading_left(heading)
corr_factor_right = pid_heading_right(heading)
motor_left.duty_cycle_sp = duty_cycle * corr_factor_left
motor_right.duty_cycle_sp = duty_cycle * corr_factor_right
motor_left.stop()
motor_right.stop()