def read(self, first_address, count):
data = [first_address, count]
data.append(crc7(data))
with self.mutex:
retcount = self.port.write(data)
if retcount != len(data):
raise IOError("Write error (%s != %s)" % (retcount, len(data)))
# FIXME
if not hal.isSimulation():
Timer.delay(0.001)
data = self.port.read(True, count + 1)
if len(data) != count + 1:
raise IOError("Read error (%s != %s)" % (len(data), count+1))
crc = data[-1]
data = data[:-1]
if crc7(data) != crc:
raise IOError("CRC error")
return data
def read(self, first_address, count):
data = [first_address, count]
data.append(crc7(data))
with self.mutex:
retcount = self.port.write(data)
if retcount != len(data):
raise IOError("Write error (%s != %s)" % (retcount, len(data)))
# FIXME
if not hal.isSimulation():
Timer.delay(0.001)
data = self.port.read(True, count + 1)
if len(data) != count + 1:
raise IOError("Read error (%s != %s)" % (len(data), count + 1))
crc = data[-1]
data = data[:-1]
if crc7(data) != crc:
raise IOError("CRC error")
return data
def wait(self):
""" Waits until the delay period has passed """
# we must *always* yield here, so other things can run
Timer.delay(0.001)
while not self.timer.hasPeriodPassed(self.delay_period):
Timer.delay(0.001)
def move_arm_to_30(self):
'''while(self.arm.getPOT() <= 30):
self.arm.armAuto(1,0,30,rate=0.7)
self.arm.armAuto(0,0,30)
'''
Timer.delay(3)
self.next_state('drive_forward_step_3')
def move_arm_to_0(self):
'''while(self.arm.getPOT() >= 0.5):
self.arm.armAuto(0,1,0,rate=0.5)
self.arm.armAuto(0,0,0.5)
'''
Timer.delay(3)
self.next_state('drive_forward_step_2')
def configure_controllers(self, pid_only=False):
'''Set the PIDs, etc for the controllers, slot 0 is position and slot 1 is velocity'''
error_list = []
if not pid_only:
controllers = [self.spark_neo_left_front, self.spark_neo_left_rear, self.spark_neo_right_front, self.spark_neo_right_rear]
for controller in controllers:
#error_list.append(controller.restoreFactoryDefaults())
#Timer.delay(0.01)
#looks like they orphaned the setIdleMode - it doesn't work. Try ConfigParameter
#error_list.append(controller.setIdleMode(rev.IdleMode.kBrake))
controller.setParameter(rev.ConfigParameter.kIdleMode, rev.IdleMode.kBrake)
error_list.append(controller.setSmartCurrentLimit(self.current_limit))
controller.setParameter(rev.ConfigParameter.kSmartMotionMaxAccel_0, self.maxacc)
controller.setParameter(rev.ConfigParameter.kSmartMotionMaxAccel_1, self.maxacc)
controller.setParameter(rev.ConfigParameter.kSmartMotionMaxVelocity_0, self.maxvel)
controller.setParameter(rev.ConfigParameter.kSmartMotionMaxVelocity_1, self.maxvel)
Timer.delay(0.01)
#controller.burnFlash()
controllers = [self.spark_neo_left_front, self.spark_neo_left_rear, self.spark_neo_right_front, self.spark_neo_right_rear]
for controller in controllers:
error_list.append(controller.setParameter(rev.ConfigParameter.kP_0, self.PID_dict_pos['kP']))
error_list.append(controller.setParameter(rev.ConfigParameter.kP_1, self.PID_dict_vel['kP']))
error_list.append(controller.setParameter(rev.ConfigParameter.kI_0, self.PID_dict_pos['kI']))
error_list.append(controller.setParameter(rev.ConfigParameter.kI_1, self.PID_dict_vel['kI']))
error_list.append(controller.setParameter(rev.ConfigParameter.kD_0, self.PID_dict_pos['kD']))
error_list.append(controller.setParameter(rev.ConfigParameter.kD_1, self.PID_dict_vel['kD']))
error_list.append(controller.setParameter(rev.ConfigParameter.kIZone_0, self.PID_dict_pos['kIz']))
error_list.append(controller.setParameter(rev.ConfigParameter.kIZone_1, self.PID_dict_vel['kIz']))
error_list.append(controller.setParameter(rev.ConfigParameter.kF_0, self.PID_dict_pos['kFF']))
error_list.append(controller.setParameter(rev.ConfigParameter.kF_1, self.PID_dict_vel['kFF']))
error_list.append(controller.setParameter(rev.ConfigParameter.kOutputMax_0, self.PID_dict_pos['kMaxOutput']))
error_list.append(controller.setParameter(rev.ConfigParameter.kOutputMax_1, self.PID_dict_vel['kMaxOutput']))
error_list.append(controller.setParameter(rev.ConfigParameter.kOutputMin_0, self.PID_dict_pos['kMinOutput']))
error_list.append(controller.setParameter(rev.ConfigParameter.kOutputMin_1, self.PID_dict_vel['kMinOutput']))
#controller.burnFlash()
Timer.delay(0.02)
# if 1 in error_list or 2 in error_list:
# print(f'Issue in configuring controllers: {error_list}')
# else:
print(f'Results of configuring controllers: {error_list}')
def disabled(self):
while self.isDisabled():
Timer.delay(0.01)
def autonomous(self):
self.autonomous_modes.run(CONTROL_LOOP_WAIT_TIME,
iter_fn=self.update_all)
Timer.delay(CONTROL_LOOP_WAIT_TIME)
def disabled(self):
while self.isDisabled():
self.update_networktables()
Timer.delay(0.01)
