def handleEntering(self):
if (self.entering_enabled):
if (not self.started_entering):
sensors.setLeftMotor(self.entering_fwd_speed)
sensors.setRightMotor(self.entering_fwd_speed)
self.started_entering = True
else:
if (not self.getLeftAvailable()
and not self.getRightAvailable()):
# drive until left and right see the walls
# motor off may be removed
time.sleep(0.2)
sensors.setMotorOff()
print("Entered Maze")
self.gui.log_message("Entered Maze")
map.logStartPoint(self.robot.odometry.getFieldToVehicle().transformBy( \
RigidTransform2d(Translation2d(self.hallway_width/2, 0), \
Rotation2d(1, 0, False))).getTranslation())
self.state = self.Drive_State_t.HALLWAY_FOLLOWING
self.started_entering = False
self.setHeading(0)
else:
print("Entering bypassed")
self.gui.log_message("Entering Bypassed")
self.state = self.Drive_State_t.HALLWAY_FOLLOWING
map.logStartPoint(
self.robot.odometry.getFieldToVehicle().getTranslation())
self.setHeading(0)
def loop(self):
loop_counter = 0
self.current = RigidTransform2d(Translation2d(0, 0),
Rotation2d.fromDegrees(0))
while (self.enabled):
loop_counter += 1
#self.gui.log_message("robot main")
# Able to decrease sensor update frequency
if (loop_counter % 1 == 0):
sensors.updateSensors()
self.odometry.updateOdometry()
self.drive.updateDrive()
if (loop_counter % 1 == 0):
self.current = self.odometry.getFieldToVehicle()
self.gui.log_pos(self.current)
self.gui.log_sonics(sensors.getLeftWallDistance(),
sensors.getForwardWallDistance(),
sensors.getRightWallDistance())
self.gui.log_mag(sensors.getMagneticMagnitude())
self.gui.log_ir(0.0, 0.0)
self.gui.log_state(self.drive.state)
if (loop_counter >= 1000):
loop_counter = 0
time.sleep(self.cycle_time)
sensors.shutdown()
def integrateForwardKinematics(self, current_pose, left, right, heading):
rads = current_pose.getRotation().inverse().rotateBy(
heading).getRadians()
with_gyro = self.forwardKinematics(left, right, rads)
return current_pose.transformBy(
RigidTransform2d.fromVelocity(with_gyro))
def forwardKinematics(self, left, right, rads):
return RigidTransform2d.Delta((left + right) / 2, 0, rads)
def reset(self):
self.current_pos = RigidTransform2d(Translation2d(0, 0),
Rotation2d.fromDegrees(90))
self.last_left_encoder_reading = 0
self.last_right_encoder_reading = 0
def __init__(self, dt):
self.current_pos = RigidTransform2d(Translation2d(0, 0),
Rotation2d.fromDegrees(90))
self.last_left_encoder_reading = 0
self.last_right_encoder_reading = 0
self.dt = dt
def handleHazardScanning(self):
if (self.hzd_scan_state == 0):
# begins looking for hazards in all three directions, goal is to narrow down which directions need to be scanned
sensors.updateSensors()
self.hzd_lt_avail = self.getLeftAvailable()
self.hzd_rt_avail = self.getRightAvailable()
self.hzd_ft_avail = self.getFrontAvailable()
# check to ensure we are still in a maze
open_count = 0
open_count += 1 if self.hzd_lt_avail else 0
open_count += 1 if self.hzd_rt_avail else 0
open_count += 1 if self.hzd_ft_avail else 0
self.gui.log_message("3: Beginning of Hazard Scanning")
print("Left Wall: " + str(self.hzd_lt_avail) + " Fwd: " +
str(self.hzd_ft_avail) + " Right: " + str(self.hzd_rt_avail))
print("Sum: " + str(open_count))
#if(self.getLeftAvailable() or self.getRightAvailable()):
if (self.prev_all_open and open_count > 2): # >= 2
# we have exited the maze lol yeet
# exit location 1 backward and 1 to the right
self.gui.log_message("Maze Exited")
curr_rigid = self.robot.odometry.getFieldToVehicle()
# current intersection
map.removePoint(
curr_rigid.transformBy(
RigidTransform2d(Translation2d(0, -5),
Rotation2d(1, 0,
False))).getTranslation())
# previous intersection
map.removePoint(
curr_rigid.transformBy(
RigidTransform2d(
Translation2d(0, -5 - self.hallway_width),
Rotation2d(1, 0, False))).getTranslation())
map.logEndPoint(curr_rigid.transformBy( \
RigidTransform2d(Translation2d(self.hallway_width, -self.hallway_width - 5), \
Rotation2d(1, 0, False))).getTranslation())
self.state = self.Drive_State_t.DELIVERING
self.prev_all_open = open_count == 3
hazard_detection.startDirectionalScan(not self.getLeftAvailable(),
not self.getFrontAvailable(),
not self.getRightAvailable())
print("Left Haz: " + str(hazard_detection.leftHazardPresent()) +
" Front Haz: " + str(hazard_detection.frontHazardPresent()) +
" Right Haz: " + str(hazard_detection.rightHazardPresent()))
self.hzd_start_rigid = self.robot.odometry.getFieldToVehicle()
self.hzd_start_heading = self.robot.odometry.getFieldToVehicle(
).getRotation()
self.hzd_scan_state += 1
else:
# Updates all scan directions, chooses which to update based on if heading is in correct direction
hazard_detection.updateAllScans(
self.robot.odometry.getFieldToVehicle().getRotation(),
self.hzd_start_heading)
if (self.hzd_scan_state == 1):
# see whats left to scan
sensors.updateSensors()
print("Left Haz: " + str(hazard_detection.leftHazardPresent()) +
" Front Haz: " + str(hazard_detection.frontHazardPresent()) +
" Right Haz: " + str(hazard_detection.rightHazardPresent()))
#if(hazard_detection.needToScanFwd()):
if (hazard_detection.needToScanLeft()):
# turn left 90
self.hzd_turn_direction = map.Turn_Direction_Robot_t.LFT
#self.gui.log_message("Scanning Left")
elif (hazard_detection.needToScanRight()):
# turn right 90
self.hzd_turn_direction = map.Turn_Direction_Robot_t.RHT
#self.gui.log_message("Scanning Right")
else:
#completely done scanning!!!
#self.gui.log_message("4: Done scanning, turning to exit direction")
if (self.hzd_rt_avail
and not hazard_detection.rightHazardPresent()):
self.hzd_turn_direction = map.Turn_Direction_Robot_t.RHT
self.gui.log_message("Turning Right")
elif (self.hzd_ft_avail
and not hazard_detection.frontHazardPresent()):
self.hzd_turn_direction = map.Turn_Direction_Robot_t.FWD
self.gui.log_message("Turning Forward")
elif (self.hzd_lt_avail
and not hazard_detection.leftHazardPresent()):
self.hzd_turn_direction = map.Turn_Direction_Robot_t.LFT
self.gui.log_message("Turning Left")
else:
# no available dirs, turn around
self.hzd_turn_direction = map.Turn_Direction_Robot_t.BCK
self.gui.log_message("Turning Around")
#heading_offset = self.hzd_start_heading.inverse().rotateBy(self.robot.odometry.getFieldToVehicle().getRotation())
heading_change = self.robot.odometry.getFieldToVehicle(
).getRotation().inverse().rotateBy(
self.hzd_start_heading.rotateBy(
Rotation2d.fromDegrees(
self.hzd_turn_direction))).getDegrees()
#heading_change = Rotation2d.fromDegrees(self.hzd_turn_direction).rotateBy(heading_offset).getDegrees()
#heading_change = round((self.hzd_turn_direction - heading_offset)/90)*90
heading_change = round(heading_change / 90) * 90
print("Heading Change: " + str(heading_change))
if (math.fabs(heading_change) > 45):
# still need to change direction
self.setHeading(heading_change)
self.hzd_scan_state += 1
mult = 1
if (heading_change < 0):
mult = -1
sensors.setLeftMotor(-self.int_turn_speed * mult)
sensors.setRightMotor(self.int_turn_speed * mult)
else:
# we are already at the deired location!
# move to last step in hazard scanning
self.hzd_scan_state += 2
elif (self.hzd_scan_state == 2):
# Turning
if (math.fabs(self.getHeadingError()) <= self.int_turn_tolerance):
# turning complete
#self.gui.log_message("Scan Turn Complete")
sensors.setMotorOff()
self.hzd_scan_state = 1
elif (self.hzd_scan_state == 3):
# log the hazards and scram to the exit intersection mode
#self.gui.log_message("5: Done with hazard detection")
hazard_detection.logAllScans(self.hzd_start_rigid)
self.state = self.Drive_State_t.INTERSECTION_EXITING
self.hzd_scan_state = 0
def handleHallwayFollowing(self):
if (self.intersection_enabled):
if (hazard_detection.irHazardExists()
or hazard_detection.magHazardExists()):
# jeepers, get outta here!
if (hazard_detection.irHazardExists()):
map.logHeatSource(
self.robot.odometry.getFieldToVehicle().transformBy(
RigidTransform2d(
Translation2d(
hazard_detection.getIRDistanceLeft(), 0),
Rotation2d(1, 0, False))).getTranslation(),
sensors.getIRLevelLeft())
self.gui.log_message("IR Detected, turning around")
if (hazard_detection.magHazardExists()):
map.logMagneticSource(
self.robot.odometry.getFieldToVehicle().transformBy(
RigidTransform2d(
Translation2d(
hazard_detection.getIRDistanceLeft(), 0),
Rotation2d(1, 0, False))).getTranslation(),
sensors.getMagneticMagnitude())
self.gui.log_message("Mag Detected, turning around")
self.state = self.Drive_State_t.TURNING_AROUND
sensors.setMotorOff()
time.sleep(1)
return
if (self.getLeftAvailable() or self.getRightAvailable()):
sensors.setMotorOff()
fwd_dist = sensors.getForwardWallDistance()
#if(fwd_dist < 35):
# self.intersection_forward_dist = fwd_dist - 12
#else:
self.intersection_forward_dist = self.int_side_fwd_dist
self.state = self.Drive_State_t.INTERSECTION_ENTERING
self.gui.log_message("Left or right path open")
time.sleep(1)
return
elif (not self.getFrontAvailable()):
self.intersection_forward_dist = self.int_front_fwd_dist
self.state = self.Drive_State_t.INTERSECTION_ENTERING
self.gui.log_message("Forward closed")
sensors.setMotorOff()
time.sleep(1)
return
# >0 if too far to the right, <0 if too far left
# if error >0 => correct by increasing right speed
error = sensors.getLeftWallDistance() - sensors.getRightWallDistance()
# use error to control difference in motor speed
pError = error * self.kP * self.hallway_speed
if (math.fabs(pError) > self.kLimit):
pError = self.kLimit * (pError / math.fabs(pError))
leftSpeed = self.hallway_speed
rightSpeed = leftSpeed
if (pError > 0):
rightSpeed += pError
else:
leftSpeed -= pError
# leftSpeed = self.hallway_speed - pError
# rightSpeed = self.hallway_speed + pError
heading_error = self.getHeadingError()
lSpeed = self.hallway_speed - heading_error * self.kPHeading
rSpeed = self.hallway_speed + heading_error * self.kPHeading
#sensors.setLeftMotor(leftSpeed)
#sensors.setRightMotor(rightSpeed)
sensors.setLeftMotor(lSpeed)
sensors.setRightMotor(rSpeed)