# pybricks blocks file:{"blocks":{"languageVersion":0,"blocks":[{"type":"blockGlobalSetup","id":"^Hi0K0b,Zk5*[)J5qRGu","x":400,"y":-200,"deletable":false,"next":{"block":{"type":"variables_setup_imported_function","id":"c9(uXOyS{=5%MoBTcx3;","fields":{"FROM_MODULE":"MBC_sensors","VAR":{"id":"~)k%w_AhO]Ki-q%A`,8F"}},"next":{"block":{"type":"variables_setup_object_instance","id":"q[%7GuzIK+/k?SMmS0#?","extraState":{"optionLevel":1},"fields":{"VAR":{"id":"y}(R(irPCs%{|o6lO_,p"},"CLASS_NAME":"MBC_LINE16"},"inputs":{"ARG0":{"shadow":{"type":"blockMathNumber","id":"L~=3oEI_^1N4Fc]9a_K^","fields":{"NUM":3}}}},"next":{"block":{"type":"variables_set_prime_hub","id":"D3m%IqBFveisLnz:rj$R","extraState":{"optionLevel":0},"fields":{"VAR":{"id":"LH.Vk2/f?poqTKc$?7gx"}},"next":{"block":{"type":"variables_set_motor","id":"ptHkS|wU6itC$e7[b;);","fields":{"VAR":{"id":"~OI0ROvYql6qFl{ZcB-y"}},"inputs":{"PORT":{"shadow":{"type":"blockParametersPort","id":"bslIH=^}F4-Bt@H/M+cc","fields":{"NAME":"E"}}},"POSITIVE_DIRECTION":{"shadow":{"type":"blockParametersDirection","id":"l_SFw6G*Hc-V]31SF{U_","fields":{"SELECTION":"Direction.COUNTERCLOCKWISE"}}}},"next":{"block":{"type":"variables_set_motor","id":"L9[T5#|FNOK+9rW6o/3/","fields":{"VAR":{"id":"Ing[XT4ihrKvScll;RH)"}},"inputs":{"PORT":{"shadow":{"type":"blockParametersPort","id":";f@QB?6q6!]]=X]l`-di","fields":{"NAME":"F"}}},"POSITIVE_DIRECTION":{"shadow":{"type":"blockParametersDirection","id":"K7vGsU:B1R?a6[zCn8@c","fields":{"SELECTION":"Direction.CLOCKWISE"}}}},"next":{"block":{"type":"variables_setup_any","id":"_9#655RBWx@9*i9FJj(:","fields":{"VAR":{"id":"t4-VC^pJ+;|U7[v:lZGf"}},"inputs":{"VALUE0":{"shadow":{"type":"blockMathNumber","id":"}B@M|4Eli!(;Cn@{`TD}","fields":{"NUM":50}}}},"next":{"block":{"type":"variables_setup_any","id":"URsitb8B::L~novq56xy","fields":{"VAR":{"id":"|[mVEEgQ)9J?frYA.y)4"}},"inputs":{"VALUE0":{"shadow":{"type":"blockMathNumber","id":"{`96qlmxtxjP:TA1AxN!","fields":{"NUM":0.4}}}},"next":{"block":{"type":"variables_setup_any","id":"g^X%g?w9wb6DgBp7:jZ5","fields":{"VAR":{"id":"j+TJczeiW_I(9n{06rA)"}},"inputs":{"VALUE0":{"shadow":{"type":"blockMathNumber","id":"c3DZa:QbV-dT2KR:C`uv","fields":{"NUM":1.5}}}},"next":{"block":{"type":"variables_setup_any","id":"3[9-Nc2WK1cYc2]%RTs#","fields":{"VAR":{"id":"@;t,nTG@GfC5P7pk6:8q"}},"inputs":{"VALUE0":{"shadow":{"type":"blockMathNumber","id":"%5o/XWN/?9}WBmbCQy_s","fields":{"NUM":0}}}},"next":{"block":{"type":"variables_setup_any","id":"j`k5pxE7cAF^*M5{1p)?","fields":{"VAR":{"id":"$KgwuV!E3BU!q,|M^-Nw"}},"inputs":{"VALUE0":{"shadow":{"type":"blockMathNumber","id":"Ug=,IR!ny#fANd^$2ygm","fields":{"NUM":0}}}},"next":{"block":{"type":"variables_setup_any","id":":[[s`llpbVa7RJ2MP3Io","fields":{"VAR":{"id":"D-0-1chai)1aeLs~6ga="}},"inputs":{"VALUE0":{"shadow":{"type":"blockMathNumber","id":"g1k0IY:T]^7W/^+KimqH","fields":{"NUM":0}}}}}}}}}}}}}}}}}}}}}}}}}}},{"type":"blockGlobalStart","id":"~`S+O1z]?Rr0xaVfT8US","x":400,"y":260,"deletable":false,"next":{"block":{"type":"blockFlowWhile","id":"F]viOM|.9Z`j7^`^qyF.","fields":{"MODE":"WHILE"},"inputs":{"BOOL":{"shadow":{"type":"blockLogicTrue","id":"bs#.#5q~?`jPpx|{;2/J"}},"DO":{"block":{"type":"blockIfElse","id":"g)5Tg*ThWzbFFX~nm,_J","extraState":{"optionLevel":1},"inputs":{"IF0":{"shadow":{"type":"blockLogicTrue","id":"^4CRD,`If/}b8bNI;Q[w"},"block":{"type":"blockLogicCompare","id":"zP*d;Fo)!d!sj+KNz~03","fields":{"OP1":"GT"},"inputs":{"A":{"shadow":{"type":"blockMathNumber","id":"eG^2tHCd]G4Xb%o8W4%X","fields":{"NUM":1}},"block":{"type":"blockObjectInstanceCallValue","id":"H_d{q{QuTSWz6M/lov=q","extraState":{"optionLevel":0},"fields":{"CALL_TYPE":"CLASS_CALL_METHOD_SYNC","METHOD":"width"},"inputs":{"VAR":{"shadow":{"type":"variables_get_object_instance","id":"lvcD#~8(!]8`zRpyIv}c","fields":{"VAR":{"id":"y}(R(irPCs%{|o6lO_,p","name":"line16","type":"ObjectInstance"}}}}}}},"B":{"shadow":{"type":"blockMathNumber","id":"oG94Ax,SdGxs7Mu!z09p","fields":{"NUM":10}}}}}},"DO0":{"block":{"type":"blockMotorDuty","id":"*H@FCoJtb3LB0_L(9Co]","inputs":{"VAR":{"shadow":{"type":"variables_get_simple_motor_device","id":"{~R9)bFzIr%,wV[o2zf~","fields":{"VAR":{"id":"~OI0ROvYql6qFl{ZcB-y","name":"motorL","type":"Motor"}}}},"VALUE0":{"shadow":{"type":"unit_percent","id":"63K$uabm%e_(M85M{.e6","fields":{"VALUE0":0}}}},"next":{"block":{"type":"blockMotorDuty","id":"?FUkhVEH_wx,K(vt9R|h","inputs":{"VAR":{"shadow":{"type":"variables_get_simple_motor_device","id":")bZu5vdJq}OlC2Vn7M[)","fields":{"VAR":{"id":"Ing[XT4ihrKvScll;RH)","name":"motorR","type":"Motor"}}}},"VALUE0":{"shadow":{"type":"unit_percent","id":"GCl;@LT%m^+hH;B!^%Vz","fields":{"VALUE0":0}}}}}}}},"ELSE":{"block":{"type":"blockVariableSetValue","id":"CKFE+}GA)2B+V$%yPzT6","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"(A8@/w;H[uns?*u:4F{p","fields":{"VAR":{"id":"@;t,nTG@GfC5P7pk6:8q","name":"error","type":"Any"}}}},"VALUE0":{"shadow":{"type":"blockMathNumber","id":":$bSO%7@hq-i[t3QHJ3a","fields":{"NUM":1}},"block":{"type":"blockObjectInstanceCallValue","id":"!2@1cr_/K|]~o/4sxGO0","extraState":{"optionLevel":0},"fields":{"CALL_TYPE":"CLASS_CALL_METHOD_SYNC","METHOD":"pos100"},"inputs":{"VAR":{"shadow":{"type":"variables_get_object_instance","id":"5X+-HgXm-Ri)xmckn1|r","fields":{"VAR":{"id":"y}(R(irPCs%{|o6lO_,p","name":"line16","type":"ObjectInstance"}}}}}}}},"next":{"block":{"type":"blockVariableSetValue","id":"oh,P/3;:0?i#){xn7Wf9","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"|*BM]XYOh8M,0|VOcoU6","fields":{"VAR":{"id":"D-0-1chai)1aeLs~6ga=","name":"PD","type":"Any"}}}},"VALUE0":{"shadow":{"type":"blockMathNumber","id":"4OX.UagjpfA}-N$M.h:a","fields":{"NUM":1}},"block":{"type":"blockMathArithmetic","id":"1J0s4B2(:{X=62Qx`Vw}","fields":{"OP":"ADD"},"inputs":{"A":{"shadow":{"type":"blockMathNumber","id":"bF$`C1beL2Wks]n[=eSm","fields":{"NUM":1}},"block":{"type":"blockMathArithmetic","id":"MJ0%RzvDXx%PfHUuE0_!","fields":{"OP":"MULTIPLY"},"inputs":{"A":{"shadow":{"type":"blockMathNumber","id":";th/ERrOg-=b$^K_/_u@","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"*5gQ;:gl/0yXE5JvQ,L7","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"(r]N.sU:3F(0m2Nk%5zk","fields":{"VAR":{"id":"@;t,nTG@GfC5P7pk6:8q","name":"error","type":"Any"}}}}}}},"B":{"shadow":{"type":"blockMathNumber","id":"QJr7f9RDpr:om*aZeuIc","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"DT!Vm;7lOS64Xq?zK|I7","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"#Xc9X@,`y!!_bQ3x{ME`","fields":{"VAR":{"id":"|[mVEEgQ)9J?frYA.y)4","name":"KP","type":"Any"}}}}}}}}}},"B":{"shadow":{"type":"blockMathNumber","id":"ig!xL=Db:xY41;/Th9I^","fields":{"NUM":1}},"block":{"type":"blockMathArithmetic","id":"M2K)d7GOmrV4]9(bV^zl","fields":{"OP":"MULTIPLY"},"inputs":{"A":{"shadow":{"type":"blockMathNumber","id":"5/AMlx[;t`GR.8tG98;6","fields":{"NUM":1}},"block":{"type":"blockMathArithmetic","id":"N-}[fy1(}826f2zn*1?y","fields":{"OP":"MINUS"},"inputs":{"A":{"shadow":{"type":"blockMathNumber","id":"u]y.)3UBP=u#C{N=UEkX","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"/m1=*lg-#IFS0kB6/6=n","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"44#x#Y[!)R|89N^==8h`","fields":{"VAR":{"id":"@;t,nTG@GfC5P7pk6:8q","name":"error","type":"Any"}}}}}}},"B":{"shadow":{"type":"blockMathNumber","id":":g9A)?_MZ#ermI)lLC1h","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"#9f{eC^QAcGX|G*Ak70}","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"k:}iKFT(Jkuv|,afH3iz","fields":{"VAR":{"id":"$KgwuV!E3BU!q,|M^-Nw","name":"lastError","type":"Any"}}}}}}}}}},"B":{"shadow":{"type":"blockMathNumber","id":"#D?nvk0GC1a?+(-x|](1","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"mv_d;4w@,C~^uW{}yKQu","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"E2btu$qi`J_xQW?7tXPq","fields":{"VAR":{"id":"j+TJczeiW_I(9n{06rA)","name":"KD","type":"Any"}}}}}}}}}}}}}},"next":{"block":{"type":"blockMotorDuty","id":"LyA}Py4l=kNJXALQCt=F","inputs":{"VAR":{"shadow":{"type":"variables_get_simple_motor_device","id":"7puR*?]M=hv./c%cyj[T","fields":{"VAR":{"id":"~OI0ROvYql6qFl{ZcB-y","name":"motorL","type":"Motor"}}}},"VALUE0":{"shadow":{"type":"unit_percent","id":"Qf_d.dvO.m4/Wnh*Evgs","fields":{"VALUE0":50}},"block":{"type":"blockMathArithmetic","id":"h%nhRxn;O*4nIrSInlyZ","fields":{"OP":"ADD"},"inputs":{"A":{"shadow":{"type":"blockMathNumber","id":"`X3eiZFY2sGJTj-wtW1}","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"p7ER+_7%3jollHP$sa^?","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"/Z*SxQPtw@X:y1Fl!sS3","fields":{"VAR":{"id":"t4-VC^pJ+;|U7[v:lZGf","name":"SPEED","type":"Any"}}}}}}},"B":{"shadow":{"type":"blockMathNumber","id":"S5|tXinC!+-gXD;fFu+?","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"2|-p8bv3Um*xBt`AoLVO","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"k5]*_E`RK)/M,?sS_[?!","fields":{"VAR":{"id":"D-0-1chai)1aeLs~6ga=","name":"PD","type":"Any"}}}}}}}}}}},"next":{"block":{"type":"blockMotorDuty","id":"h@o_j9J;D@i6:OLg14dj","inputs":{"VAR":{"shadow":{"type":"variables_get_simple_motor_device","id":"bQTkBrYf+.g1c8Kt!awV","fields":{"VAR":{"id":"Ing[XT4ihrKvScll;RH)","name":"motorR","type":"Motor"}}}},"VALUE0":{"shadow":{"type":"unit_percent","id":"?tnA+eAsXY%[L^$}t3).","fields":{"VALUE0":50}},"block":{"type":"blockMathArithmetic","id":"})~5b4fn6w?Qm[|[X.UX","fields":{"OP":"MINUS"},"inputs":{"A":{"shadow":{"type":"blockMathNumber","id":"=h;a-0BTsWfhUb$*$gL=","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"Exs$,@S#Zu%E2v(*5lo=","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"qVk@M7_=2hV9MhV=Eum+","fields":{"VAR":{"id":"t4-VC^pJ+;|U7[v:lZGf","name":"SPEED","type":"Any"}}}}}}},"B":{"shadow":{"type":"blockMathNumber","id":"!#kiHT/byZP2UA5#4E~5","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"s%2xh4H%jY%oM-eu[5H8","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"_y99E(a?^V%R|LEQfXdK","fields":{"VAR":{"id":"D-0-1chai)1aeLs~6ga=","name":"PD","type":"Any"}}}}}}}}}}},"next":{"block":{"type":"blockVariableSetValue","id":"`c0:x*ko1=ZLr}PV#IO0","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"5Bg~4ZA5f7zl$={9@t[e","fields":{"VAR":{"id":"$KgwuV!E3BU!q,|M^-Nw","name":"lastError","type":"Any"}}}},"VALUE0":{"shadow":{"type":"blockMathNumber","id":"4l}5^1maST?|MSID~VoG","fields":{"NUM":1}},"block":{"type":"blockVariableGetValue","id":"*Y$~dAqlIQG}{(bTFG89","inputs":{"VAR":{"shadow":{"type":"variables_get_any","id":"Q-62Lcq9u2,-,9Fg%KuC","fields":{"VAR":{"id":"@;t,nTG@GfC5P7pk6:8q","name":"error","type":"Any"}}}}}}}}}}}}}}}}}}},"next":{"block":{"type":"blockWaitTime","id":"?D)q!sq(9WN,Vg,0!(/_","inputs":{"VALUE0":{"shadow":{"type":"unit_time","id":"1f90--w/VG:d@bNZ68ym","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":"LH.Vk2/f?poqTKc$?7gx","type":"PrimeHub"},{"name":"motorL","id":"~OI0ROvYql6qFl{ZcB-y","type":"Motor"},{"name":"motorR","id":"Ing[XT4ihrKvScll;RH)","type":"Motor"},{"name":"SPEED","id":"t4-VC^pJ+;|U7[v:lZGf","type":"Any"},{"name":"KP","id":"|[mVEEgQ)9J?frYA.y)4","type":"Any"},{"name":"KD","id":"j+TJczeiW_I(9n{06rA)","type":"Any"},{"name":"error","id":"@;t,nTG@GfC5P7pk6:8q","type":"Any"},{"name":"lastError","id":"$KgwuV!E3BU!q,|M^-Nw","type":"Any"},{"name":"PD","id":"D-0-1chai)1aeLs~6ga=","type":"Any"},{"name":"MBC_LINE16","id":"~)k%w_AhO]Ki-q%A`,8F","type":"FunctionImported"},{"name":"line16","id":"y}(R(irPCs%{|o6lO_,p","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_sensors 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)