centerX = (lines[i][0][2] - lines[i][0][0]) / 2 + lines[i][0][0]
centerY = (lines[i][0][3] - lines[i][0][1]) / 2 + lines[i][0][1]
d[0][i] = 250 - centerX
cv2.circle(warp, (centerX, centerY), 15, (0, 0, 255), -1)
cv2.imwrite('houghliness.jpg', warp)
beta[0][i] = (np.arctan(d[0][i] / centerY) * 180 / np.pi)
#print alpha[i]
angle = int(np.median(alpha))
if abs(angle) >= 85:
print 'geradeaus'
print(angle)
set_speed(speed_def)
speed[0][0] = speed_def
speed[0][1] = speed_def
PID(90, beta, d)
detectLeft = None
detectRight = None
print(speed_def)
elif angle > 0 and abs(angle) < 85: #drehe rechts
print 'drehe rechts'
detectRight = True
detectLeft = None
speedPID = PID(angle, beta, d)
speed[0][0] = int(speed[0][0] - speedPID[0][0])
if speed[0][0] < 0:
speed[0][0] = 0
set_right_speed(int(speed[0][0] - 10))
set_left_speed(speed_def - 10)
if turn_r > 1:
set_right_speed(speed_def)
set_left_speed(speed_def - 15)
print 'speed right: ', speed_def
print 'speed left: ', speed_def - 15
elif dError < -5: #Mittellinie rechts
set_left_speed(speed_def)
set_right_speed(speed_def - 15)
print 'speed right: ', speed_def - 15
print 'speed left: ', speed_def
else:
set_speed(speed_def)
print 'speed right: ', speed_def
print 'speed left: ', speed_def
speed_right = speed_def
speed_left = speed_def
PID(90, beta, d)
detectLeft = None
detectRight = None
elif direction == 'rechts': #drehe rechts
print 'drehe rechts'
detectRight = True
detectLeft = None
speed_right = int(speed_right - PID(angle, beta, d))
if speed_right < 20:
speed_right = 15
set_right_speed(int(speed_right))
set_left_speed(speed_def - 15)
if turn_r > 2:
turn_r = 0
print 'speed right: ', speed_right
#!/usr/bin/env python
#-*- coding: utf-8 -*-
#Defining Python Source Code Encodings
#---------------------------
# import the necessary packages
import time
import cv2
import numpy as np
import matplotlib.pyplot as plt
from PID_CenterDrive import PID
from birdviewThresholdCM import find
default = 40
inpAngle = 70
beta = 5.8
d = 25
while True:
print 'Default speed:', default
print 'Input angle:', inpAngle
value = PID(inpAngle,beta,d)
print 'Set speed:',(default-value)
print'---------------------'
time.sleep(0.5)