# pybricks blocks file:{"blocks":{"languageVersion":0,"blocks":[{"type":"blockGlobalSetup","id":"ZbxVYEa|[le#4k/7!#U]","x":400,"y":-200,"deletable":false,"next":{"block":{"type":"variables_setup_imported_function","id":";DQ^ciUP/1+!iZLyBFTW","fields":{"FROM_MODULE":"MBC_line16_obj_Lib","VAR":{"id":"LI2o1bQY-%+)DH|Gue:B"}},"next":{"block":{"type":"variables_setup_object_instance","id":"A[VhR7@h`n-hV?lEVQSP","extraState":{"optionLevel":1},"fields":{"VAR":{"id":"G616F,yl|[Ez(o[.QBjD"},"CLASS_NAME":"MBC_LINE16"},"inputs":{"ARG0":{"shadow":{"type":"blockMathNumber","id":"tReZx*a2r[`AgP=6mV`Z","fields":{"NUM":3}}}},"next":{"block":{"type":"variables_set_prime_hub","id":"7^.M`lw.XuQV@:$Gw{Mt","extraState":{"optionLevel":0},"fields":{"VAR":{"id":"Bs$!RoWlzPta02/,Fg2r"}},"next":{"block":{"type":"variables_set_motor","id":"eI@[LtG*cgsdrH?NTcK#","fields":{"VAR":{"id":"=xNe.V^%J}[Dhp0al~h["}},"inputs":{"PORT":{"shadow":{"type":"blockParametersPort","id":"{?3HS6|wL-pj~W3w7]It","fields":{"NAME":"E"}}},"POSITIVE_DIRECTION":{"shadow":{"type":"blockParametersDirection","id":"q,Ly~#gqY|SP{Lx2p:dG","fields":{"SELECTION":"Direction.COUNTERCLOCKWISE"}}}},"next":{"block":{"type":"variables_set_motor","id":"UXG!LiD%xy7fBzEo:a]1","fields":{"VAR":{"id":"QQYTM#Tb9:o@PCI93;`5"}},"inputs":{"PORT":{"shadow":{"type":"blockParametersPort","id":"t1`l_81-A+3u#C6dp-x-","fields":{"NAME":"F"}}},"POSITIVE_DIRECTION":{"shadow":{"type":"blockParametersDirection","id":"Qa8_d$(#~82=Kohlh.B7","fields":{"SELECTION":"Direction.CLOCKWISE"}}}},"next":{"block":{"type":"variables_setup_any","id":"T40SgA3{nXQU7n{NH^PG","fields":{"VAR":{"id":"6-f^L1cawA}?Ig#^#P]$"}},"inputs":{"VALUE0":{"shadow":{"type":"blockMathNumber","id":"_1%aO]Sk})k;@N3tm#ww","fields":{"NUM":50}}}},"next":{"block":{"type":"variables_setup_any","id":"*K/9kcnCrMK_WT$QrcUp","fields":{"VAR":{"id":".@z8X7E]=]sO1-aI:SD1"}},"inputs":{"VALUE0":{"shadow":{"type":"blockMathNumber","id":"~x_xThv_#`NYN/9m~J2-","fields":{"NUM":0.4}}}},"next":{"block":{"type":"variables_setup_any","id":"n5Yt^kTg)Dvq|Et!Zsw?","fields":{"VAR":{"id":"0hUel0ek6-LTeeI6HyHo"}},"inputs":{"VALUE0":{"shadow":{"type":"blockMathNumber","id":";wuMy/)ouD[h[cZhrl9o","fields":{"NUM":1.5}}}},"next":{"block":{"type":"variables_setup_any","id":"F@Vucke.!^[F:Y,Yt@`z","fields":{"VAR":{"id":"L5A02aw*M17I;oV4lRau"}},"inputs":{"VALUE0":{"shadow":{"type":"blockMathNumber","id":";~7cKCQ(iG4Y9,VgR(WF","fields":{"NUM":0}}}},"next":{"block":{"type":"variables_setup_any","id":"FaFVjLF_+Kvib14ZVO[C","fields":{"VAR":{"id":")l0eGqM6ZOnH45^,!z?i"}},"inputs":{"VALUE0":{"shadow":{"type":"blockMathNumber","id":"/@es6QzN{qSwC~HMX+9u","fields":{"NUM":0}}}},"next":{"block":{"type":"variables_setup_any","id":"+CMEps|bMZ/cLk8vaFx^","fields":{"VAR":{"id":"eI]29bha:*C%Y=v85w]P"}},"inputs":{"VALUE0":{"shadow":{"type":"blockMathNumber","id":"[[p{)1soqD2!Q5L-vA)A","fields":{"NUM":0}}}}}}}}}}}}}}}}}}}}}}}}}}},{"type":"blockGlobalStart","id":"j6Kn)W7)xlV$}8QiZTEv","x":400,"y":260,"deletable":false,"next":{"block":{"type":"blockFlowWhile","id":"Kw[QLO(@e6A/ndKJK8=J","fields":{"MODE":"WHILE"},"inputs":{"BOOL":{"shadow":{"type":"blockLogicTrue","id":"Cj+xaDVp3.67RW3t)W%;"}},"DO":{"block":{"type":"blockIfElse","id":"vsWeoe/Ek6z!:5;[?jqh","extraState":{"optionLevel":1},"inputs":{"IF0":{"shadow":{"type":"blockLogicTrue","id":"%mcJKY_wr1U:l:2Z?L0p"},"block":{"type":"blockLogicCompare","id":"+]/d9;OXdMFy=t4gzIc7","fields":{"OP1":"GT"},"inputs":{"A":{"shadow":{"type":"blockMathNumber","id":":stFq;}y~n%8~2NRS@)Z","fields":{"NUM":1}},"block":{"type":"blockObjectInstanceCallValue","id":"ug(~{+!pS,h9UGf!l:*c","extraState":{"optionLevel":0},"fields":{"CALL_TYPE":"CLASS_CALL_METHOD_SYNC","METHOD":"width"},"inputs":{"VAR":{"shadow":{"type":"variables_get_object_instance","id":"C*fx_{m{7Y0P!{SbYwe#","fields":{"VAR":{"id":"G616F,yl|[Ez(o[.QBjD","name":"line16","type":"ObjectInstance"}}}}}}},"B":{"shadow":{"type":"blockMathNumber","id":"c8j#{K=O`B8]9*$`wy/+","fields":{"NUM":10}}}}}},"DO0":{"block":{"type":"blockMotorDuty","id":"@,u|o}P=O1OkKq[6W_kw","inputs":{"VAR":{"shadow":{"type":"variables_get_simple_motor_device","id":"(p-v4-6}FNwA-fU;t3Z8","fields":{"VAR":{"id":"=xNe.V^%J}[Dhp0al~h[","name":"motorL","type":"Motor"}}}},"VALUE0":{"shadow":{"type":"unit_percent","id":"L]6Wm7c41n$CV$WGby3C","fields":{"VALUE0":0}}}},"next":{"block":{"type":"blockMotorDuty","id":"vw_r(V?Rm%V4=3%V#Z;i","inputs":{"VAR":{"shadow":{"type":"variables_get_simple_motor_device","id":"2K{;m(qT|6gv%H#W{Bvg","fields":{"VAR":{"id":"QQYTM#Tb9:o@PCI93;`5","name":"motorR","type":"Motor"}}}},"VALUE0":{"shadow":{"type":"unit_percent","id":"uVup(Ci}Q`FMW{?u_5}c","fields":{"VALUE0":0}}}}}}}},"ELSE":{"block":{"type":"blockVariableSetValue","id":"Cewp22^YP4G;D*rvF3j2","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"rw#xo#:HU12C1_P^:,gW","fields":{"VAR":{"id":"L5A02aw*M17I;oV4lRau","name":"error","type":"Any"}}}},"VALUE0":{"shadow":{"type":"blockMathNumber","id":")eN2WR~;X26vd9u/2.b8","fields":{"NUM":1}},"block":{"type":"blockObjectInstanceCallValue","id":"1xMPv5tDL4M~g)VCw=/S","extraState":{"optionLevel":0},"fields":{"CALL_TYPE":"CLASS_CALL_METHOD_SYNC","METHOD":"pos100"},"inputs":{"VAR":{"shadow":{"type":"variables_get_object_instance","id":",LhR5.1i=]?yGPF]Brqg","fields":{"VAR":{"id":"G616F,yl|[Ez(o[.QBjD","name":"line16","type":"ObjectInstance"}}}}}}}},"next":{"block":{"type":"blockVariableSetValue","id":"!~*,S1svhcWR*M7_J}$l","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"riKf:#1w{G*c43x[0((y","fields":{"VAR":{"id":"eI]29bha:*C%Y=v85w]P","name":"PD","type":"Any"}}}},"VALUE0":{"shadow":{"type":"blockMathNumber","id":"$,:X|LSb.YqMFAlY}itg","fields":{"NUM":1}},"block":{"type":"blockMathArithmetic","id":"#riVp#SDnnu_]+_lr?NT","fields":{"OP":"ADD"},"inputs":{"A":{"shadow":{"type":"blockMathNumber","id":"m+~6Aro3mKt_a[Q!$+KX","fields":{"NUM":1}},"block":{"type":"blockMathArithmetic","id":"eV`UE[KK0V#x$gND6Fib","fields":{"OP":"MULTIPLY"},"inputs":{"A":{"shadow":{"type":"blockMathNumber","id":"Ck#nSW[pS[jn6Rwvd6x.","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"tutFMGimOL[HFUZ.aP5?","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"^HT#]t`EMbZNn`q_8!}x","fields":{"VAR":{"id":"L5A02aw*M17I;oV4lRau","name":"error","type":"Any"}}}}}}},"B":{"shadow":{"type":"blockMathNumber","id":"hKh^]pN0*b3rSOS2x.HO","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"cRJ+O{(zrF2W*u+pVV^F","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"BQ!*O[!L0LEgIqQnDk{U","fields":{"VAR":{"id":".@z8X7E]=]sO1-aI:SD1","name":"KP","type":"Any"}}}}}}}}}},"B":{"shadow":{"type":"blockMathNumber","id":"/]?u84Ch5j479W{frD!q","fields":{"NUM":1}},"block":{"type":"blockMathArithmetic","id":"HMHHmEm8OP2sENVnqpya","fields":{"OP":"MULTIPLY"},"inputs":{"A":{"shadow":{"type":"blockMathNumber","id":"A#zKn/G0L2a8K}yasEGq","fields":{"NUM":1}},"block":{"type":"blockMathArithmetic","id":"{[UOU8MNm+//?oD3n2Np","fields":{"OP":"MINUS"},"inputs":{"A":{"shadow":{"type":"blockMathNumber","id":"dO@5ad)?t,k#*i|)Tqm0","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"[yXX/-whzj7-EU;qKnT3","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"I+eSRSLZj=9]=.GR4$#@","fields":{"VAR":{"id":"L5A02aw*M17I;oV4lRau","name":"error","type":"Any"}}}}}}},"B":{"shadow":{"type":"blockMathNumber","id":"#D#4UlLEZF!+l#8S-(.q","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"mE8l5@eBX`n}yQqEyqbd","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":",:$FfWgoPH`k#)uf;MQ{","fields":{"VAR":{"id":")l0eGqM6ZOnH45^,!z?i","name":"lastError","type":"Any"}}}}}}}}}},"B":{"shadow":{"type":"blockMathNumber","id":"V6#S8,XedRYOa3E[Ic9_","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"_)6x{Z`dT5|A7ki3%~_=","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"lRs3;Pos6qd#cxgdq4u;","fields":{"VAR":{"id":"0hUel0ek6-LTeeI6HyHo","name":"KD","type":"Any"}}}}}}}}}}}}}},"next":{"block":{"type":"blockMotorDuty","id":"ZiRt!}n5U5~wKgoMZ90_","inputs":{"VAR":{"shadow":{"type":"variables_get_simple_motor_device","id":"`;Klc~x)O_:Dkjy@#1@L","fields":{"VAR":{"id":"=xNe.V^%J}[Dhp0al~h[","name":"motorL","type":"Motor"}}}},"VALUE0":{"shadow":{"type":"unit_percent","id":"h(q_f.J+izwp]Ty2UOL7","fields":{"VALUE0":50}},"block":{"type":"blockMathArithmetic","id":"c!.A1n@]gI,qqp;Zld0B","fields":{"OP":"ADD"},"inputs":{"A":{"shadow":{"type":"blockMathNumber","id":"+;kZzPSUe#inl,u##Ck+","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"Of03UG4FWdIG!Q@YR/(2","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"6m983bX0?.^{?~0Lhg+7","fields":{"VAR":{"id":"6-f^L1cawA}?Ig#^#P]$","name":"SPEED","type":"Any"}}}}}}},"B":{"shadow":{"type":"blockMathNumber","id":"RMWZDDDf%lLtL]:QKo^^","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"5zpjXMNX*2gWuk9rogkG","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"2H;J+L!UbM!+UFjL6x|g","fields":{"VAR":{"id":"eI]29bha:*C%Y=v85w]P","name":"PD","type":"Any"}}}}}}}}}}},"next":{"block":{"type":"blockMotorDuty","id":"YvA)RRL=(8JQ:Y%e]6*V","inputs":{"VAR":{"shadow":{"type":"variables_get_simple_motor_device","id":"fYCQhfCc1|soEK4VzW$7","fields":{"VAR":{"id":"QQYTM#Tb9:o@PCI93;`5","name":"motorR","type":"Motor"}}}},"VALUE0":{"shadow":{"type":"unit_percent","id":"/c%oIU1%u*`H,s#(1v6m","fields":{"VALUE0":50}},"block":{"type":"blockMathArithmetic","id":"Hr|3-n,DidU0vCNP{N%|","fields":{"OP":"MINUS"},"inputs":{"A":{"shadow":{"type":"blockMathNumber","id":"a:;+|KP3EXna1LU:@e+n","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"$k,%]3.RJHLr@^nNguy3","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"uZwYaVZR$gA/=kPzxgl*","fields":{"VAR":{"id":"6-f^L1cawA}?Ig#^#P]$","name":"SPEED","type":"Any"}}}}}}},"B":{"shadow":{"type":"blockMathNumber","id":"Ff)ZlyKB!M#osy4KtF:M","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"Eh)P%Wxrmvs|FJ.hQ9G[","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"uxi!WIcLEmyHj$wdCd!;","fields":{"VAR":{"id":"eI]29bha:*C%Y=v85w]P","name":"PD","type":"Any"}}}}}}}}}}},"next":{"block":{"type":"blockVariableSetValue","id":"7Ih7~rqS0|`yUAcXGD/@","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"AecKXJ4Wt^w,sgkNrmY1","fields":{"VAR":{"id":")l0eGqM6ZOnH45^,!z?i","name":"lastError","type":"Any"}}}},"VALUE0":{"shadow":{"type":"blockMathNumber","id":"VD0E74dg1o3:~^vJ!C/g","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"RPFbnDm}+-yvK?Qj~r]7","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"h+shL|EJG/-)*@iShY9I","fields":{"VAR":{"id":"L5A02aw*M17I;oV4lRau","name":"error","type":"Any"}}}}}}}}}}}}}}}}}}},"next":{"block":{"type":"blockWaitTime","id":"NX[xE1+;HU?dhqAbT(Yd","inputs":{"VALUE0":{"shadow":{"type":"unit_time","id":"+wivxm23lqzX)RErO,vw","fields":{"VALUE0":10}}}}}}}}}}}}]},"variables":[{"name":"red","id":"m;P|xbP~Owy]3YESw6#P","type":"ColorDef"},{"name":"orange","id":"[j@=rq!IEasH0?lU7,e5","type":"ColorDef"},{"name":"yellow","id":"b4f9U`p*sT%y*~_]?+[r","type":"ColorDef"},{"name":"green","id":"s/!L{}z^IN[y4G!H9)Qz","type":"ColorDef"},{"name":"cyan","id":"5Aq2T5#/tgGYW%.gZR@)","type":"ColorDef"},{"name":"blue","id":"hOl;G*0YIc=O^({,{W4#","type":"ColorDef"},{"name":"violet","id":"`+z@2NxMW_%~G0J`o[+8","type":"ColorDef"},{"name":"magenta","id":"A{bYb=,*wMLoP(D{SHU|","type":"ColorDef"},{"name":"white","id":"~;yfi~/`5$+yYey:TssW","type":"ColorDef"},{"name":"none","id":"n[kr@A~O|Lvu1Z(Ys5p/","type":"ColorDef"},{"name":"prime hub","id":"Bs$!RoWlzPta02/,Fg2r","type":"PrimeHub"},{"name":"motorL","id":"=xNe.V^%J}[Dhp0al~h[","type":"Motor"},{"name":"motorR","id":"QQYTM#Tb9:o@PCI93;`5","type":"Motor"},{"name":"SPEED","id":"6-f^L1cawA}?Ig#^#P]$","type":"Any"},{"name":"KP","id":".@z8X7E]=]sO1-aI:SD1","type":"Any"},{"name":"KD","id":"0hUel0ek6-LTeeI6HyHo","type":"Any"},{"name":"error","id":"L5A02aw*M17I;oV4lRau","type":"Any"},{"name":"lastError","id":")l0eGqM6ZOnH45^,!z?i","type":"Any"},{"name":"PD","id":"eI]29bha:*C%Y=v85w]P","type":"Any"},{"name":"MBC_LINE16","id":"LI2o1bQY-%+)DH|Gue:B","type":"FunctionImported"},{"name":"line16","id":"G616F,yl|[Ez(o[.QBjD","type":"ObjectInstance"}],"info":{"type":"pybricks","version":"2.1.1"},"workspaceOptions":{"scrollX":0,"scrollY":0,"scale":0.7}}
from pybricks.hubs import PrimeHub
from pybricks.parameters import Direction, Port
from pybricks.pupdevices import Motor
from pybricks.tools import wait

from MBC_line16_obj_Lib import MBC_LINE16

# Set up.
line16 = MBC_LINE16(3)
prime_hub = PrimeHub()
motorL = Motor(Port.E, Direction.COUNTERCLOCKWISE)
motorR = Motor(Port.F, Direction.CLOCKWISE)
SPEED = 50
KP = 0.4
KD = 1.5
error = 0
lastError = 0
PD = 0


# The main program starts here.
while True:
    if line16.width() > 10:
        motorL.dc(0)
        motorR.dc(0)
    else:
        error = line16.pos100()
        PD = error * KP + (error - lastError) * KD
        motorL.dc(SPEED + PD)
        motorR.dc(SPEED - PD)
        lastError = error
    wait(10)