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")