fastParams.pidParameters.k_p = 270 fastParams.pidParameters.k_i = 400 fastParams.pidParameters.k_d = 160 # (273,21,-90), (525,21,-90), (21,21,-90) startTime = datetime.now() measurements = robot.turnSonarTakingMeasurements() # Change sonar to move FASTAAAAAAAAAAAAR! robot.setMotorAngleControllerParameters(robot.sonar_motor, fastParams) # place.get_Loc(measurements) robot.sonarSpin(-90) toturn = robot.getMeanAngle(measurements) -13 toturn = toturn if toturn <= 180 else toturn - 360 robot.turnLeftDeg(toturn) robot.forward(disout) #### ***Decide location sonarRight = robot.getSonarMeasurements(1)[0] robot.sonarSpin(-180)
import sys sys.path.insert(0, '../') from robot import robot robot = robot(0, 0, 0, True, True) robot.enableSonar() measurements = [63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 65, 65, 65, 65, 65, 65, 67, 67, 67, 67, 67, 67, 67, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 26, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 27, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 30, 30, 30, 30, 30, 30, 68, 68, 68, 68, 68, 68, 68, 65, 65, 65, 65, 65, 65, 65, 65, 65, 65, 65, 65, 65, 65, 65, 65, 65, 65, 65, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63, 63] print measurements print robot.getMeanAngle(measurements) robot.terminate()
fastParams.pidParameters.k_p = 270 fastParams.pidParameters.k_i = 400 fastParams.pidParameters.k_d = 160 # (273,21,-90), (525,21,-90), (21,21,-90) startTime = datetime.now() measurements = robot.turnSonarTakingMeasurements() # Change sonar to move FASTAAAAAAAAAAAAR! robot.setMotorAngleControllerParameters(robot.sonar_motor, fastParams) # place.get_Loc(measurements) robot.sonarSpin(-90) toturn = robot.getMeanAngle(measurements) - 13 toturn = toturn if toturn <= 180 else toturn - 360 robot.turnLeftDeg(toturn) robot.forward(disout) #### ***Decide location sonarRight = robot.getSonarMeasurements(1)[0] robot.sonarSpin(-180) sonarLeft = robot.getSonarMeasurements(1)[0] if sonarRight > 30: if sonarLeft > 30: