def handleIntersectionEntering(self):
if (self.int_enter_state == 0):
# first call
self.gui.log_message("1: Centering into square")
# begin driving forward
if (self.intersection_forward_dist != 0):
# Don't move if the dist is 0!
sensors.setLeftMotor(self.int_st_speed)
sensors.setRightMotor(self.int_st_speed)
self.int_start_pos = self.robot.odometry.getFieldToVehicle()
self.int_enter_state += 1
elif (self.int_enter_state == 1):
if (self.int_start_pos.inverse().transformBy(
self.robot.odometry.getFieldToVehicle()).getTranslation().
norm() >= self.intersection_forward_dist):
# the current position should be approximately the center of the intersection
sensors.setMotorOff()
sensors.updateSensors()
time.sleep(1)
#self.gui.log_message("2: Centered, Searching For Hazards")
# Log current pos as an intersection point
map.logIntersection(
self.robot.odometry.getFieldToVehicle().getTranslation())
# Transition to hazard search
self.state = self.Drive_State_t.HAZARD_SCANNING
self.int_enter_state = 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 setHeading(self, offset):
sensors.updateSensors()
deg = self.robot.odometry.getFieldToVehicle().getRotation().getDegrees(
)
deg = round(deg / 90) * 90
self.heading_setpoint = Rotation2d.fromDegrees(deg).rotateBy(
Rotation2d.fromDegrees(offset))
print("Heading set to: " + str(self.heading_setpoint))
def startDirectionalScan(left_wall, fwd_wall, right_wall):
sensors.updateSensors()
global left_haz, fwd_haz, right_haz
left_haz = HazardDirection(left_wall)
fwd_haz = HazardDirection(fwd_wall)
right_haz = HazardDirection(right_wall)
fwd_haz.possible_mag = HazardDirection.DetectionState.PRES if magHazardExists(
) and not fwd_wall else HazardDirection.DetectionState.NOTPRES
fwd_haz.possible_ir = HazardDirection.DetectionState.PRES if irHazardExists(
) and not fwd_wall else HazardDirection.DetectionState.NOTPRES
def baseOnDistanceComponentsIR(): #Components
#initialization
distLeftX = 0
distLeftY = 0
distRightX = 0
distRightY = 0
leftDistsX = []
leftDistsY = []
rightDistsX = []
rightDistsY = []
leftData = []
rightData = []
entries = 0
#Collects Data
while (distLeftX != -1):
distLeftX = float(
input(
"please enter the X (horizontal, robot left = +) component distance away from the hazard the left IR sensor is or enter -1 to end: "
))
if (distLeftX != -1):
distLeftY = float(
input(
"please enter the Y component distance away from the hazard the left IR sensor is or enter -1 to end: "
))
distRightX = distLeftX - ir_spacing # input("please enter the X component distance away from the hazard the Right IR sensor is: ")
distRightY = distLeftY # input("please enter the Y component distance away from the hazard the Right IR sensor is: ")
leftDistsX.append(distLeftX)
leftDistsY.append(distLeftY)
rightDistsX.append(distRightX)
rightDistsY.append(distRightY)
sensors.updateSensors()
leftData.append(sensors.getIRLevelLeft())
rightData.append(sensors.getIRLevelRight())
entries = entries + 1
#output to CSV
data = open("IR_Distance_Component_Data.csv", "w")
data.write("Left IR Distance (X),")
data.write("Left IR Distance (Y),")
data.write("Left IR Reading,")
data.write("Right IR Distance (X),")
data.write("Right IR Distance (Y),")
data.write("Right IR Reading\n")
i = 0
while (i < entries):
data.write("%.1f cm," % leftDistsX[i])
data.write("%.1f cm," % leftDistsY[i])
data.write("%.3f ," % leftData[i])
data.write("%.1f cm," % rightDistsX[i])
data.write("%.1f cm," % rightDistsY[i])
data.write("%.3f \n" % rightData[i])
i = i + 1
def device_twin_callback(update_state, payload, user_context):
global TWIN_CALLBACKS
print("")
print ( "")
print ( "Twin callback called with: - Called from client")
print ( "updateStatus: %s" % update_state )
print ( "context: %s" % user_context )
print ( "payload: %s" % payload )
TWIN_CALLBACKS += 1
print ( "Total calls confirmed: %d\n" % TWIN_CALLBACKS )
payloadDict = json.loads(payload)
s.updateSensors(payloadDict)
def handleIntersectionExiting(self):
if (self.int_exit_state == 0):
# drive forward to exit the intersection
sensors.updateSensors()
self.gui.log_message("6: Exiting Intersection")
sensors.setLeftMotor(self.int_st_speed -
self.getHeadingError() * self.kPHeading)
sensors.setRightMotor(self.int_st_speed +
self.getHeadingError() * self.kPHeading)
self.int_start_pos = self.robot.odometry.getFieldToVehicle()
self.left_was_available = self.getLeftAvailable()
self.right_was_available = self.getRightAvailable()
self.int_exit_state += 1
elif (self.int_exit_state == 1):
# Exiting Intersection State
sensors.setLeftMotor(self.int_st_speed -
self.getHeadingError() * self.kPHeading)
sensors.setRightMotor(self.int_st_speed +
self.getHeadingError() * self.kPHeading)
# continue to check for forward wall
if (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 while exiting intersection")
sensors.setMotorOff()
self.int_exit_state = 0
time.sleep(1)
return
# check for a change in state of the sonic from wall to no wall (sides)
if ((not self.left_was_available and self.getLeftAvailable()) or
(not self.right_was_available and self.getRightAvailable())):
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 while exiting intersection")
sensors.setMotorOff()
self.int_exit_state = 0
time.sleep(1)
return
# distance between current pos and start pos
if (self.int_start_pos.inverse().transformBy(
self.robot.odometry.getFieldToVehicle()).getTranslation().
norm() >= self.int_exit_distance):
sensors.setMotorOff()
sensors.updateSensors()
# Keep going, you can do it little buddy!
self.gui.log_message("7: Continuing Hallway Following")
self.int_exit_state = 0
self.setHeading(0)
self.state = self.Drive_State_t.HALLWAY_FOLLOWING
def baseOnDistanceComponentsIMU(): #Magnitude
#initialization
distX = 0
distY = 0
magDataX = [] #x component reading
magDataY = [] #y component reaing
magDataZ = [] #z component reaing
magDataMag = [] #magnitude reading
distsX = []
distsY = []
entries = 0
#collects data
while (distX != -1):
distX = float(
input(
"please enter the X (horizontal, robot left = +) component distance away from the hazard the IMU is or enter -1 to end: "
))
if (distX != -1):
distY = float(
input(
"please enter the Y component distance away from the hazard the IMU is or enter -1 to end: "
))
distsX.append(distX)
distsY.append(distY)
sensors.updateSensors()
x, y, z = sensors.getMagneticLevel(
) #x, y, amd z component magnetic Readings
magDataX.append(x)
magDataY.append(y)
magDataZ.append(z)
magDataMag.append(sensors.getMagneticMagnitude())
entries = entries + 1
#outputs to CSV
data = open("IMU_Distance_Component_Data.csv", "w")
data.write("IMU distance (X),")
data.write("IMU distance (Y),")
data.write("x comp Mag reading,")
data.write("y comp Mag reading,")
data.write("z comp Mag reading,")
data.write("Magnitude Mag reading \n")
i = 0
while (i < entries):
data.write("%.1f cm," % distsX[i])
data.write("%.1f cm," % distsY[i])
data.write("%.3f ," % magDataX[i])
data.write("%.3f ," % magDataY[i])
data.write("%.3f ," % magDataZ[i])
data.write("%.3f \n" % magDataMag[i])
i = i + 1
def baseOnDistanceIR(): #Magnitude
#initialization
distLeft = 0
distRight = 0
leftDists = []
rightDists = []
leftData = []
rightData = []
entries = 0
#Collects Data
while (distLeft != -1):
distLeft = float(
input(
"please enter the distance away from the hazard the left IR sensor is or enter -1 to end: "
))
if (distLeft != -1):
distRight = float(
input(
"please enter the distance away from the hazard the Right IR sensor is: "
))
leftDists.append(distLeft)
rightDists.append(distRight)
sensors.updateSensors()
leftData.append(sensors.getIRLevelLeft())
rightData.append(sensors.getIRLevelRight())
entries = entries + 1
#output to CSV
data = open("IR_Distance_Magnitude_Data.csv", "w")
data.write("Left IR Distance,")
data.write("Left IR Reading,")
data.write("Right IR Distance,")
data.write("Right IR Reading")
i = 0
while (i < entries):
data.write("%.1f cm," % leftDists[i])
data.write("%.3f ," % leftData[i])
data.write("%.1f cm," % rightDists[i])
data.write("%.3f \n" % rightData[i])
i = i + 1
def baseOnDistanceIMU(): #Magnitude
#initialization
distMag = 0
magDataX = [] #x component reading
magDataY = [] #y component reaing
magDataMag = [] #magnitude reading
dists = []
entries = 0
#Collects Data
while (distMag != -1):
distMag = float(
input(
"please enter the distance away from the hazard the IMU is or enter -1 to end: "
))
if (distMag != -1):
dists.append(distMag)
sensors.updateSensors()
x, y, z = sensors.getMagneticLevel(
) #x, y, amd z component magnetic Readings
magDataX.append(x)
magDataY.append(y)
entries = entries + 1
#output to CSV
data = open("IMU_Distance_Magnitude_Data.csv", "w")
data.write("IMU distance,")
data.write("x comp Mag reading,")
data.write("y comp Mag reading,")
data.write("Magnitude Mag reading\n")
i = 0
while (i < entries):
data.write("%.1f cm," % dists[i])
data.write("%.3f ," % magDataX[i])
data.write("%.3f ," % magDataY[i])
data.write("%.3f \n" % magDataMag[i])
i = i + 1
import sensors
import time
file_name = 'max_mag.txt'
sensors.initSensors()
max_val = 0
sensors.setLeftMotor(-0.15)
sensors.setRightMotor(0.15)
try:
while True:
sensors.updateSensors()
curr = sensors.getMagneticMagnitude()
max_val = max(curr, max_val)
time.sleep(0.01)
except KeyboardInterrupt:
print("Done Scanning")
print("Max Val: ")
print(max_val)
sensors.shutdown()
with open(file_name, 'w') as f:
f.write(str(max_val))
def setHeading(offset):
global heading_setpoint
sensors.updateSensors()
deg = odometry.getFieldToVehicle().getRotation().getDegrees()
heading_setpoint = Rotation2d.fromDegrees(deg).rotateBy(Rotation2d.fromDegrees(offset))
print("Heading set to: " + str(heading_setpoint))
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