PID_Controller

#!/usr/bin/env python3
from ev3dev.ev3 import *
import os
import time

#FONTOS!
#Kp,Ki,Kd,alzheimer értéke 0-1 között kalibrálandó
#A kalibrációs értékek minden roboton más
#Számít: A tengelyek távolságától, A szenzor és a motorok távolgágától, A robot tömeg/fordulási középpontjától



#alap kalibrációk

os.system('setfont Lat15-TerminusBold14')

touch = TouchSensor('in1')
assert touch.connected, 'TouchSensor not connected'

szin = ColorSensor('in3')
assert szin.connected, 'ColorSensor not connected'
szin.mode = 'COL-REFLECT'

mA = LargeMotor('outA')
mB = LargeMotor('outB')

mA.stop_action = 'hold'
mB.stop_action = 'hold'


Kp = 0.9 #Propersional control érték szórzója // A kívánt értéket (target) összehasonlítja mért értékkel majd ezzel ad egy mozási/kilengési értéket a controllernek \\Jelen
Ki = 0.35 #Integral control érték szorzója   // Az egyensúlyi hiba küszöbölés, vonal finom követése, avagy ne ránghatózzon, egyenes számításának a finom hangolása \\Múlt
Kd = 0.56 #Derivative control érték szorzója // A jövőbeli eltérés megjóslsa, ha ugyan így fojtatja a mozást, avagy kanyarok számításának finomítása \\Jövő
alzheimer = 0.25 # egy olyan értk, ami szép lassan felejteti el az integral hibaértékét
target = 35 #A maximális és a minimális szenzor érték számtani közepe
integral = 0 #jelen haladás értéke
last_error=0 #előző eltérési hiba érték
speed=600 #motor alap sebessége

'''def sensor_initaialize(Max_value,Min_value):
    target = (Max_value/Min_value)/2'''

while touch.value() == 0:
    error = target - szin.value() #eltérés számítása
    integral = (integral + error)*alzheimer #Múlt eredményi
    derivative = error - last_error #Múltról következtetés jövőre

    correction = int(((error*Kp)+(integral*Ki)+(derivative*Kd))*6) #controller output PID eredmények összegzése
    last_error = error

    motor_speed_a=speed-correction #motorok sebességei
    motor_speed_b=speed+correction

    if(speed+correction > 1000): #maximális (1000)sebesség egység átlépés elleni védelem
        motor_speed_b=1000

    if(speed-correction < -1000): #minimális (-1000)sebesség egység átlépés elleni védelem
        motor_speed_a=-1000

    if(correction > 0): #korrigálés jobbra
        mA.run_forever(speed_sp = motor_speed_b)
        mB.run_forever(speed_sp = motor_speed_a)

    elif(correction < 0): #korrigálás balra
        mB.run_forever(speed_sp = motor_speed_a)
        mA.run_forever(speed_sp = motor_speed_b)

    else: #teljesen egyenes haladás
        mA.run_forever(speed_sp = speed)
        mB.run_forever(speed_sp = speed)