import time
+from compLib.Motor import Motor
+from compLib.Display import Display
+from compLib.IRSensor import IRSensor
+from compLib.Encoder import Encoder
-from compLib.IRWrapper import IRWrapper
-from compLib.Motor import Motor
+import time
-STRAIGHT_SPEED = 40.0
-TURN_SPEED = 40.0
+IRSensor.set(1, True)
+IRSensor.set(2, True)
+IRSensor.set(3, True)
+IRSensor.set(4, True)
+IRSensor.set(5, True)
-COLOR_BREAK = 500
+DRIVE_SPEED = 85
+TURN_SPEED = 50
+COLOR_BREAK = 750
+KP = 50.0
+KD = 0.0
+def drive(leftSpeed, rightSpeed):
+ leftSpeed *= -1
+ rightSpeed *= 0.906
-def drive(left, right, active_break=False, back_mult=0.0):
- left = max(min(left, 100), -100)
- right = max(min(right, 100), -100)
+ Motor.power(1, min(max(-100, leftSpeed), 100))
+ Motor.power(2, min(max(-100, rightSpeed), 100))
- print("left {} right {}".format(left, right))
+def follow(sleepTime = 0.1):
+ lastError = 0
+ sensorsBlack = 0
+ while sensorsBlack < 3:
+ sensorsBlack = 0
+ for i in range(1, 6):
+ if IRSensor.read(i) > COLOR_BREAK:
+ sensorsBlack += 1
- Motor.power(0, left)
- Motor.power(1, left * back_mult)
- Motor.power(2, right * back_mult)
- Motor.power(3, right)
+ error = lastError
+ if IRSensor.read(3) > COLOR_BREAK:
+ error = 0
+ elif IRSensor.read(1) > COLOR_BREAK:
+ error = -1.5
+ elif IRSensor.read(5) > COLOR_BREAK:
+ error = 1.5
+ elif IRSensor.read(2) > COLOR_BREAK:
+ error = -1
+ elif IRSensor.read(4) > COLOR_BREAK:
+ error = 1
+ elif error == 1.5:
+ error = 3
+ elif error == -1.5:
+ error = -3
- if active_break:
- time.sleep(0.001)
+ lastError = error
- Motor.power(0, 10)
- Motor.power(1, 10)
- Motor.power(2, 10)
- Motor.power(3, 10)
+ adjustment = KP * error + KD * (error - lastError)
+ leftSpeed = DRIVE_SPEED + adjustment
+ rightSpeed = DRIVE_SPEED - adjustment
- time.sleep(0.001)
-
-
-def follow_till_line():
- while True:
- print("left: {:.2f} middle: {:.2f} right: {:.2f}".format(IRWrapper.bottom_left_calibrated(),
- IRWrapper.bottom_middle_calibrated(),
- IRWrapper.bottom_right_calibrated()))
-
- left = IRWrapper.bottom_left_calibrated()
- right = IRWrapper.bottom_right_calibrated()
-
- if left > COLOR_BREAK:
- drive(-TURN_SPEED, TURN_SPEED, back_mult=1.25)
- while IRWrapper.bottom_middle_calibrated() < COLOR_BREAK and \
- IRWrapper.bottom_right_calibrated() < COLOR_BREAK:
- pass
- drive(0, 0)
- elif right > COLOR_BREAK:
- drive(TURN_SPEED, -TURN_SPEED, back_mult=1.25)
- while IRWrapper.bottom_middle_calibrated() < COLOR_BREAK and \
- IRWrapper.bottom_left_calibrated() < COLOR_BREAK:
- pass
- drive(0, 0)
- else:
- drive(STRAIGHT_SPEED, STRAIGHT_SPEED, True)
-
- if IRWrapper.bottom_left_calibrated() > 750 and IRWrapper.bottom_right_calibrated() > 750:
- break
-
- print("left: {} middle: {} right: {}".format(IRWrapper.bottom_left_calibrated(),
- IRWrapper.bottom_middle_calibrated(),
- IRWrapper.bottom_right_calibrated()))
+ print(f"{leftSpeed} {rightSpeed} {adjustment} {error}")
+ drive(leftSpeed, rightSpeed)
drive(0, 0)
+ time.sleep(sleepTime)
- time.sleep(0.2)
+def main():
+ follow()
+ follow()
+ follow()
+ follow()
+ follow(0.2)
-
-if __name__ == "__main__":
- # Put bot with middle sensor onto black line
- IRWrapper.calibrate()
-
- # Initialize Motors, so the bot doesn't speed off instantly
- drive(1, 1)
- time.sleep(1)
-
- # Follow line, till the left and right sensor are on black
- follow_till_line()
+main()
diff --git a/searchindex.js b/searchindex.js
index a2f2778..452a635 100644
--- a/searchindex.js
+++ b/searchindex.js
@@ -1 +1 @@
-Search.setIndex({docnames:["index","lib/Api","lib/Aruco","lib/Display","lib/Encoder","lib/IRSensor","lib/Linefollower","lib/Logging","lib/Motor","lib/Servo","lib/Vision","other/usage"],envversion:{"sphinx.domains.c":2,"sphinx.domains.changeset":1,"sphinx.domains.citation":1,"sphinx.domains.cpp":3,"sphinx.domains.index":1,"sphinx.domains.javascript":2,"sphinx.domains.math":2,"sphinx.domains.python":2,"sphinx.domains.rst":2,"sphinx.domains.std":1,sphinx:56},filenames:["index.rst","lib/Api.rst","lib/Aruco.rst","lib/Display.rst","lib/Encoder.rst","lib/IRSensor.rst","lib/Linefollower.rst","lib/Logging.rst","lib/Motor.rst","lib/Servo.rst","lib/Vision.rst","other/usage.rst"],objects:{"compLib.Api":{DoubleElim:[1,0,1,""],Position:[1,0,1,""],Seeding:[1,0,1,""]},"compLib.Api.DoubleElim":{get_goal:[1,1,1,""],get_items:[1,1,1,""],get_opponent:[1,1,1,""],get_position:[1,1,1,""],get_scores:[1,1,1,""]},"compLib.Api.Seeding":{get_park:[1,1,1,""],pay_park:[1,1,1,""],simon_says:[1,1,1,""]},"compLib.Display":{Display:[3,0,1,""]},"compLib.Display.Display":{clear:[3,1,1,""],write:[3,1,1,""]},"compLib.Encoder":{Encoder:[4,0,1,""]},"compLib.Encoder.Encoder":{clear:[4,1,1,""],clear_all:[4,1,1,""],read:[4,1,1,""],read_raw:[4,1,1,""]},"compLib.IRSensor":{IRSensor:[5,0,1,""]},"compLib.IRSensor.IRSensor":{read:[5,1,1,""],set:[5,1,1,""]},"compLib.LogstashLogging":{Logging:[7,0,1,""]},"compLib.LogstashLogging.Logging":{get_logger:[7,1,1,""],set_debug:[7,1,1,""]},"compLib.Motor":{Motor:[8,0,1,""]},"compLib.Motor.Motor":{active_break:[8,1,1,""],all_off:[8,1,1,""],get_motor_curve:[8,1,1,""],power:[8,1,1,""],power_raw:[8,1,1,""],pwm:[8,1,1,""],set_motor_curve:[8,1,1,""]},"compLib.Vision":{__Streaming:[10,0,1,""]},"compLib.Vision.__Streaming":{get_frame:[10,1,1,""],publish_frame:[10,1,1,""]}},objnames:{"0":["py","class","Python class"],"1":["py","method","Python method"]},objtypes:{"0":"py:class","1":"py:method"},terms:{"001":[6,10],"100":[6,8],"204":1,"403":1,"480":2,"500":6,"640":2,"750":6,"9898":10,"break":[6,8],"class":[0,1,5,10],"enum":8,"float":8,"function":2,"import":[1,2,3,5,6,7,8,10,11],"int":[1,3,4,5,8],"return":[1,2,4,5,7,8,10],"static":[1,3,4,5,7,8],"true":[2,5,6,8,10],"while":[2,5,6,10],BUT:10,For:10,NOT:10,The:[2,10],Use:10,Using:0,Will:4,With:2,__main__:[2,6,11],__name__:[2,6,11],__stream:10,access:[3,5],accuraci:5,act:2,activ:8,active_break:[6,8],all:[1,4,8],all_off:8,allow:8,alwai:1,analog:0,api:0,arrai:8,aruco:0,aruco_dict:2,aruco_paramet:2,automat:10,back:1,back_mult:6,backward:11,base:2,becaus:10,beep:11,between:[3,4,5,8],bin:11,bit:[5,10],black:6,bool:5,bot:6,bottom:2,bottom_left_calibr:6,bottom_middle_calibr:6,bottom_right_calibr:6,browser:10,buffer:10,buup:11,calibr:6,call:0,cam:10,camera:10,can:[1,2,7,10],captur:10,center:2,charact:3,check:[1,8],chessboard:0,clear:[3,4],clear_al:4,code:1,color_bgr2grai:[2,10],color_break:6,come:10,commun:[1,7],competit:7,complib:[1,2,3,4,5,6,7,8,10,11],connect:[4,8,10],control:8,convert:10,coordin:[1,2],copi:2,corner:[2,10],correct:1,creat:10,criteria:10,current:[1,4,8],curv:8,cv2:[2,10],cvtcolor:[2,10],datastructur:1,debug:7,def:[2,6,11],degre:1,desiredid:2,detect:0,detectmark:2,detectorparameters_cr:2,dict:1,dict_6x6_250:2,dictionari:1,dictionary_get:2,differ:[3,5],displai:[0,10],document:[0,10],doesn:6,done:10,doubl:0,doubleelim:1,draw:10,drawchessboardcorn:10,drawdetectedmark:2,drive:[0,1,6],easi:2,ein:11,element:8,elif:[1,6],elimin:0,els:[1,2,6],emitt:5,encod:0,exactli:8,exampl:0,fail:1,fals:6,field:1,find:10,findchessboardcorn:10,follow:6,follow_till_lin:6,format:[5,6],forward:11,found:2,frame:[2,10],from:[1,2,3,4,5,6,7,8,10,11],game:1,get:[1,2,7,8,10],get_fram:[2,10],get_goal:1,get_item:1,get_logg:7,get_motor_curv:8,get_oppon:1,get_park:1,get_posit:1,get_scor:1,getnormalizedtagposit:2,gettagcenterfromfram:2,gettagposit:2,global:8,goal:1,grab:10,grai:[2,10],grayscal:10,hallo:11,handl:10,height:2,hello:3,here:10,hold:1,hopefulli:1,how:[2,10],http:10,ich:11,ids:2,imag:10,includ:1,indexerror:[3,4,5,8],info:8,infrar:0,initi:6,instanc:10,instantli:6,integr:3,interfac:10,internet:10,irsensor:5,irwrapp:6,item:1,json:1,lag:10,left:[2,5,6],level:7,librari:7,like:[1,10],line:[0,6],linear:8,linefollow:0,list:1,littl:10,log:0,log_metr:8,logger:7,logstash:7,logstashlog:7,look:[1,10],main:11,make:1,manual:7,max:[6,8],mayb:0,middl:[5,6],might:10,min:6,mode:8,modul:[0,1],more:8,motor:[0,1,4,6,11],motormod:8,move:1,must:8,need:10,newest:10,next:1,none:[2,10],normal:2,note:10,now:1,object:[1,7],off:[5,6],one:5,onto:[6,10],opencv:0,oppon:1,organ:7,output:10,own:10,pai:1,paramet:[2,3,4,5,8,10],park:1,parkingsapc:1,pass:6,pay_park:1,percent:8,percentag:8,pixel:2,point:[1,10],port:[4,8],posit:[0,2,4,10],power:[6,8,11],power_raw:8,print:[1,2,5,6,11],process:10,proport:8,provid:10,publish:10,publish_fram:[2,10],put:6,pwm:8,python:7,quit:2,rais:[3,4,5,8],rang:2,raspberri:10,raw:[4,8],read:[4,5],read_raw:4,realtim:10,recogn:0,record:10,rejectedimgpoint:2,request:1,reset:4,respond:1,ret:10,right:[2,5,6],robot:[1,3,5,11],rotat:1,roughli:8,rtmp:10,run:10,sai:1,same:2,score:1,screen:2,screenshot:10,second:10,see:8,seed:0,self:1,sensor:[0,3,6],server:[1,10],servo:0,set:[5,8],set_debug:7,set_motor_curv:8,shape:2,should:[1,7,8,10],show:[2,10],similar:1,simon:1,simon_sai:1,simpl:[0,2],sleep:[6,11],some:[1,10],someth:1,specif:8,specifi:[2,4,8],speed:[6,8],start:4,statu:1,str:3,straight:0,straight_spe:6,stream:[0,2],string:3,success:1,tag:0,tag_id:2,take:10,term_criteria_ep:10,term_criteria_max_it:10,test:[0,10],text:3,thi:[1,2,7,10],tick:8,till:6,time:[2,3,6,11],top:2,tupl:1,turn:[0,5,8],turn_spe:6,type:[1,5,7],usag:0,use:[2,7,10],used:[1,4,7,8],using:[1,10],valu:[5,8],valueerror:8,veri:2,view:10,vision:[0,2],wai:2,wait:10,want:[2,10],web:10,webserv:10,websit:10,whatev:10,when:[7,10],which:[1,4,5,8,10],width:2,world:3,write:0,yet:1,you:[2,10],your:10,your_raspi_ip:10,yourself:10,zip:2,zone:1},titles:["Competition Robot Library","Api","Aruco","Display","Encoder","Infrared Sensor","Linefollower Examples","Logging","Motor","Servo","Vision","Usage"],titleterms:{"class":[3,4,7,8],Using:10,analog:5,api:1,aruco:2,call:1,chessboard:10,competit:0,content:0,detect:10,displai:3,document:[3,4,7,8],doubl:1,drive:8,elimin:1,encod:4,exampl:[1,2,3,5,6,7,8,10],infrar:5,librari:0,line:3,linefollow:6,log:7,mayb:8,modul:10,motor:8,opencv:10,posit:1,recogn:2,robot:0,seed:1,sensor:5,servo:9,simpl:6,straight:8,stream:10,tag:2,test:5,turn:7,usag:11,vision:10,write:3}})
\ No newline at end of file
+Search.setIndex({docnames:["index","lib/Api","lib/Aruco","lib/Display","lib/Encoder","lib/IRSensor","lib/Linefollower","lib/Logging","lib/Motor","lib/Servo","lib/Vision","other/usage"],envversion:{"sphinx.domains.c":2,"sphinx.domains.changeset":1,"sphinx.domains.citation":1,"sphinx.domains.cpp":3,"sphinx.domains.index":1,"sphinx.domains.javascript":2,"sphinx.domains.math":2,"sphinx.domains.python":2,"sphinx.domains.rst":2,"sphinx.domains.std":1,sphinx:56},filenames:["index.rst","lib/Api.rst","lib/Aruco.rst","lib/Display.rst","lib/Encoder.rst","lib/IRSensor.rst","lib/Linefollower.rst","lib/Logging.rst","lib/Motor.rst","lib/Servo.rst","lib/Vision.rst","other/usage.rst"],objects:{"compLib.Api":{DoubleElim:[1,0,1,""],Position:[1,0,1,""],Seeding:[1,0,1,""]},"compLib.Api.DoubleElim":{get_goal:[1,1,1,""],get_items:[1,1,1,""],get_opponent:[1,1,1,""],get_position:[1,1,1,""],get_scores:[1,1,1,""]},"compLib.Api.Seeding":{get_park:[1,1,1,""],pay_park:[1,1,1,""],simon_says:[1,1,1,""]},"compLib.Display":{Display:[3,0,1,""]},"compLib.Display.Display":{clear:[3,1,1,""],write:[3,1,1,""]},"compLib.Encoder":{Encoder:[4,0,1,""]},"compLib.Encoder.Encoder":{clear:[4,1,1,""],clear_all:[4,1,1,""],read:[4,1,1,""],read_raw:[4,1,1,""]},"compLib.IRSensor":{IRSensor:[5,0,1,""]},"compLib.IRSensor.IRSensor":{read:[5,1,1,""],set:[5,1,1,""]},"compLib.LogstashLogging":{Logging:[7,0,1,""]},"compLib.LogstashLogging.Logging":{get_logger:[7,1,1,""],set_debug:[7,1,1,""]},"compLib.Motor":{Motor:[8,0,1,""]},"compLib.Motor.Motor":{active_break:[8,1,1,""],all_off:[8,1,1,""],get_motor_curve:[8,1,1,""],power:[8,1,1,""],power_raw:[8,1,1,""],pwm:[8,1,1,""],set_motor_curve:[8,1,1,""]},"compLib.Vision":{__Streaming:[10,0,1,""]},"compLib.Vision.__Streaming":{get_frame:[10,1,1,""],publish_frame:[10,1,1,""]}},objnames:{"0":["py","class","Python class"],"1":["py","method","Python method"]},objtypes:{"0":"py:class","1":"py:method"},terms:{"001":10,"100":[6,8],"204":1,"403":1,"480":2,"640":2,"750":6,"906":6,"9898":10,"break":8,"class":[0,1,5,10],"enum":8,"float":8,"function":2,"import":[1,2,3,5,6,7,8,10,11],"int":[1,3,4,5,8],"return":[1,2,4,5,7,8,10],"static":[1,3,4,5,7,8],"true":[2,5,6,8,10],"while":[2,5,6,10],BUT:10,For:10,NOT:10,The:[2,10],Use:10,Using:0,Will:4,With:2,__main__:[2,11],__name__:[2,11],__stream:10,access:[3,5],accuraci:5,act:2,activ:8,active_break:8,adjust:6,all:[1,4,8],all_off:8,allow:8,alwai:1,analog:0,api:0,arrai:8,aruco:0,aruco_dict:2,aruco_paramet:2,automat:10,back:1,backward:11,base:2,becaus:10,beep:11,between:[3,4,5,8],bin:11,bit:[5,10],bool:5,bottom:2,browser:10,buffer:10,buup:11,call:0,cam:10,camera:10,can:[1,2,7,10],captur:10,center:2,charact:3,check:[1,8],chessboard:0,clear:[3,4],clear_al:4,code:1,color_bgr2grai:[2,10],color_break:6,come:10,commun:[1,7],competit:7,complib:[1,2,3,4,5,6,7,8,10,11],connect:[4,8,10],control:8,convert:10,coordin:[1,2],copi:2,corner:[2,10],correct:1,creat:10,criteria:10,current:[1,4,8],curv:8,cv2:[2,10],cvtcolor:[2,10],datastructur:1,debug:7,def:[2,6,11],degre:1,desiredid:2,detect:0,detectmark:2,detectorparameters_cr:2,dict:1,dict_6x6_250:2,dictionari:1,dictionary_get:2,differ:[3,5],displai:[0,6,10],document:[0,10],done:10,doubl:0,doubleelim:1,draw:10,drawchessboardcorn:10,drawdetectedmark:2,drive:[0,1,6],drive_spe:6,easi:2,ein:11,element:8,elif:[1,6],elimin:0,els:[1,2],emitt:5,encod:[0,6],error:6,exactli:8,exampl:0,fail:1,field:1,find:10,findchessboardcorn:10,follow:6,format:5,forward:11,found:2,frame:[2,10],from:[1,2,3,4,5,6,7,8,10,11],game:1,get:[1,2,7,8,10],get_fram:[2,10],get_goal:1,get_item:1,get_logg:7,get_motor_curv:8,get_oppon:1,get_park:1,get_posit:1,get_scor:1,getnormalizedtagposit:2,gettagcenterfromfram:2,gettagposit:2,global:8,goal:1,grab:10,grai:[2,10],grayscal:10,hallo:11,handl:10,height:2,hello:3,here:10,hold:1,hopefulli:1,how:[2,10],http:10,ich:11,ids:2,imag:10,includ:1,indexerror:[3,4,5,8],info:8,infrar:0,instanc:10,integr:3,interfac:10,internet:10,irsensor:[5,6],item:1,json:1,lag:10,lasterror:6,left:[2,5],leftspe:6,level:7,librari:7,like:[1,10],line:0,linear:8,linefollow:0,list:1,littl:10,log:0,log_metr:8,logger:7,logstash:7,logstashlog:7,look:[1,10],main:[6,11],make:1,manual:7,max:[6,8],mayb:0,middl:5,might:10,min:6,mode:8,modul:[0,1],more:8,motor:[0,1,4,6,11],motormod:8,move:1,must:8,need:10,newest:10,next:1,none:[2,10],normal:2,note:10,now:1,object:[1,7],off:5,one:5,onto:10,opencv:0,oppon:1,organ:7,output:10,own:10,pai:1,paramet:[2,3,4,5,8,10],park:1,parkingsapc:1,pay_park:1,percent:8,percentag:8,pixel:2,point:[1,10],port:[4,8],posit:[0,2,4,10],power:[6,8,11],power_raw:8,print:[1,2,5,6,11],process:10,proport:8,provid:10,publish:10,publish_fram:[2,10],pwm:8,python:7,quit:2,rais:[3,4,5,8],rang:[2,6],raspberri:10,raw:[4,8],read:[4,5,6],read_raw:4,realtim:10,recogn:0,record:10,rejectedimgpoint:2,request:1,reset:4,respond:1,ret:10,right:[2,5],rightspe:6,robot:[1,3,5,11],rotat:1,roughli:8,rtmp:10,run:10,sai:1,same:2,score:1,screen:2,screenshot:10,second:10,see:8,seed:0,self:1,sensor:[0,3],sensorsblack:6,server:[1,10],servo:0,set:[5,6,8],set_debug:7,set_motor_curv:8,shape:2,should:[1,7,8,10],show:[2,10],similar:1,simon:1,simon_sai:1,simpl:[0,2],sleep:[6,11],sleeptim:6,some:[1,10],someth:1,specif:8,specifi:[2,4,8],speed:8,start:4,statu:1,str:3,straight:0,stream:[0,2],string:3,success:1,tag:0,tag_id:2,take:10,term_criteria_ep:10,term_criteria_max_it:10,test:[0,10],text:3,thi:[1,2,7,10],tick:8,time:[2,3,6,11],top:2,tupl:1,turn:[0,5,8],turn_spe:6,type:[1,5,7],usag:0,use:[2,7,10],used:[1,4,7,8],using:[1,10],valu:[5,8],valueerror:8,veri:2,view:10,vision:[0,2],wai:2,wait:10,want:[2,10],web:10,webserv:10,websit:10,whatev:10,when:[7,10],which:[1,4,5,8,10],width:2,world:3,write:0,yet:1,you:[2,10],your:10,your_raspi_ip:10,yourself:10,zip:2,zone:1},titles:["Competition Robot Library","Api","Aruco","Display","Encoder","Infrared Sensor","Linefollower Examples","Logging","Motor","Servo","Vision","Usage"],titleterms:{"class":[3,4,7,8],Using:10,analog:5,api:1,aruco:2,call:1,chessboard:10,competit:0,content:0,detect:10,displai:3,document:[3,4,7,8],doubl:1,drive:8,elimin:1,encod:4,exampl:[1,2,3,5,6,7,8,10],infrar:5,librari:0,line:3,linefollow:6,log:7,mayb:8,modul:10,motor:8,opencv:10,posit:1,recogn:2,robot:0,seed:1,sensor:5,servo:9,simpl:6,straight:8,stream:10,tag:2,test:5,turn:7,usag:11,vision:10,write:3}})
\ No newline at end of file