def __init__(self, left_low=True, right_low=True, name="VelocityDrivetrainModel"):
super(VelocityDrivetrainModel, self).__init__(name)
self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
right_low=right_low)
self.dt = 0.005
self.A_continuous = numpy.matrix(
[[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
[self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
self.B_continuous = numpy.matrix(
[[self._drivetrain.B_continuous[1, 0], self._drivetrain.B_continuous[1, 1]],
[self._drivetrain.B_continuous[3, 0], self._drivetrain.B_continuous[3, 1]]])
self.C = numpy.matrix(numpy.eye(2))
self.D = numpy.matrix(numpy.zeros((2, 2)))
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
self.B_continuous, self.dt)
# FF * X = U (steady state)
self.FF = self.B.I * (numpy.eye(2) - self.A)
self.PlaceControllerPoles([0.6, 0.6])
self.PlaceObserverPoles([0.02, 0.02])
self.G_high = self._drivetrain.G_high
self.G_low = self._drivetrain.G_low
self.R = self._drivetrain.R
self.r = self._drivetrain.r
self.Kv = self._drivetrain.Kv
self.Kt = self._drivetrain.Kt
self.U_max = self._drivetrain.U_max
self.U_min = self._drivetrain.U_min
def __init__(self,
num_sensors,
num_actions,
show=True,
agent_name='test_agent'):
"""
Configure the Agent
num_sensors and num_actions are the only absolutely necessary
arguments. They define the number of elements in the
sensors and actions arrays that the agent and the world use to
communicate with each other.
"""
self.BACKUP_PERIOD = 1e4
self.FORGETTING_RATE = 1e-3
self.show = show
self.name = agent_name
self.log_dir = os.path.normpath(os.path.join(mod_path, '..', 'log'))
if not os.path.isdir(self.log_dir):
os.makedirs(self.log_dir)
self.pickle_filename = os.path.join(self.log_dir,
'.'.join([agent_name, 'pickle']))
self.num_sensors = num_sensors
self.num_actions = num_actions
# Initialize agent infrastructure.
# Choose min_cables to account for all sensors, actions,
# and two reward sensors, at a minimum.
min_cables = self.num_actions + self.num_sensors
self.drivetrain = drivetrain.Drivetrain(min_cables)
num_cables = self.drivetrain.cables_per_gearbox
self.hub = hub.Hub(num_cables)
self.spindle = spindle.Spindle(num_cables)
self.mainspring = mainspring.Mainspring(num_cables)
self.arborkey = arborkey.Arborkey()
self.action = np.zeros((self.num_actions, 1))
self.cumulative_reward = 0
self.time_since_reward_log = 0
self.reward_history = []
self.reward_steps = []
self.reward_max = -tools.BIG
self.timestep = 0
self.graphing = True
def robotInit(self):
'''Robot-wide initialization code should go here'''
self.drivetrain = drivetrain.Drivetrain()
self.sensors = sensor_handler.sensorHandler()
self.joystick = joystick_handler.JoystickHandler()
self.elevator = elevator.Elevator()
self.intakeHandler = intake.Intake(self.sensors, self.joystick)
self.auto = Autonomous_mode_handler.AutonomousModeHandler(
self.drivetrain, self.sensors, self.elevator, self.intakeHandler)
self.autoPath = auto_path.AutoPath(self.sensors, self.drivetrain)
self.chooser = wpilib.SendableChooser()
self.chooser.addObject('center', 'robot_constant.CENTER_POS')
self.chooser.addObject('left', 'robot_constant.LEFT_POS')
self.chooser.addObject('right', 'robot_constant.RIGHT_POS')
self.chooser.addObject('default', 'robto_constant.DEFAULT')
wpilib.SmartDashboard.putData('Auto Mode', self.chooser)
logging.basicConfig(filename='robotLogs.log', level=logging.DEBUG)
logging.info("Init robot")
def robotInit(self):
print("init")
self.drivetrain = drivetrain.Drivetrain()
# self.motor_test = WPI_TalonSRX(3)
# self.motor_test2 = WPI_TalonSRX(4)
# self.motor5 = WPI_TalonSRX(5)
# self.motor6 = WPI_TalonSRX(6)
# self.motor7 = WPI_TalonSRX(7)
# self.motor8 = WPI_TalonSRX(8)
# self.motor0 = WPI_TalonSRX(0)
#self.motor.setInverted(True)
self.encoderCPR = 2048 * 10.71 #one wheel rotation
self.wheelDiameterFt = 6/12 #6 inches
self.rightDistanceTraveledFt = 0
self.leftDistanceTraveledFt = 0
self.goalDistanceFt = 10
import time
import gpiozero
import drivetrain
drivetrain = drivetrain.Drivetrain([gpiozero.Motor(22, 27, 17)],
[gpiozero.Motor(3, 2, 4)])
while True:
drivetrain.setBothOutputs(1, 1)
time.sleep(2)
drivetrain.setBothOutputs(-1, -1)
time.sleep(2)
alpha = frame['Azimuth'].apply(lambda x: x * (math.pi / 180)).values
planet_speed = frame['RotSpeed'].apply(lambda x: x * abs(1 - (
N_r / N_p))).values #translate rotor speed to planet speed (rpm)
torque = frame['RotTorq'].apply(lambda x: x * 1E3).values # in N-m
m_y = frame['LSSGagMys'].apply(lambda x: x * 1E3).values # in N-m
m_z = frame['LSSGagMzs'].apply(lambda x: x * 1E3).values # in N-m
# Instantiate the drivetrain object
drivetrain_model = drivetrain.Drivetrain(FF_timestep=FF_timestep,
m_c=m_c,
d_c=d_c,
m_s=m_s,
m_p=m_p,
N=N,
g=g,
beta=beta,
L_c=L_c,
L_s=L_s,
L_p=L_p,
rho=rho,
C=C,
e=e,
omega=omega)
# Calculate planet forces
startTime = datetime.now()
F, f_t, f_r, planet_speed = drivetrain_model.calc_planet_forces(
planet_speed, alpha, torque, m_y, m_z)
#planet_forces, planet_speed = drivetrain_model.calc_planet_forces(planet_speed, alpha, torque, m_y, m_z)
print('Planet Forces Calculated: ', datetime.now() - startTime)
"""__BLOCKLY__
<xml xmlns="http://www.w3.org/1999/xhtml"><block type="when_run" id="1" x="-96" y="-115"><next><block type="variables_set" id="{%~puW~O7k9e/b4afLc:"><field name="VAR">item</field><value name="VALUE"><block type="math_number" id="*.OC80%zoa9f`O]49eCU"><field name="NUM">0</field></block></value><next><block type="vexiq_Drivetrain_turn_until" id="2wC?~RW4/Lb00=tqrGI`"><field name="DIRECTION">fwd</field><value name="POWER"><block type="math_number" id="q?OOY]rtm|c@3V}o:C]b"><field name="NUM">100</field></block></value><value name="ANGLE"><block type="math_number" id="2.-lA(Cc%2g~:/8#gRmm"><field name="NUM">52.5</field></block></value><next><block type="vexiq_Drivetrain_drive_until" id="7~:?T%PfS!jL7_k+^d}I"><field name="DIRECTION">fwd</field><value name="POWER"><block type="math_number" id="VN=cLgqN7N:IBn.:io@6"><field name="NUM">100</field></block></value><value name="DISTANCE"><block type="math_number" id="[b;jQ5)mpD}ViI2!CvXW"><field name="NUM">435</field></block></value><next><block type="variables_set" id="7Dw8N)D~CKJHV~c`^Dlm"><field name="VAR">item</field><value name="VALUE"><block type="math_number" id="c)iXXV/zS#T_I)ucI^X="><field name="NUM">1</field></block></value><next><block type="vexiq_Motor_run_until_time" id="bbk/5WD=6s{lgWu(xOS/"><field name="WIDGET_ID">8</field><field name="DIRECTION">rev</field><value name="POWER"><block type="math_number" id="1*#_6:=xZ=X),4MpuOLQ"><field name="NUM">100</field></block></value><value name="TIME"><block type="math_number" id="vH)%J2bZ`^3A4cTI.{Q_"><field name="NUM">1.12</field></block></value><value name="HOLD"><block type="logic_boolean" id="N@SM2_*P(26UKTbH/D!1"><field name="BOOL">TRUE</field></block></value><next><block type="vexiq_Drivetrain_turn_until" id="{c+m0*soG)k:aVuNKq?Q"><field name="DIRECTION">fwd</field><value name="POWER"><block type="math_number" id=".)U4-Es~SaGpfUfUtj?0"><field name="NUM">50</field></block></value><value name="ANGLE"><block type="math_number" id=",eP!}TA#9^^leNlNKHqM"><field name="NUM">92</field></block></value><next><block type="vexiq_Drivetrain_drive_until" id="m#lYq0N9!]~^_;3IUbT{"><field name="DIRECTION">fwd</field><value name="POWER"><block type="math_number" id="^As0I!|(}%XTjUQ%PJ{O"><field name="NUM">100</field></block></value><value name="DISTANCE"><block type="math_number" id="zpYO.7E^IuWPY_x5G-vT"><field name="NUM">100</field></block></value><next><block type="variables_set" id="Nol/_]NiAF,P}=,mq)t0"><field name="VAR">item</field><value name="VALUE"><block type="math_number" id="eM,%9]QRl1Tw=WlHN{+R"><field name="NUM">0</field></block></value><next><block type="vexiq_Motor_run_until_time" id=")o/+w=sG4-{Tg#.odXIU"><field name="WIDGET_ID">8</field><field name="DIRECTION">fwd</field><value name="POWER"><block type="math_number" id="KU7}jz5V(./:S[/D.5zI"><field name="NUM">100</field></block></value><value name="TIME"><block type="math_number" id="bcGf8T[YKkE{QPgEyV=b"><field name="NUM">0.25</field></block></value><value name="HOLD"><block type="logic_boolean" id="dg^5k@STcflbtr~%;l!q"><field name="BOOL">TRUE</field></block></value><next><block type="vexiq_Drivetrain_drive_until" id="s#4)KPM5oeXU5jXsB*t]"><field name="DIRECTION">rev</field><value name="POWER"><block type="math_number" id="Sn7.FxeDDI;0J`kq[]*m"><field name="NUM">50</field></block></value><value name="DISTANCE"><block type="math_number" id="bCH96sz17RgT`#cwG-td"><field name="NUM">200</field></block></value></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block></next></block><block type="when_run" id="=])8A(dD9GJN},!nL^K(" x="484" y="-134"><next><block type="wait_until" id="9LYFWorpWiV7V7LX=]zG"><value name="EXPR"><block type="logic_compare" id="Q7SPEVkoz6Q]^(ez3MWY"><field name="OP">EQ</field><value name="A"><block type="variables_get" id="6zZYEIj){L*JK{j.KQVC"><field name="VAR">item</field></block></value><value name="B"><block type="math_number" id=".b.=+4#ypc+*:ri2^-b#"><field name="NUM">1</field></block></value></block></value><next><block type="vexiq_Motor_run_until_time" id="0PG=6]0uLc9r0^lCaQYe"><field name="WIDGET_ID">11</field><field name="DIRECTION">fwd</field><value name="POWER"><block type="math_number" id="=/3E?Uge,vyDM%1vEIy7"><field name="NUM">100</field></block></value><value name="TIME"><block type="math_number" id="Dt#qu!IG,U)GO97kz@*a"><field name="NUM">1.12</field></block></value><value name="HOLD"><block type="logic_boolean" id=":]|Z#Wq|qCc~OGYvn*In"><field name="BOOL">TRUE</field></block></value><next><block type="wait_until" id="_s;a_.:TXaFntOCI.yeQ"><value name="EXPR"><block type="logic_compare" id="Hca:|2VhTWeClmHfR6n-"><field name="OP">EQ</field><value name="A"><block type="variables_get" id=")-D66Pt[x`i([Weot/N["><field name="VAR">item</field></block></value><value name="B"><block type="math_number" id="nw^-GN{2;*b.D7kU8u(J"><field name="NUM">0</field></block></value></block></value><next><block type="vexiq_Motor_run_until_time" id="ldt,!Qcm/wpg|lPcbPSf"><field name="WIDGET_ID">11</field><field name="DIRECTION">rev</field><value name="POWER"><block type="math_number" id="zV@rMkyx*),/k|9CIvff"><field name="NUM">100</field></block></value><value name="TIME"><block type="math_number" id="~wHJ@1i_m;_IR^w=E45j"><field name="NUM">0.25</field></block></value><value name="HOLD"><block type="logic_boolean" id="a6[Z=:]nD*c+DA}L3y6e"><field name="BOOL">TRUE</field></block></value></block></next></block></next></block></next></block></next></block></xml>
"""
import vexiq
import sys
#region config
motor_7 = vexiq.Motor(7)
motor_8 = vexiq.Motor(8)
motor_9 = vexiq.Motor(9)
motor_11 = vexiq.Motor(11)
motor_12 = vexiq.Motor(12, True) # Reverse Polarity
import drivetrain
dt = drivetrain.Drivetrain(motor_7, motor_12, 200, 176)
#endregion config
item = None
def thread2():
global item
sys.wait_for(lambda: item == 1)
motor_11.run_until_time(100, 1.12, True)
sys.wait_for(lambda: item == 0)
motor_11.run_until_time(-(100), 0.25, True)
sys.run_in_thread(thread2)
# main thread
item = 0
