Ich habe jetzt das nötige Equipment zuhause.
Code: Alles auswählen
# -*- coding: utf-8 -*-
import serial #OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//OLAF OS//
import time
import RPi.GPIO as GPIO
import atexit
from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor
# create a default object, no changes to I2C address or frequency
mh = Adafruit_MotorHAT(addr=0x60)
# recommended for auto-disabling motors on shutdown!
def turnOffMotors():
mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE)
mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE)
atexit.register(turnOffMotors)
#DC motor test
#Links
Motor_Rechts = mh.getMotor(1)
#Rechts
Motor_Links = mh.getMotor(2)
Abbiegezeit = 1
Motor_Rechts.setSpeed(100)
Motor_Links.setSpeed(100)
serial = serial.Serial('/dev/ttyACM0', 9600) #Für Arduino
#serial = serial.Serial('COM3', 9600) #Für PC
serial.open
time.sleep(1)
def Vorwarts(motor): #Definiere Funktionen der Motoren
motor.run(Adafruit_MotorHAT.FORWARD)
def Ruckwarts(motor):
motor.run(Adafruit_MotorHAT.BACKWARD)
def Stop(motor):
motor.run(Adafruit_MotorHAT.RELEASE)
uv = 25
ur1 = 0
ur2 = 15
while True:
try:
#Split Variablen
try:
response = serial.readline()
#print (response)
if response == b'US1\r\n':
response = serial.readline()
uv =int(response.split(b"=")[-1])
elif response == b'US2\r\n':
response = serial.readline()
ur1 =int(response.split(b"=")[-1])
elif response == b'US3\r\n':
response = serial.readline()
ur2 =int(response.split(b"=")[-1])
print("UV=" ,uv)
print("UR1=" ,ur1)
print("UR2=" ,ur2)
Sensor_Vorne = uv
Sensor_Rechts_1 = ur1
Sensor_Rechts_2 = ur2
except ValueError:
print("ValueError")
#Rechte Hand
if Sensor_Rechts_2 < 20 and Sensor_Vorne < 20:
Vorwarts(Motor_Rechts)
Vorwarts(Motor_Links)
time.sleep(2)
Stop(Motor_Rechts)
Stop(Motor_Links)
Ruckwarts(Motor_Rechts)
Ruckwarts(Motor_Links)
time.sleep(1)
Vorwarts(Motor_Rechts)
time.sleep(Abbiegezeit)
Vorwarts(Motor_Rechts)
Vorwarts(Motor_Links)
time.sleep(1.5)
elif Sensor_Rechts_2 > 20 and Sensor_Vorne > 20:
Ruckwarts(Motor_Rechts)
Vorwarts(Motor_Links)
time.sleep(Abbiegezeit)
Vorwarts(Motor_Rechts)
Vorwarts(Motor_Links)
time.sleep(1.5)
elif Sensor_Rechts_2 > 20 and Sensor_Vorne < 20:
Vorwarts(Motor_Rechts)
Vorwarts(Motor_Links)
time.sleep(2)
Stop(Motor_Rechts)
Stop(Motor_Links)
Ruckwarts(Motor_Rechts)
Ruckwarts(Motor_Links)
time.sleep(1)
Ruckwarts(Motor_Rechts)
Vorwarts(Motor_Links)
time.sleep(Abbiegezeit)
Vorwarts(Motor_Rechts)
Vorwarts(Motor_Links)
time.sleep(1.5)
elif Sensor_Rechts_2 < 20 and Sensor_Vorne > 20:
Vorwarts(Motor_Rechts)
Vorwarts(Motor_Links)
#Motor Geschwindigkeit
if Sensor_Rechts_1 > Sensor_Rechts_2:
Motor_Links. setSpeed(150)
Motor_Rechts. setSpeed(100)
elif Sensor_Rechts_1 < Sensor_Rechts_2:
Motor_Links. setSpeed(100)
Motor_Rechts. setSpeed(150)
elif Sensor_Rechts_1 == Sensor_Rechts_2:
Motor_Rechts. setSpeed(100)
Motor_Links. setSpeed(100)
except KeyboardInterrupt:
turnOffMotors()
serial.close()
print("Programm wurde gestoppt!!")