from modul import motor
try:
i2cHandler = i2cHandler.I2cHandler()
i2cHandler.start()
motor = motor.motor()
motor.start()
velocityUtilities = velocityUtilities.velocityUtilities(i2cHandler)
while (True):
velocity = velocityUtilities.getVelocity()
print(velocity)
motor.setVelocityLeft(velocity)
motor.setVelocityRight(velocity)
sleep(0.1)
i2cHandler.terminate()
except KeyboardInterrupt:
i2cHandler.terminate()
motor.terminate()
print("Goodbye!")
except:
i2cHandler.terminate()
motor.terminate()
print("Aaaaaargh!")
raise
pid_angle.SetPoint = 0.0
while (True):
error_dist = i2c.getDistanceLeftFront() - i2c.getDistanceRightFront()
error_angle = atan(
(i2c.getDistanceLeftFront() - i2c.getDistanceLeftBack()) / 180)
print("\033c")
print("DIST: " + str(error_dist))
print("ANGL: " + str(error_angle))
if pid_dist.update(error_dist):
print("pid_dist: " + str(pid_dist.output))
pid_angle.SetPoint = pid_dist.output
if pid_angle.update(error_angle):
# motor.setVelocityLeft(config.guidedDriveVelocity * (1 + pid_angle.output))
# motor.setVelocityRight(config.guidedDriveVelocity)
print("pid_angle: " + str(pid_angle.output))
motor.setVelocityLeft(100 * (pid_angle.output))
motor.setVelocityRight(-100 * (pid_angle.output))
sleep(0.1)
except KeyboardInterrupt:
motor.terminate()
i2c.terminate()
print("Goodbye!")
except:
motor.terminate()
i2c.terminate()
print("Aaaaaargh!")
raise
i2c = i2cHandler.I2cHandler()
i2c.start()
pid = pid.PID(0.01, 0, 0)
pid.setWindup(0.5)
while (True):
ist_dist = i2c.getDistanceLeftFront() - i2c.getDistanceRightFront()
pid.SetPoint = 0.0
if pid.update(ist_dist):
DeltaVelocityLeft_proz = pid.output
VelocityLeft_proz = 1 + DeltaVelocityLeft_proz
motor.setVelocityLeft(config.guidedDriveVelocity *
VelocityLeft_proz)
motor.setVelocityRight(config.guidedDriveVelocity)
print("Reg. Distanz: Soll: 0.000mm, Ist: " +
str(round(ist_dist, 3)) + "mm, Fehler: " +
str(round(ist_dist, 3)) + "mm, Out: " +
str(round(100 * DeltaVelocityLeft_proz, 3)) + "%")
sleep(0.0001)
except KeyboardInterrupt:
motor.terminate()
i2c.terminate()
print("Goodbye!")
except:
motor.terminate()
i2c.terminate()
i2c.resetRelativeYaw()
if (angle < 0):
# Turn left
print("Start: CurrentRelative -> {}, Gyro --> {}".format(
i2c.currRelativeYaw, i2c.getCurrYaw()))
# currVelocity = velocity_max
while (True):
currDelta = abs(angle) - abs(i2c.currRelativeYaw)
# Anfahren
if (currDelta > (abs(angle) - 15)):
if (currVelocity < velocity_max):
currVelocity = currVelocity + 0.025
motor.setVelocityLeft(-currVelocity)
motor.setVelocityRight(currVelocity)
continue
# Stoppen
if (currDelta < 2):
motor.setVelocityLeft(0.0)
motor.setVelocityRight(0.0)
break
# Bremsen
if (currDelta <= 15):
currVelocity = (velocity_max / 15.0) * currDelta
motor.setVelocityLeft(-currVelocity)
motor.setVelocityRight(currVelocity)
continue
while (True):
ist_dist = i2c.getDistanceLeftBack() - i2c.getDistanceRightBack()
ist_angle = atan((i2c.getDistanceLeftFront() -
(i2c.getDistanceLeftBack() - 0.5)) / 180)
pid_dist.SetPoint = 0.0
if pid_dist.update(ist_dist):
soll_angle = pid_dist.output
pid_angle.SetPoint = soll_angle
regD_out_str = str(round(pid_dist.SetPoint, 3)) + "; " + str(round(ist_dist, 3)) + "; " + \
str(round(pid_dist.SetPoint - ist_dist, 3)) + "; " + str(round(soll_angle, 1)) + "; ;"
if pid_angle.update(ist_angle):
DeltaVelocityLeft_proz = pid_angle.output
VelocityLeft_proz = 1 + DeltaVelocityLeft_proz
motor.setVelocityLeft(config.guidedDriveVelocity)
motor.setVelocityRight(config.guidedDriveVelocity *
VelocityLeft_proz)
# motor.setVelocityLeft(config.guidedDriveVelocity * -DeltaVelocityLeft_proz)
# motor.setVelocityRight(config.guidedDriveVelocity * DeltaVelocityLeft_proz)
regW_out_str = str(round(soll_angle, 3)) + " ; " + str(round(ist_angle, 3)) + " ; " + \
str(round(soll_angle - ist_angle, 3)) + " ; " + \
str(round(DeltaVelocityLeft_proz, 5))
# print("\033c")
print(
str(round(pid_dist.current_time, 3)) + "; " + regD_out_str +
regW_out_str)
regD_out_str = ""
regW_out_str = ""
sleep(0.02)
i2c = i2cHandler.I2cHandler()
i2c.start()
startAngle = i2c.currYaw
endAngleUnder = startAngle + 89
endAngleUpper = startAngle + 91
if endAngleUnder > 360:
endAngleUnder - 360
if endAngleUpper > 360:
endAngleUpper - 360
motor.setVelocityLeft(60)
motor.setVelocityRight(-60)
while (True):
currentAngle = i2c.currYaw
if (endAngleUnder - currentAngle < 20):
motor.setVelocityLeft(20)
motor.setVelocityRight(-20)
if currentAngle > endAngleUpper:
motor.setVelocityLeft(-20)
motor.setVelocityRight(20)
print("\033c")
print("Start Angle:" + str(startAngle))
print("endAngleUnder" + str(endAngleUnder))
print("endAngleUpper" + str(endAngleUpper))