def __init__(self):
# set True if input range is -1 to +1
self.is_normalized = False
self.expect_degrees = True # convert to radians if True
if self.is_normalized:
print 'Platform Input uses normalized parameters'
else:
print 'Platform Input uses realworld parameters'
self.levels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # command values sent to the chair
self.angles = [0,4,8,12,16,20,0,-4,-8,-12,-16,-20] # range of angles (roll, pitch, yaw) to be measured
self.translations = [0,2,4,6,8,10,0,-2,-4,-6,-8,-10] #range of translation (x,y,z) movements
self.sensors = SensorInput()
self.controller = ControllerInput()
def __init__(self):
# set True if input range is -1 to +1
self.is_normalized = False
self.expect_degrees = True # convert to radians if True
if self.is_normalized:
print 'Platform Input uses normalized parameters'
else:
print 'Platform Input uses realworld parameters'
max_angle = 0.174
step = max_angle / 5
self.levels = [0.0, 0.0, 0.0, 0.0, 0.0,
0.0] # command values sent to the chair
self.angles = [
0, step, step * 2, step * 3, step * 4, step * 5, 0, -step,
-step * 2, -step * 3, -step * 4, -step * 5
] # range of angles (roll, pitch, yaw) to be measured
self.translations = [
0, 20, 40, 60, 80, 100, 0, -20, -40, -60, -80, -100
] #range of translation (x,y,z) movements
self.sensors = SensorInput()
self.controller = ControllerInput()
class InputInterface(object):
USE_GUI = False # set True if using tkInter
def __init__(self):
# set True if input range is -1 to +1
self.is_normalized = False
self.expect_degrees = True # convert to radians if True
if self.is_normalized:
print 'Platform Input uses normalized parameters'
else:
print 'Platform Input uses realworld parameters'
max_angle = 0.174
step = max_angle / 5
self.levels = [0.0, 0.0, 0.0, 0.0, 0.0,
0.0] # command values sent to the chair
self.angles = [
0, step, step * 2, step * 3, step * 4, step * 5, 0, -step,
-step * 2, -step * 3, -step * 4, -step * 5
] # range of angles (roll, pitch, yaw) to be measured
self.translations = [
0, 20, 40, 60, 80, 100, 0, -20, -40, -60, -80, -100
] #range of translation (x,y,z) movements
self.sensors = SensorInput()
self.controller = ControllerInput()
def quit(self):
self.command("quit")
def command(self, cmd):
if self.cmd_func is not None:
print "Requesting command:", cmd
self.cmd_func(cmd)
def chair_status_changed(self, chair_status):
print(chair_status[0])
def intensity_status_changed(self, intensity):
self.intensity_status_Label.config(text=intensity[0], fg=intensity[1])
def begin(self, cmd_func, move_func, limits):
self.cmd_func = cmd_func
self.move_func = move_func
self.limits = limits # note limits are in mm and radians
self.cmd_func("enable") # enable the platform
self.state = 0 # states are translations and rotations
self.sequence = 0 # sequences are: neutral, 5 steps up, neutral, 5 steps down
self.move_func(self.levels) # move to neutral
self.outfile = open("errors.csv", "w")
self.next_reading_time = time.time() + settling_interval
self.move_func(self.levels) # set to neutral position
if self.outfile:
self.outfile.write(
"Len1,Len2,Len3,Len4,Len5,Len6,Roll, Pitch,Yaw\n")
raw_input("press key to start")
def fin(self):
# client exit code goes here
self.outfile.close()
def get_current_pos(self):
return self.levels
def service(self):
if time.time(
) > self.next_reading_time: # two seconds between chair movements
self.next_reading_time = time.time() + settling_interval
self.process_data()
self.move_platform()
def process_data(self):
sensor_data = self.sensors.latest_data()
controller_data = self.controller.latest_data()
if sensor_data and controller_data:
print "sensor data average", [
'%.1f' % elem for elem in sensor_data
]
print "commanded data", controller_data,
expected_values = []
error_values = []
for idx, val in enumerate(compare_sequence):
idx_of_ctrl_data = compare_sequence[
idx] # the ith index of sensor data is compared to this index into control data
expected_values.append(controller_data[idx_of_ctrl_data])
# sensor rpy in sent as degrees, but controller uses radians
if idx >= 6: # angles follows the six lengths, convert to degrees
expected_values[idx] = math.degrees(expected_values[idx])
error_values.append(expected_values[idx] - sensor_data[idx])
outstring = ','.join([str(i) for i in error_values])
# print "outstring", outstring
if self.outfile:
self.outfile.write(str(outstring) + "\n")
else:
if sensor_data == None:
print "NO SENSOR DATA"
if controller_data == None:
print "NO CONTROLLER DATA"
def move_platform(self):
# move platform to position for the current state and sequence indices
self.levels = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # reset all fields
if self.state < 3:
self.levels[self.state] = self.translations[self.sequence]
self.sequence += 1
if self.sequence >= len(self.translations):
self.state = self.state + 1
self.sequence = 0
else:
self.levels[self.state] = self.angles[self.sequence]
self.sequence += 1
if self.sequence >= len(self.angles):
self.state = self.state + 1
self.sequence = 0
print self.levels
if self.state > 5:
print "finished all measurements"
exit()
else:
self.move_func(self.levels)
#send_dummy_controller_msg(self.controller, self.levels) # only for testing
print "state=", self.state, " sequence=", self.sequence