jazda.py

import RPi.GPIO as GPIO
import time
from kalman import Kalman


def forward(speed):
	GPIO.output(AIN1, GPIO.LOW)
	GPIO.output(AIN2, GPIO.HIGH)    
	GPIO.output(BIN1, GPIO.LOW)
	GPIO.output(BIN2, GPIO.HIGH)
	PWMA.ChangeDutyCycle(speed-2)      
	PWMB.ChangeDutyCycle(speed)
		

def stop():
	GPIO.output(AIN1, GPIO.LOW)
	GPIO.output(AIN2, GPIO.LOW)
	GPIO.output(BIN1, GPIO.LOW)
	GPIO.output(BIN2, GPIO.LOW)
	PWMA.ChangeDutyCycle(0)
	PWMB.ChangeDutyCycle(0)
	time.sleep(0.3)


def backward(speed):
	GPIO.output(AIN1, GPIO.HIGH)
	GPIO.output(AIN2, GPIO.LOW)
	GPIO.output(BIN1, GPIO.HIGH)
	GPIO.output(BIN2, GPIO.LOW)
	PWMA.ChangeDutyCycle(speed)
	PWMB.ChangeDutyCycle(speed)

# Sprawdzić które to lewo, które to prawo i nazwać poprawnie
def turn_in_place_left(speed):
	GPIO.output(AIN1, GPIO.LOW)
	GPIO.output(AIN2, GPIO.LOW)    
	GPIO.output(BIN1, GPIO.LOW)
	GPIO.output(BIN2, GPIO.HIGH)
	PWMA.ChangeDutyCycle(speed)      
	PWMB.ChangeDutyCycle(speed)


def turn_in_place_right(speed):
	GPIO.output(AIN1, GPIO.LOW)
	GPIO.output(AIN2, GPIO.HIGH)    
	GPIO.output(BIN1, GPIO.LOW)
	GPIO.output(BIN2, GPIO.LOW)
	PWMA.ChangeDutyCycle(speed)      
	PWMB.ChangeDutyCycle(speed)


def backward_turn_in_place_right(speed):
	GPIO.output(AIN1, GPIO.HIGH)
	GPIO.output(AIN2, GPIO.LOW)
	GPIO.output(BIN1, GPIO.LOW)
	GPIO.output(BIN2, GPIO.LOW)
	PWMA.ChangeDutyCycle(speed)
	PWMB.ChangeDutyCycle(speed)


def backward_turn_in_place_left(speed):
	GPIO.output(AIN1, GPIO.LOW)
	GPIO.output(AIN2, GPIO.LOW)
	GPIO.output(BIN1, GPIO.HIGH)
	GPIO.output(BIN2, GPIO.LOW)
	PWMA.ChangeDutyCycle(speed)
	PWMB.ChangeDutyCycle(speed)

def get_dist(filter):
        GPIO.output(TRIG, True)
        time.sleep(0.00001)
        GPIO.output(TRIG, False)

        while GPIO.input(ECHO) == 0:
            start_time = time.time()
        
        while GPIO.input(ECHO) == 1:
            end_time = time.time() 

        dist = (end_time - start_time) * 34300/2 # cm
        dist = filter.get_filtered_value(dist)

        return dist

def calculate_PID(delta_time, I, last_error):
        error = distance - setpoint
        P = error * kP
        I += delta_time * error * kI
        D = ((error - last_error) / delta_time) * kD

        return (P + I + D, I, error)


if __name__=='__main__':

	#robot = AlphaBot2()
	try:
		GPIO.setmode(GPIO.BCM)     #adresy pinow wyjscia
		GPIO.setwarnings(False)
		AIN1 = 12
		AIN2 = 13
		PWMA_pin = 6		# pin na ktorym mozemy ustawic pwm
		BIN1 = 20
		BIN2 = 21
		PWMB_pin = 26
		ECHO = 27
		TRIG = 22		  #funkcja = numer wyjscia na raspberry z obrazka z prezentacji
		GPIO.setup(AIN1, GPIO.OUT)		     #AIN1 - silnik A w jedna strone
		GPIO.setup(AIN2, GPIO.OUT)		     #AIN1 - silnik A w druga strone
		GPIO.setup(BIN1, GPIO.OUT)
		GPIO.setup(BIN2, GPIO.OUT)
		GPIO.setup(PWMA_pin, GPIO.OUT)		     #PWMA - sygnal PWM do sterowania predkoscia silnika A
		GPIO.setup(PWMB_pin, GPIO.OUT)
		GPIO.setup(TRIG, GPIO.OUT, initial=GPIO.LOW)		      #do czujnika odleglosci
		GPIO.setup(ECHO, GPIO.IN)		 #do czujnika odleglosci    
		PWMA = GPIO.PWM(PWMA_pin,500)    #PWM tu bedzie z czestotliwoscia 500Hz
		PWMB = GPIO.PWM(PWMB_pin,500)
		PWMA.start(50)
		PWMB.start(50)      #startujemy PWM-a z wypelnieniem 50%
		GPIO.output(AIN1,GPIO.LOW)
		GPIO.output(AIN2,GPIO.LOW)
		GPIO.output(BIN1,GPIO.LOW)
		GPIO.output(BIN2,GPIO.LOW)
		PWMA.ChangeDutyCycle(0)
		PWMB.ChangeDutyCycle(0)	
		
		##########################

		kP = 1.5
		kI = 0
		kD = 0


		setpoint = 30  # cm
		error = 0
		last_error = 0

		##########################

		myFilter = Kalman(1.5, 16, 1023, 0)

		I = 0
		last_error = 0
		prev_time = time.time()
		sample_time = 0.06


		while True:
			curr_time = time.time()
			delta_time = curr_time - prev_time

			# every 10ms (or more)
			if delta_time >= sample_time:
				distance = get_dist(myFilter)

				curr_pid, I, last_error = calculate_PID(delta_time, I, last_error)
				if curr_pid > 100:
					curr_pid = 100

				if curr_pid < -100:
					curr_pid = -100
				
				if curr_pid < -20:
				    backward(abs(curr_pid))
				elif curr_pid > 20:
				    forward(curr_pid)

				elif 3 < curr_pid <= 20:
					forward(20)
				elif -20 < curr_pid < -3:
					backward(20)
				
				else:
				    stop()
				
				#time.sleep(0.1)

				prev_time = curr_time # or time.time() ?

				# DEBUGGING
				print("DISTANCE: ") 
				print(distance, end="\n")
				print("PID VALUE: ") 
				print(curr_pid, end="\n")

	except KeyboardInterrupt:
		GPIO.cleanup()