def algo(self, values, all_data):
"""
Use the angular velosity to estimate the time to switch the posture
"""
current_be = values['be']
previous_be = all_data['be'][-1]
print 'Time: {:.2f}'.format(
values['time']), 'Big encoder value: {:.2f}'.format(values['be'])
if (sign_zero(previous_be) == -1 and previous_be - current_be < 0):
if (self.increasing == True):
return 'seated'
elif (self.increasing == False):
return 'extended'
self.previous_max_angle = previous_be
print('max_angle', previous_be)
elif (sign_zero(previous_be) == 1 and previous_be - current_be > 0):
if (self.increasing == True):
return 'extended'
elif (self.increasing == False):
return 'seated'
self.previous_max_angle = previous_be
print('max_angle', previous_be)
# switch conditions
if (self.increasing == True):
print 'Increase', values['time']
if abs(values['be']) > self.max_angle:
return 'switch'
elif (self.increasing == False):
print 'Decrease', values['time']
if abs(self.previous_max_angle) < self.min_angle:
return 'switch'
if values['time'] - self.start_time > self.duration:
return 'switch'
def algo(self, values, all_data):
# sign of big encoder changes when crossing zero point
current_ta = self.total_angle(values['be'], values['se0'],
values['se1'])
if sign_zero(current_ta) != sign_zero(self.previous_ta):
self.min_time = last_zero_crossing(values, self.previous_time,
self.previous_ta)
self.max_time, self.last_maximum = self.last_maxima_ta(
all_data, time_values='both')
# 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
print 'Next switching time', self.time_switch
# At the end of the loop, set the value of big encoder to the previous value
self.previous_ta = current_ta
self.previous_time = values['time']
if values['time'] > self.time_switch:
self.time_switch += 100
return self.next_position_calculation(values)
return self.end_conditions(values)
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 algo(self, values, all_data):
print 'Nothing', values['time'], values['be']
if sign_zero(values['be']) != sign_zero(
self.previous_be) and values['be'] >= 0:
if values['time'] - self.start_time > self.duration:
return 'switch'
self.previous_be = values['be']
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 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 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 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):
"""
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):
"""
Use the angular velosity to estimate the time to switch the posture
"""
self.current_be = total_angle(values['be'], values['se0'],
values['se1'])
print 'Time: {:.2f}'.format(
values['time']), 'Total angle value: {:.2f}'.format(
self.current_be)
if (sign_zero(self.previous_be) == -1
and self.previous_be - self.current_be < 0):
self.previous_be = self.current_be
if (self.increasing == True):
return ['seated', 0.5]
elif (self.increasing == False):
return ['extended', 0.5]
self.previous_max_angle = self.previous_be
print('max_angle', self.previous_be)
elif (sign_zero(self.previous_be) == 1
and self.previous_be - self.current_be > 0):
self.previous_be = self.current_be
if (self.increasing == True):
return ['extended', 0.5]
elif (self.increasing == False):
return ['seated', 0.5]
self.previous_max_angle = self.previous_be
print('max_angle', self.previous_be)
self.previous_be = self.current_be
# switch conditions
if (self.increasing == True):
if abs(self.current_be) > self.max_angle:
return 'switch'
elif (self.increasing == False):
print 'Decrease', values['time']
if abs(self.previous_max_angle) < self.min_angle:
return 'switch'
if values['time'] - self.start_time > self.duration:
return 'switch'
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'