def handle_bac_state(channel, data):
global state_lock, state_known, last_crossed_open_inner, last_crossed_closed_inner
global last_known_position, last_state_time, last_motor_rate, last_prop_rate, last_on_off_state
msg = bac_state_high_rate_t.decode(data)
state_lock.acquire()
now = time.time()
if (state_known):
if (msg.estimated_distance > brake_open_inner
and last_known_position <= brake_open_inner):
frac_after = msg.estimated_distance - brake_open_inner
frac_before = brake_open_inner - last_known_position
last_crossed_open_inner = (now * frac_after +
last_state_time * frac_before) / (
frac_after + frac_before)
print "Crossed open inner outwards %f" % last_crossed_open_inner
elif (msg.estimated_distance < brake_open_inner
and last_known_position >= brake_open_inner):
frac_after = brake_open_inner - msg.estimated_distance
frac_before = last_known_position - brake_open_inner
last_crossed_open_inner = (now * frac_after +
last_state_time * frac_before) / (
frac_after + frac_before)
print "Crossed open inner inwards at %f" % last_crossed_open_inner
if (msg.estimated_distance > brake_closed_inner
and last_known_position <= brake_closed_inner):
frac_after = msg.estimated_distance - brake_closed_inner
frac_before = brake_closed_inner - last_known_position
last_crossed_closed_inner = (now * frac_after +
last_state_time * frac_before) / (
frac_after + frac_before)
print "Crossed closed inner outwards at %f" % last_crossed_closed_inner
elif (msg.estimated_distance < brake_closed_inner
and last_known_position >= brake_closed_inner):
frac_after = brake_closed_inner - msg.estimated_distance
frac_before = last_known_position - brake_closed_inner
last_crossed_closed_inner = (now * frac_after +
last_state_time * frac_before) / (
frac_after + frac_before)
print "Crossed closed inner inwards at %f" % last_crossed_closed_inner
state_known = True
last_known_position = msg.estimated_distance
last_state_time = now
last_motor_rate = msg.PumpMotorPWM
last_prop_rate = msg.ValvePWM
last_on_off_state = msg.OnOffValve
state_lock.release()
def handle_bac_state_high_rate_t(channel, data):
global last_msg_plot_time
msg = bac_state_high_rate_t.decode(data)
t = msg.utime / 1000. / 1000.
if (not last_msg_plot_time or t - last_msg_plot_time > 0.05):
distancesPlotter.addDataPoint(0, t, msg.distance_1)
distancesPlotter.addDataPoint(1, t, msg.distance_2)
estimatedDistancePlotter.addDataPoint(0, t, msg.estimated_distance)
pressurePlotter.addDataPoint(0, t, msg.brake_pressure)
errorPlotter.addDataPoint(0, t, msg.error)
integratorPlotter.addDataPoint(0, t, msg.integrator)
effortsPlotter.addDataPoint(0, t, msg.PumpMotorPWM)
effortsPlotter.addDataPoint(1, t, msg.ValvePWM)
last_msg_plot_time = t
def handle_bac_state_high_rate_t(self, channel, data):
msg = bac_state_high_rate_t.decode(data)
d1 = msg.distance_1
d2 = msg.distance_2
gh_front = msg.gh_front
gh_rear = msg.gh_rear
# apply a little low pass
# flip pots (length 5.9, so subtract from that) to get distance inward the caliper has traveled from home
self.caliperLeft_Pos = self.caliperLeft_Pos * 0.5 + 0.5 * -1.0 * (
5.9 - d2) * self.total_inch_to_im_scale
self.caliperRight_Pos = self.caliperRight_Pos * 0.5 + 0.5 * -1.0 * (
5.9 - d1) * self.total_inch_to_im_scale
# units are mm, and hardcode "resting on rail" from logs:
# 38 mm for front
# 39 mm for rear
# higher = farther down
self.caliperVert_Pos_Front = (
(gh_front - 38) / 25.4) * self.total_inch_to_im_scale
self.caliperVert_Pos_Rear = (
(gh_rear - 39) / 25.4) * self.total_inch_to_im_scale
def handle_bac_state(channel, data):
global min_command, max_command, tolerance, settling_time_required, max_test_duration
global state_lock, state_known, cmd_known, setpoint, cmd_confirmed_time
global est_response_time, max_overshoot, max_overshoot_time, est_settling_time
global last_known_position, last_state_time
msg = bac_state_high_rate_t.decode(data)
state_lock.acquire()
now = time.time()
# if we have a command and it has been confirmed...
if (state_known and setpoint > 0):
# response time is first time we're in tolerance
if est_response_time < 0 and abs(msg.estimated_distance - setpoint) < tolerance:
est_response_time = time.time()
print "\tResponse time found"
# overshoot cases
if (est_response_time > 0 and msg.estimated_distance > setpoint and approach_direction > 0):
if (msg.estimated_distance - setpoint > max_overshoot):
max_overshoot = msg.estimated_distance - setpoint
max_overshoot_time = now
print "\tOvershoot updated"
if (est_response_time > 0 and msg.estimated_distance < setpoint and approach_direction < 0):
if (setpoint - msg.estimated_distance > max_overshoot):
max_overshoot = setpoint - msg.estimated_distance
max_overshoot_time = now
print "\tOvershoot updated"
# settling time calculation
if (est_response_time > 0):
if est_settling_time < 0 or abs(msg.estimated_distance - setpoint) > tolerance:
est_settling_time = now
print "\tSettling time updated"
state_known = True
last_known_position = msg.estimated_distance
last_state_time = now
state_lock.release()
def handle_bac_state_high_rate(self, channel, data):
msg = bac_state_high_rate_t.decode(data)
self.last_bac_high_rate_recv_time = time.time()
self.brake_width = msg.estimated_distance
self.brake_pressure = msg.brake_pressure