def forwards(self, initial_call, state_tm):
self.found = False
SmartDashboard.putBoolean("Found", False)
if initial_call:
self.drive.reset_encoders()
self.logger.info(
SmartDashboard.getString("auto_position/selected"))
if SmartDashboard.getString("auto_position/selected",
"Position 1") is "Position 2":
distance = self.middle_peg_distance
else:
distance = self.initial_distance
if self.drive.drive_distance(distance, speed=0.25):
self.drive.stop()
self.next_state('turn')
def turn(self):
station = SmartDashboard.getString("auto_position/selected",
"Position 1")
SmartDashboard.putBoolean("run_vision", False)
if station is "Position 1":
self.next_state('turn_right')
elif station is "Position 3":
self.next_state('turn_left')
else:
self.next_state('approach')
def periodic(self):
self.getEncoders()
self.driveType = SmartDashboard.getString("DriveStyle", "Tank")
