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