-
Notifications
You must be signed in to change notification settings - Fork 3
/
globals.py
173 lines (134 loc) · 7.27 KB
/
globals.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
import math
import logging
import numpy as np
from CartPole.cartpole_parameters import TrackHalfLength
CHIP = "ZYNQ" # Can be "STM" or "ZYNQ"; remember to change chip specific values on firmware if you want to run control from there
CONTROLLER_NAME = 'mpc' # e.g. 'pid', 'mpc', 'do-mpc', 'do-mpc-discrete'
OPTIMIZER_NAME = 'rpgd-tf' # e.g. 'rpgd-tf', 'mppi', only taken into account if CONTROLLER_NAME = 'mpc'
# Motor type selection
# Choose 'POLOLU' or 'ORIGINAL'
# It is set automatically during calibration
# You must set it here correctly only if you want to skip calibration
# Also: In our lab we have two cartpoles - with original and pololu motors
# Setting the motor type identifies the robot instance and determines parameters which are not dependent on motor
MOTOR = 'POLOLU'
##### Controller Settings #####
if CONTROLLER_NAME == 'pid':
CONTROL_PERIOD_MS = 5
elif CONTROLLER_NAME == 'neural-imitator':
CONTROL_PERIOD_MS = 5
elif CONTROLLER_NAME == 'fpga':
CONTROL_PERIOD_MS = 15
else:
CONTROL_PERIOD_MS = 20 # e.g. 5 for PID or 20 for mppi
TIMESTEPS_FOR_DERIVATIVE = 1 # TODO: Python only, hardware sets it separately. In number of control cycles
if CHIP == 'STM':
MOTOR_PWM_PERIOD_IN_CLOCK_CYCLES = 7200
# First number in the tuple is multiplicative factor by which control command Q (in the range[-1,1]) is multiplied.
# The other two shift (additive) to account for friction indep. of speed (separate for pos and neg Q)
# Only applied if CORRECT_MOTOR_DYNAMICS is True
MOTOR_CORRECTION_ORIGINAL = (0.63855139, 0.11653139, 0.11653139)
MOTOR_CORRECTION_POLOLU = (0.595228, 0.0323188, 0.0385016)
# The 12-bit ADC has a range of 4096 units
# However due to potentiometer dead angle these 4096 units are mapped on less than full circle
# The full circle in adc units was determined
# by readout difference between up and down position on the side not including dead angle
ANGLE_360_DEG_IN_ADC_UNITS = 4271.34
ANGLE_HANGING_POLOLU = 1028.579 # Value from sensor when pendulum is at stable equilibrium point
ANGLE_HANGING_ORIGINAL = 910.0 # Value from sensor when pendulum is at stable equilibrium point
POSITION_ENCODER_RANGE = 4164 # This is an empirical approximation
elif CHIP == 'ZYNQ':
MOTOR_PWM_PERIOD_IN_CLOCK_CYCLES = 10000 # STM value is the default, we make it match concerning Zybo PL clock
MOTOR_CORRECTION_ORIGINAL = (0.63855139, 0.11653139, 0.11653139)
MOTOR_CORRECTION_POLOLU = (0.5733488, 0.0257380, 0.0258429)
ANGLE_360_DEG_IN_ADC_UNITS = 4069.05 # Explanation - see above for STM case.
# FIXME: At first one would expect ANGLE_360_DEG_IN_ADC_UNITS to be the same for Zybo and STM
# It is unclear if the difference comes from measuring it on different cartpoles
# or is due to imprecise voltage shifting which is required on Zybo
# Please think it through and adjust this comment appropriately.
ANGLE_HANGING_POLOLU = 1005.5 # Value from sensor when pendulum is at stable equilibrium point
ANGLE_HANGING_ORIGINAL = 1008.5 # Value from sensor when pendulum is at stable equilibrium point
POSITION_ENCODER_RANGE = 4695.0 # For new implementation with Zybo. FIXME: Not clear why different then for STM
else:
raise Exception("Unknown chip " + CHIP)
if MOTOR == 'ORIGINAL':
MOTOR_CORRECTION = MOTOR_CORRECTION_ORIGINAL
ANGLE_HANGING = ANGLE_HANGING_ORIGINAL
elif MOTOR == 'POLOLU':
MOTOR_CORRECTION = MOTOR_CORRECTION_POLOLU
ANGLE_HANGING = ANGLE_HANGING_POLOLU
else:
raise Exception("Unknown motor type " + MOTOR)
DANCE_PATH = 'square' # 'square', 'sin'
DANCE_AMPL = 0.1 # m
DANCE_PERIOD_S = 10.0
DANCE_START_TIME = 0.0
DANCE_UP_AND_DOWN = False
TIME_LIMITED_RECORDING_LENGTH = 1000 # in time steps (1 step = CONTROL_PERIOD_MS)
##### Logging and Recordings #####
LOGGING_LEVEL = logging.ERROR
PATH_TO_EXPERIMENT_RECORDINGS = './ExperimentRecordings/' # Path where the experiments data is stored
PRINT_PERIOD_MS = 10 # shows state in terminal every this many control updates
STATISTICS_IN_TERMINAL_AVERAGING_LENGTH = 500
##### Live Plot (start with 6, save plot with 7 and reset with 8) #####
LIVE_PLOTTER_USE_REMOTE_SERVER = False
LIVE_PLOTTER_REMOTE_USERNAME = 'marcinpaluch'
LIVE_PLOTTER_REMOTE_IP = '192.168.194.233'
DEFAULT_ADDRESS = ('localhost', 6000)
CONTROL_SYNC = True # Delays Input until next Timeslot for more accurate measurements
AUTOSTART = False # Autostarts Zero-Controller for Performance Measurement
JSON_PATH = 'CartPoleSimulation/Control_Toolkit_ASF/'
##### Motor Settings #####
CORRECT_MOTOR_DYNAMICS = False if CONTROLLER_NAME == 'pid' else True # Linearize and Threshold Motor Commands
MOTOR_FULL_SCALE_SAFE = int(0.95 * MOTOR_PWM_PERIOD_IN_CLOCK_CYCLES + 0.5) # Including a safety constraint
##### Angle Conversion #####
# Angle unit conversion adc to radians: (ANGLE_TARGET + ANGLE DEVIATION - ANGLE_360_DEG_IN_ADC_UNITS/2)/ANGLE_360_DEG_IN_ADC_UNITS*math.pi
ANGLE_AVG_LENGTH = 1 # adc routine in firmware reads ADC this many times quickly in succession to reduce noise
ANGLE_HANGING_DEFAULT = True # If True default ANGLE_HANGING is loaded for a respective cartpole when motor is detected at calibration
# This variable changes to false after b is pressed - you can first measure angle hanging and than calibrate without overwritting
# At the beginning always default angle hanging for default motor specified in globals is loaded
ANGLE_NORMALIZATION_FACTOR = (2 * math.pi) / ANGLE_360_DEG_IN_ADC_UNITS
ANGLE_D_MEDIAN_LEN = 1
POSITION_D_MEDIAN_LEN = 1
##### Position Conversion #####
POSITION_NORMALIZATION_FACTOR = TrackHalfLength * 2 / POSITION_ENCODER_RANGE # 0.000084978540773
JOYSTICK_DEADZONE = 0.1 # deadzone around joystick neutral position that stick is ignored
JOYSTICK_POSITION_KP = 4.0
##### Serial Port #####
SERIAL_PORT_NUMBER = 1
SERIAL_BAUD = 230400 # default 230400, in firmware. Alternatives if compiled and supported by USB serial intervace are are 115200, 128000, 153600, 230400, 460800, 921600, 1500000, 2000000
ratio = 1.05
##### Wrong Place ##### #TODO: remove functions and calculations from parameter file
ANGLE_DEVIATION = np.array(0.0)
SEND_CHANGE_IN_TARGET_POSITION_ALWAYS = True # If false it sends change in target position only if firmware control is active.
def angle_deviation_update(new_angle_hanging):
global ANGLE_360_DEG_IN_ADC_UNITS
# update angle deviation according to ANGLE_HANGING update
if new_angle_hanging < ANGLE_360_DEG_IN_ADC_UNITS / 2:
angle_deviation = - new_angle_hanging - ANGLE_360_DEG_IN_ADC_UNITS / 2 # moves upright to 0 and hanging to -pi
else:
angle_deviation = - new_angle_hanging + ANGLE_360_DEG_IN_ADC_UNITS / 2 # moves upright to 0 and hanging to pi
return angle_deviation
ANGLE_DEVIATION[...] = angle_deviation_update(ANGLE_HANGING)
def inc(param):
if param < 0.2:
param = round(param + 0.01, 2)
elif param < 2:
param = round(param + 0.1, 1)
else:
old = param
param = round(param * ratio)
if param == old:
param += 1
return param
def dec(param):
if param < 0.2:
param = max(0, round(param - 0.01, 2))
elif param < 2:
param = max(0, round(param - 0.1, 1))
else:
old = param
param = round(param / ratio)
if param == old:
param -= 1
return param