def algo(self, values, all_data):
t = values['time']
if t > self.last_move + self.wait_time + self.offset:
print 'Last maxima {}'.format(
abs(
last_maxima(all_data['time'],
all_data['be'],
values_time='values',
dt=self.period)))
print 'Stop swing', values['time']
self.last_move = t
return self.next_position()
# ending if duration is over or minimum angle is reached
if t - self.start_time > self.duration:
print 'duration reached, stopping'
return 'switch'
if abs(
last_maxima(all_data['time'],
all_data['values'],
values_time='values',
dt=self.period)) < self.min_angle:
print 'min angle reached, stopping'
return 'switch'
def algo(self, values, all_data):
# sign_zero of big encoder changes when crossing zero point
if sign_zero(values['be']) != sign_zero(self.previous_be):
self.max_angle = last_maxima(all_data['time'], all_data['be'], values_time='values', dt=self.period)
print 'Last maximum angle {}'.format(self.max_angle)
# If angle of last swing is greater than the maintain angle then
# want to kick earlier to counteract it, vice versa for smaller
if abs(self.max_angle) > self.maintain_angle + 0.5:
self.offset_angle += 0.5
print '\033[1mChanging offset to {} degrees\033[0m'.format(self.offset_angle)
if abs(self.max_angle) < self.maintain_angle - 0.5:
self.offset_angle -= 0.5
print '\033[1mChanging offset to {} degrees\033[0m'.format(self.offset_angle)
self.previous_be = values['be']
if values['be'] > self.maintain_angle - self.offset_angle and values['pos'] != 'extended' and values['av'] > 0:
print values['pos']
print 'Should kick at greater than {}, actually kicking at {}'.format(self.maintain_angle - self.offset_angle, values['be'])
return 'extended'
if values['be'] < -self.maintain_angle + self.offset_angle and values['pos'] != 'seated' and values['av'] < 0:
print values['pos']
print 'Should kick at less than {}, actually kicking at {}'.format(-self.maintain_angle + self.offset_angle, values['be'])
return 'seated'
if values['time'] - self.start_time > self.duration:
print '\033[1mDuration over, switching algorithm\033[0m'
return 'switch'
def __init__(self, values, all_data, **kwargs):
self.start_time = values['time']
self.previous_time = values['time']
self.switch_time = 100
self.offset = -0.21
self.period = kwargs.get('period', 0.005)
self.last_maximum = last_maxima(all_data['time'], all_data['be'], time_values='values', dt=self.period)
self.duration = kwargs.get('duration', float('inf'))
self.previous_be = values['be']
def algo(self, values, all_data):
max_encoder_angle = last_maxima(all_data['time'],
all_data['be'],
time_values='values',
dt=self.period)
speed = abs(max_encoder_angle * 0.1)
if speed > 0.8:
speed = 0.8
return speed
if speed < 0.05:
speed = 0.05
return speed
'''sign of big encoder changes when crossing zero point'''
if sign_zero(values['be']) != sign_zero(self.previous_be):
print 'After {}'.format(values['be']), 'Before {}'.format(
self.previous_be)
self.min_time = last_zero_crossing(values, self.previous_time,
self.previous_be)
self.max_time, self.last_maximum = last_maxima(all_data['time'],
all_data['be'],
time_values='both',
dt=self.period)
# quarter period difference between time at maxima and minima
self.quart_period = np.abs(self.min_time - self.max_time)
print 'Ran at time {}'.format(values['time'])
# set time for position to switch
self.time_switch = self.min_time + self.quart_period + self.offset
print 'Next switching time: {:.3f}'.format(
self.time_switch), 'Last maximum: {:.3f}'.format(
self.last_maximum)
# At the end of the loop, set the value of big encoder to the previous value
self.previous_be = values['be']
self.previous_time = values['time']
if values['time'] > self.time_switch:
self.time_switch += 50
return self.next_position(speed)
return self.end_conditions(values)
def algo(self, values, all_data, **kwargs):
"""
Define parametric swinging function, for current swing state 'values' and past states 'all_data'.
See interface.py for details.
"""
# Getting period:
# Shuffle values around, such that we compare the current state to
# the previous state.
if abs(values['be']) >= self.max_angle:
self.max_angle_reached = True # Switch at next zero.
self.prev, self.curr = self.curr, values['be']
self.prev_time, self.curr_time = self.curr_time, values['time']
if sign(self.curr) != sign(self.prev): # Zero crossed
print values['time'], 'Crossed zero point'
self.moving_down = False
self.max_times = self.last_maxima(all_data)
dt = self.curr_time - self.prev_time
interpolate = dt * np.abs(self.curr) / \
np.abs(self.curr - self.prev)
true_zero_time = values['time'] - interpolate
quarter_period = abs(self.max_times[-1] - true_zero_time)
self.time_to_switch_unfolded = true_zero_time + quarter_period
# Top of swing reached.
elif abs(self.curr) <= abs(self.prev) and not self.moving_down:
print values['time'], 'At maximum'
self.moving_down = True
# Moving down flag prevents max_times from being appended to more than once.
self.zero_times = last_minima(all_data)
self.max_times = last_maxima(all_data)
quarter_period = abs(self.max_times[-1] - self.zero_times[-1])
self.time_to_switch_folded = self.max_times[-1] + quarter_period
if values['time'] > self.time_to_switch_unfolded + self.tolerance_zero:
self.time_to_switch_unfolded += 100 # Some arbitrary big number.
print 'Angle', values['time'], values['be']
return self.maxima_pos # Change position.
if values['time'] > self.time_to_switch_folded + self.tolerance_max:
if values[
'time'] >= self.duration + self.start_time or self.max_angle_reached:
print("Maximum duration/angle reached, switching.")
return "switch" # Switch ready for next zero.
print 'Angle', values['time'], values['be']
self.time_to_switch_folded += 100 # Some arbitrary big number.
return self.zero_pos # Change position.
def algo(self, values, all_data):
# calculate times to kick one per period, when crossing from positive to negative
if sign_zero(values['be']) != sign_zero(
self.previous_be) and values['be'] < 0:
self.min_time = last_zero_crossing(values, self.previous_time,
self.previous_be)
self.max_time, self.last_maximum = last_maxima(all_data['time'],
all_data['be'],
time_values='both',
dt=self.period)
# quarter period difference between time at maxima and minima
self.quart_period = np.abs(self.min_time - self.max_time)
# do two standard, poor, or good kicks, dependent on how far from maintain amplitude it is
if -0.1 <= abs(self.last_maximum) - abs(
self.maintain_angle) <= 0.1:
self.offset = self.offsets['standard']
elif abs(self.last_maximum) - abs(self.maintain_angle) > 0.1:
self.offset = self.offsets['poor']
elif abs(self.last_maximum) - abs(self.maintain_angle) < -0.1:
self.offset = self.offsets['good']
else:
print 'Offset condition not found\nLast maximum: {}, Maintain angle: {}, \
Difference between{}'.format(
self.last_maximum, self.maintain_angle,
abs(self.last_maximum) - abs(self.maintain_angle))
# set time for position to switch
self.time_switch_sea = self.min_time + self.quart_period + self.offset
self.time_switch_ext = self.time_switch_sea + 2 * self.quart_period
print 'Current time: {:.3f}'.format(values['time']), \
'Last maximum: {:.3f} degrees'.format(self.last_maximum)
# 'Next seated switching time: {:.3f}'.format(self.time_switch_sea), \
# 'Next extended switching time: {:.3f}'.format(self.time_switch_ext), \
# At the end of the loop, set the value of big encoder to the previous value
self.previous_be = values['be']
self.previous_time = values['time']
# position changes, move slower to hopefully control amplitude better
if values['time'] >= self.time_switch_sea:
self.time_switch_sea = float('inf')
return ['seated', 1.0]
if values['time'] >= self.time_switch_ext:
self.time_switch_ext = float('inf')
return ['extended', 1.0]
# duration over
if values['time'] - self.start_time > self.duration:
print 'Switching from maintaining, duration ended'
return 'switch'
def last_maxima_ta(self, all_data, time_values='time'):
times = all_data['time']
be = all_data['be']
se0 = all_data['se0']
se1 = all_data['se1']
ta_list = [self.total_angle(*values) for values in zip(be, se0, se1)]
return last_maxima(times,
ta_list,
time_values=time_values,
dt=self.period)
def __init__(self, values, all_data, **kwargs):
self.period = kwargs.get('period', 0.005)
self.start_time = values['time']
self.duration = kwargs.get('duration', float('inf'))
self.wait_time = 20
self.last_move = last_maxima(all_data['time'],
all_data['be'],
time_values='time',
dt=self.period)
self.offset = -0.2
# offset is time from maximum to swing
self.time_switch = 50
self.last_maximum = last_maxima(all_data['time'],
all_data['be'],
time_values='values',
dt=self.period)
# setting up times
self.previous_time = values['time']
self.previous_be = values['be']
# max_angle used for increasing min_angle for decreasing
self.increasing = kwargs.get('increasing', True)
self.max_angle = kwargs.get('max_angle', 180)
self.min_angle = kwargs.get('min_angle', 5)
# sets up correct position
av = values['av']
if av >= 0:
# positive angular momentum means going backward, therefore want seated to decrease
self.next_pos = 'extended'
else:
# opposite of above
self.next_pos = 'seated'
print 'Stopping swing, constant period'
def algo(self, values, all_data):
"""
This algorithm uses the accelerometer data to reconstruct the big encoder values, from this
sinusoid it is used with the quarter period algorithm to predict the maximas and switch
accordingly.
"""
times = np.append(all_data['time'], np.array(
values['time'])) + self.filter_offset
az = np.append(all_data['az'], np.array(values['az']))
# will collect the last n results
n = int(1.0 / self.period * 6.0)
if n <= 39:
n = 40
filtered_az = -final_filter(times[-n:], az[-n:])
times = times[-n:]
current_az = filtered_az[-1]
previous_az = filtered_az[-2]
if sign_zero(current_az) != sign_zero(previous_az):
if values['time'] - self.last_time_set > 0.7:
# print 'After {}'.format(current_az), 'Before {}'.format(previous_az)
self.min_time = last_zero_crossing_az(times, filtered_az)
self.max_time = last_maxima(times,
filtered_az,
time_values='time',
dt=self.period)
# quarter period difference between time at maxima and minima
self.quart_period = np.abs(self.min_time - self.max_time)
# set time for position to switch
self.time_switch = self.min_time + self.quart_period + self.offset
self.last_time_set = values['time']
print 'Current time: {}'.format(
values['time']), 'Next switching time: {:.3f}'.format(
self.time_switch)
if values['time'] > self.time_switch:
self.time_switch = float('inf')
return self.next_position_calculation(filtered_az)
# switch conditions
if values['time'] - self.start_time > self.duration:
return 'switch'
def algo(self, values, all_data):
# sign_zero_zero of big encoder changes when crossing zero point
if sign_zero(values['be']) != sign_zero(
self.previous_be) and values['be'] >= 0:
self.min_time = last_zero_crossing(values, self.previous_time,
self.previous_be)
self.max_time, self.max_angle = last_maxima(all_data['time'],
all_data['be'],
time_values='both',
dt=self.period)
# only worry is if offset becomes >= the quarter period then nao will never change
# position, until the angle decreases enough that the offset rises again mind
# same for other way around
if abs(self.max_angle) > self.maintain_angle + 0.2:
self.offset += 0.01
print '\033[1mChanging offset to {}\033[0m'.format(self.offset)
if abs(self.max_angle) < self.maintain_angle - 0.2:
self.offset -= 0.01
print '\033[1mChanging offset to {}\033[0m'.format(self.offset)
# quarter period difference between time at maxima and minima
self.quart_period = abs(self.min_time - self.max_time)
# set time for position to switch
self.time_switch = self.min_time + self.quart_period + self.offset
# Need to adjust offset based on difference between max angle and supposed maintain angle
print 'Next switching time: {:.2f}'.format(
self.time_switch), 'Last maximum: {:.2f}'.format(
self.max_angle)
# At the end of the loop, set the value of big encoder to the previous value
self.previous_be = values['be']
self.previous_time = values['time']
if values['time'] > self.time_switch:
print 'Time to switch, changing position'
self.time_switch += 100
return self.next_position_calculation(values)
if values['time'] - self.start_time > self.duration:
print '\033[1mSwitching, duration ended\033[0m'
return 'switch'
def __init__(self, values, all_data, **kwargs):
self.period = kwargs.get('period', 0.005)
self.start_time = values['time']
self.duration = kwargs.get('duration', float('inf'))
self.min_angle = kwargs.get('min_angle', 1.0)
self.wait_time = 1.4
self.last_move = last_maxima(all_data, 'time')
self.offset = -0.3
# sets up correct position
av = values['av']
if av >= 0:
# positive angular momentum means going backward, therefore want seated to decrease
self.next_pos = 'seated'
else:
# opposite of above
self.next_pos = 'extended'
print 'Stopping swing, constant period'
def algo(self, values, all_data, **kwargs):
if sign_zero(values['be']) != sign_zero(self.prev_be):
# calculate true zero crossing point
true_zero_time = last_zero_crossing(values, self.prev_time,
self.prev_be)
# calculate quarter period based on latest maximum and minimum
self.max_times = last_maxima(all_data['time'],
all_data['be'],
time_values='time',
dt=self.period)
quarter_period = abs(self.max_times - true_zero_time)
# maximum and minimum point
self.next_max = true_zero_time + quarter_period + self.max_offset
self.next_min = true_zero_time + 2 * quarter_period + self.min_offset
print values['time'], self.next_max, self.next_min
self.prev_be = values['be']
self.prev_time = values['time']
# if time to switch change to correct position
if values['time'] > self.next_max:
self.next_max += 100
if self.increasing:
return 'lowered'
else:
return 'raised'
if values['time'] > self.next_min:
self.next_min += 100
if self.increasing:
return 'raised'
else:
return 'lowered'
# switch if angle is big enough or duration is over
if values['time'] - self.start_time > self.duration:
print 'Duration end', values[
'time'], self.start_time, self.duration
return 'switch'
if values['be'] > self.max_angle:
print 'Angle end', values['be'], self.max_angle
return 'switch'
def __init__(self, values, all_data, **kwargs):
self.period = kwargs.get('period', 0.005)
# offset is time from maximum to swing
self.time_switch = 100
self.offset = -0.1
self.last_maximum = last_maxima(all_data['time'],
all_data['be'],
time_values='values',
dt=self.period)
self.start_time = values['time']
self.max_angle = kwargs.get('max_angle', 180)
self.increase = kwargs.get('increase', True)
self.duration = kwargs.get('duration', float('inf'))
self.pendulum_length = 1.82
self.min_angle = kwargs.get('min_angle', 0.5)
self.next_highest_angle = None
self.previous_max_angle = all_data['be'].max()
self.offset = 0
self.quart_period = 0.75
def algo(self, values, all_data):
"""
Use the max_angle approximation to estimate the time to switch the position
"""
current_be = values['be']
previous_be = all_data['be'][-1]
current_av = values['av']
previous_av = all_data['av'][-1]
current_time = values['time']
previous_time = all_data['time'][-1]
print 'Max angle', 'Time: {:.2f}'.format(
values['time']), 'Big encoder value: {:.2f}'.format(values['be'])
# sign of big encoder changes when crossing zero point
if sign_zero(values['be']) != sign_zero(previous_be):
print 'After {}'.format(
values['be']), 'Before {}'.format(previous_be)
self.min_time = last_zero_crossing(values, previous_time,
previous_be)
self.max_time, self.last_maximum = last_maxima(all_data['time'],
all_data['be'],
time_values='both',
dt=self.period)
# quarter period difference between time at maxima and minima
self.quart_period = np.abs(self.min_time -
self.max_time) + self.offset
if (np.sign(current_be) != np.sign([previous_be])):
if (current_be < 0 and values['pos'] != 'extended'):
return ['seated', 0.3 / self.quart_period]
elif (current_be > 0 and values['pos'] != 'seated'):
return ['extended', 0.3 / self.quart_period]
if (np.sign(previous_be) == -1 and previous_be - current_be < 0):
self.previous_max_angle = previous_be
print('max_angle', previous_be)
elif (np.sign(previous_be) == 1 and previous_be - current_be > 0):
self.previous_max_angle = previous_be
print('max_angle', previous_be)
if (self.previous_max_angle < self.min_angle):
return 'switch'
def algo(self, values, all_data):
t = values['time'] - self.start_time # renames current time to t
print 'Time: {:.2f}'.format(
values['time']), 'Big encoder: {:.2f}'.format(values['be'])
if t < 0.1:
# sets the first kick from standstill.
self.last_move = t # resets time of last kick
return 'extended' # kicks
if t > 0.1:
if t > self.last_move + self.wait_time / 2 and self.first_kick == True:
# first kick needed after a quarter period, not half
self.first_kick = False # go to half period kicks
self.last_move = t # reset time of last kick
if values['pos'] == 'seated':
return 'extended'
else:
return 'seated'
if t > self.last_move + self.wait_time:
# kicks after first kick use quarter period
self.last_move = t # reset time of last kick
if values['pos'] == 'extended':
return 'seated'
else:
return 'extended'
if 0.15 < t - self.last_move < 0.5:
if t > self.duration:
print 'last move', self.last_move
print 'Switch on duration'
return 'switch'
if t > 10:
if last_maxima(all_data['time'],
all_data['be'],
time_values='values',
dt=self.period) > self.max_angle:
print 'last move', self.last_move
print 'Switched on max angle'
return 'switch'
def __init__(self, values, all_data, **kwargs):
self.period = kwargs.get('period', 0.005)
# offset is time from maximum to swing
self.time_switch_ext = float('inf')
self.time_switch_sea = float('inf')
self.last_maximum = last_maxima(all_data['time'],
all_data['be'],
time_values='values',
dt=self.period)
# setting up times
self.start_time = values['time']
self.previous_time = values['time']
self.previous_be = values['be']
self.maintain_angle = kwargs.get('maintain_angle', 10.0)
# alternative switch condition
self.duration = kwargs.get('duration', float('inf'))
self.offsets = {'good': -0.25, 'poor': 0.1, 'standard': -0.1}
def __init__(self, values, all_data, **kwargs):
self.period = kwargs.get('period', 0.005)
self.start_time = values['time']
self.previous_be = values['be']
self.previous_time = values['time']
self.maintain_angle = kwargs.get('maintain_angle', 10)
# offset is time from maximum to swing
self.time_switch = 100
masses = kwargs.get('masses', False)
if masses:
self.offset = 0.20
else:
self.offset = -0.25
self.last_maximum = last_maxima(all_data['time'],
all_data['be'],
time_values='values',
dt=self.period)
# alternative switch condition
self.duration = kwargs.get('duration', float('inf'))
def algo(self,values,all_data):
if sign_zero(values['be']) != sign_zero(self.previous_be):
self.min_time = last_zero_crossing(values, self.previous_time, self.previous_be)
self.max_time, self.last_maximum = last_maxima(all_data['time'], all_data['be'], time_values='both', dt=self.period)
# quarter period difference between time at maxima and minima
self.quart_period = np.abs(self.min_time - self.max_time)
# set time for position to switch
self.switch_time = self.min_time + self.quart_period + self.offset
self.previous_be = values['be']
self.previous_time = values['time']
print 'Increase Period' ,values['time'] , values['be']
if values['time'] > self.switch_time:
self.switch_time += 100
if values['be'] < 0:
return 'crunched'
elif values['be'] > 0:
return 'extended'
if values['time'] - self.start_time > self.duration:
print 'Switch' , values['time']
return 'switch'
