def measure_position(self, initial_call):
if initial_call:
if self.vision.x != 0.0:
measure_trajectory = generate_trapezoidal_trajectory(
self.bno055.getHeading(),
0, self.bno055.getHeading()
+ self.vision.derive_vision_angle(), 0,
self.rotate_velocity, self.rotate_accel_speed, -self.rotate_accel_speed/2)
print("vision_x %s, vision_angle %s, heading %s, heading_start %s, heading_end %s" % (
self.vision.x, self.vision.derive_vision_angle(), self.bno055.getHeading(), measure_trajectory[0][0], measure_trajectory[-1][0]))
self.profilefollower.modify_queue(heading=measure_trajectory, overwrite=True)
self.profilefollower.execute_queue()
if not self.profilefollower.queue[0]:
# now measure our position relative to the targets
r = (self.range_finder.getDistance() + self.centre_to_front_bumper
- self.lidar_to_front_bumper)
current_heading = self.bno055.getHeading()
self.displacement_error = -(
(r * math.sin(current_heading-self.perpendicular_heading))/
math.sin(math.pi-current_heading))
print("vision_x: %s, range: %s, heading %s, displacement_error %s, raw_range %s" %
(self.vision.x, r, current_heading, self.displacement_error, self.range_finder.getDistance()))
if abs(self.displacement_error) < self.peg_align_tolerance:
print("WITHIN TOLERANCE, SKIPPING...")
self.next_state("rotate_towards_peg")
else:
self.next_state("rotate_straight")
def createObjects(self):
'''Create motors and stuff here'''
# Objects that are created here are shared with all classes
# that declare them. For example, if I had:
# self.elevator_motor = wpilib.TalonSRX(2)
# here, then I could have
# class Elevator:
# elevator_motor = wpilib.TalonSRX
# and that variable would be available to both the MyRobot
# class and the Elevator class. This "variable injection"
# is especially useful if you want to certain objects with
# multiple different classes.
# create the imu object
self.bno055 = BNO055()
# the "logger" - allows you to print to the logging screen
# of the control computer
self.logger = logging.getLogger("robot")
# the SmartDashboard network table allows you to send
# information to a html dashboard. useful for data display
# for drivers, and also for plotting variables over time
# while debugging
self.sd = NetworkTable.getTable('SmartDashboard')
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = XboxController(1)
self.pressed_buttons_js = set()
self.pressed_buttons_gp = set()
self.drive_motor_a = CANTalon(2)
self.drive_motor_b = CANTalon(5)
self.drive_motor_c = CANTalon(4)
self.drive_motor_d = CANTalon(3)
self.gear_alignment_motor = CANTalon(14)
self.winch_motor = CANTalon(11)
self.winch_motor.setInverted(True)
self.rope_lock_solenoid = wpilib.DoubleSolenoid(forwardChannel=0,
reverseChannel=1)
# self.rope_lock_solenoid = wpilib.DoubleSolenoid(forwardChannel=3,
# reverseChannel=4)
self.gear_push_solenoid = wpilib.Solenoid(2)
self.gear_drop_solenoid = wpilib.Solenoid(3)
# self.gear_push_solenoid = wpilib.DoubleSolenoid(forwardChannel=1, reverseChannel=2)
# self.gear_drop_solenoid = wpilib.Solenoid(0)
self.test_trajectory = generate_trapezoidal_trajectory(
0, 0, 3, 0, Chassis.max_vel, 1, -1, Chassis.motion_profile_freq)
self.throttle = 1.0
self.direction = 1.0
self.led_dio = wpilib.DigitalOutput(1)
self.compressor = wpilib.Compressor()
def rotate_towards_airship(self, initial_call):
if initial_call:
rotate = generate_trapezoidal_trajectory(
self.bno055.getHeading(), 0, self.perpendicular_heading, 0, self.rotate_velocity,
self.rotate_accel_speed, -self.rotate_accel_speed/2)
self.profilefollower.modify_queue(heading=rotate, overwrite=True)
print("Rotate Start %s, Rotate End %s" % (rotate[0], rotate[-1]))
self.profilefollower.execute_queue()
if not self.profilefollower.queue[0]:
self.next_state("measure_position")
def roll_back(self, initial_call):
if initial_call:
roll_back = generate_trapezoidal_trajectory(
0, 0, -1, 0, self.displace_velocity,
self.displace_accel, -self.displace_decel)
self.profilefollower.modify_queue(self.perpendicular_heading,
linear=roll_back, overwrite=True)
self.profilefollower.execute_queue()
if not self.profilefollower.queue[0]:
self.done()
def drive_align_segment(self, initial_call):
if initial_call:
displacement_correction = generate_trapezoidal_trajectory(
0, 0, self.displacement_error, 0, self.displace_velocity,
self.displace_accel, -self.displace_decel,
Chassis.motion_profile_freq)
self.profilefollower.modify_queue(0,
linear=displacement_correction)
self.profilefollower.execute_queue()
if not self.profilefollower.executing:
self.next_state("rotate_towards_airship")
def drive_to_wall(self, initial_call):
if initial_call:
self.profilefollower.stop()
if self.target == Targets.Centre:
peg_range = self.centre_airship_distance
else:
peg_range = 1.5
peg_range -= self.centre_to_front_bumper
r = self.range_filter.range
print("AUTO DRIVE WALL RANGE: %s" %
(self.range_finder.getDistance()))
print("AUTO DRIVE WALL FILTER RANGE: %s" %
(self.range_filter.range))
# 40 is range finder max distance, better failure mode than inf or really small
if not math.isfinite(r):
r = 40
elif r < 0.5:
r = 40
to_peg = None
if r > (2 if self.target != Targets.Centre else 4):
print("DEAD RECKON AUTO")
to_peg = generate_trapezoidal_trajectory(
0, 0, peg_range + 0.1, 0, self.displace_velocity,
self.displace_accel, -self.displace_decel,
Chassis.motion_profile_freq)
else:
print("RANGE AUTO")
to_peg = generate_trapezoidal_trajectory(
0, 0,
self.range_finder.getDistance() -
self.lidar_to_front_bumper + 0.1, 0,
self.displace_velocity, self.displace_accel,
-self.displace_decel, Chassis.motion_profile_freq)
self.profilefollower.modify_queue(self.bno055.getHeading(),
linear=to_peg,
overwrite=True)
self.profilefollower.execute_queue()
self.manipulategear.engage()
if not self.profilefollower.executing:
self.next_state("deploying_gear")
def roll_back(self, initial_call):
if initial_call:
self.profilefollower.stop()
roll_back = generate_trapezoidal_trajectory(
0, 0, -2, 0, self.displace_velocity, self.displace_accel / 2,
-self.displace_decel, Chassis.motion_profile_freq)
self.profilefollower.modify_queue(self.bno055.getHeading(),
linear=roll_back,
overwrite=True)
self.profilefollower.execute_queue()
elif not self.profilefollower.executing:
self.done()
def rotate_straight(self, initial_call):
# Drive from the range that vision and the lidar can detect the wall
# to the wall itself
if initial_call:
current_heading = self.bno055.getHeading()
self.rotate_to_correct = generate_trapezoidal_trajectory(
current_heading, 0, 0, 0, self.rotate_velocity,
self.rotate_accel_speed, -self.rotate_accel_speed / 2,
Chassis.motion_profile_freq)
self.profilefollower.modify_queue(heading=self.rotate_to_correct)
self.profilefollower.execute_queue()
if not self.profilefollower.executing:
self.next_state("drive_align_segment")
def drive_to_wall(self, initial_call):
if initial_call:
to_peg = generate_trapezoidal_trajectory(
0, 0, self.range_finder.getDistance()-self.lidar_to_front_bumper,
0,
self.displace_velocity,
self.displace_accel, -self.displace_decel*2)
self.profilefollower.modify_queue(self.perpendicular_heading,
linear=to_peg, overwrite=True)
self.profilefollower.execute_queue()
self.manipulategear.engage()
if not self.manipulategear.is_executing:
self.next_state("roll_back")
def rotate_towards_peg(self, initial_call):
if initial_call:
if self.vision.x != 0.0:
measure_trajectory = generate_trapezoidal_trajectory(
self.bno055.getHeading(),
0, self.bno055.getHeading()
+ self.vision.derive_vision_angle(), 0,
self.rotate_velocity, self.rotate_accel_speed, -self.rotate_accel_speed/2)
print("vision_x %s, vision_angle %s, heading %s, heading_start %s, heading_end %s" % (
self.vision.x, self.vision.derive_vision_angle(), self.bno055.getHeading(), measure_trajectory[0][0], measure_trajectory[-1][0]))
self.profilefollower.modify_queue(heading=measure_trajectory, overwrite=True)
self.profilefollower.execute_queue()
# self.done()
if not self.profilefollower.queue[0]:
self.next_state("drive_to_wall")
def drive_to_airship(self, initial_call):
# Drive to a range where we can close the loop using vision, lidar and
# gyro to close the loop on position
if initial_call:
self.vision.vision_mode = True
displace = generate_trapezoidal_trajectory(
0, 0, self.dr_displacement,
0, self.displace_velocity,
self.displace_accel, -self.displace_decel)
self.profilefollower.modify_queue(heading=0, linear=displace)
self.profilefollower.execute_queue()
if not self.profilefollower.queue[0]:
if self.target is Targets.Centre:
self.next_state("rotate_towards_peg")
else:
self.next_state("rotate_towards_airship")
def forward_open(self, initial_call, state_tm):
self.put_dashboard()
self.geardepositiondevice.drop_gear()
self.chassis.input_enabled = True
if initial_call:
self.profilefollower.stop()
roll_back = generate_trapezoidal_trajectory(
0, 0, -0.3, 0, 2,
1.0, -2, 50)
self.profilefollower.modify_queue(self.bno055.getHeading(),
linear=roll_back, overwrite=True)
self.profilefollower.execute_queue()
if initial_call:
self.initial_distances = self.chassis.get_raw_wheel_distances()
if ((abs(abs(self.initial_distances[0]) - abs(self.chassis.get_raw_wheel_distances()[0]))
+abs(abs(self.initial_distances[1]) - abs(self.chassis.get_raw_wheel_distances()[1])))
/ 2 > self.move_back_close_tol):
self.next_state_now("backward_open")