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'