User Manual

Table Of Contents
78
X = accelerometer_data['x']
X = kalman_filter_X.kalman(X)
Y = accelerometer_data['y']
Y = kalman_filter_Y.kalman(Y)
'''''
Carry out Kalman filtering on the data
'''
X_fix_output += -X_pid.GenOut(X - target_X)
X_fix_output = ctrl_range(X_fix_output, steady_range_Max, -steady_range_Max)
Y_fix_output += -Y_pid.GenOut(Y - target_Y)
Y_fix_output = ctrl_range(Y_fix_output, steady_range_Max, -steady_range_Max)
#LEFT_I
left_I_input = ctrl_range((X_fix_output + Y_fix_output), steady_range_Max, steady_range_Min)
left_I(0, 35, left_I_input)
#LEFT_II
left_II_input=ctrl_range((abs(X_fix_output*0.5)+Y_fix_output),steady_range_Max,steady_range_M
in)
left_II(0, 35, left_II_input)
#LEFT_III
left_III_input = ctrl_range((-X_fix_output + Y_fix_output), steady_range_Max, steady_range_Mi
n)
left_III(0, 35, left_III_input)
#RIGHT_III
right_III_input = ctrl_range((X_fix_output - Y_fix_output), steady_range_Max, steady_range_Mi
n)
right_III(0, 35, right_III_input)
#RIGHT_II
right_II_input=ctrl_range((abs(-X_fix_output*0.5)-Y_fix_output),steady_range_Max,steady_range
_Min)
right_II(0, 35, right_II_input)
#RIGHT_I
right_I_input = ctrl_range((-X_fix_output-Y_fix_output), steady_range_Max, steady_range_Min)
right_I(0, 35, right_I_input)