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
Exemple #2
0
    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
Exemple #3
0
    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()
Exemple #4
0
    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
Exemple #5
0
    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)
Exemple #6
0
    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))