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 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 updateAllScans(heading, start_heading):
IR = sensors.getIRLevelLeft()
x, y, Mag_z = sensors.getMagneticLevel()
Mag_Mag = sensors.getMagneticMagnitude()
heading_delta = start_heading.inverse().rotateBy(heading)
if (math.fabs(start_heading.inverse().rotateBy(heading).getDegrees()) <
scan_range):
# In range for front
updateFwdScan(IR, Mag_Mag, Mag_Mag, heading) #heading_delta)
#print("Updating FRONT SCAN")
elif(math.fabs(start_heading.rotateBy(Rotation2d(0,1,False)).inverse()\
.rotateBy(heading).getDegrees()) < scan_range):
# In range for left
updateLeftScan(IR, Mag_Mag, Mag_Mag, heading) #heading_delta)
#print("Updating LEFT SCAN")
elif(math.fabs(start_heading.rotateBy(Rotation2d(0,-1,False)).inverse()\
.rotateBy(heading).getDegrees()) < scan_range):
# In range for right
updateRightScan(IR, Mag_Mag, Mag_Mag, heading) #heading_delta)
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 getMagneticDistance():
return getMagneticDistanceFromReading(sensors.getMagneticMagnitude())
def magHazardExists():
#x, y, z = sensors.getMagneticLevel()
return math.fabs(sensors.getMagneticMagnitude()) > mag_possible_threshold
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)
try:
while (navigating):
sensors.updateSensors()
odometry.updateOdometry()
current = odometry.getFieldToVehicle().getTranslation()
goal_offset = current.inverse().translateBy(goal_location)
heading_to_goal = Rotation2d(goal_offset.getX(), goal_offset.getY(),
True)
current_heading = odometry.getFieldToVehicle().getRotation()
heading_error = current_heading.inverse().rotateBy(
heading_to_goal).getDegrees()
mag = sensors.getMagneticMagnitude()
mag_error = 0
if (mag > error_mag):
mag_error = mag - resting_mag
print("mag error")
sensors.setLeftMotor(speed - heading_error * kP * speed +
mag_error * kMP)
sensors.setRightMotor(speed + heading_error * kP * speed -
mag_error * kMP)
#detect if within radius
if (goal_offset.norm() <= error_radius):
print("Navigating to point")
sensors.setMotorOff()
navigating = False