def shootBall():
m=yield {}
while True:
DegToGoal=toFloat(m[degToGoal])
if is_true(m[BInRobot]):
if abs(DegToGoal)<=MaxAngToGoal:
m=yield {Request([Su,BInRobot]):And(Su==100, BInRobot==false),
Block: Or(Su!=100, BInRobot==true)}
else:
m=yield {Request([Su,BInRobot]):And(Su==-100, BInRobot==true),
Block: Or(Su!=-100, BInRobot==false)}
else:
m=yield {}
def getBall():
m=yield {}
while True:
Dist=toFloat(m[dist])
if is_false(m[BInRobot]):
if Dist<4.3:
m=yield {Request([Su,BInRobot]):And(Su==-100, BInRobot==true),
Block: Or(Su!=-100, BInRobot==false)}
else:
m=yield {Request([Su,BInRobot]):And(Su==-100, BInRobot==false),
Block: Or(Su!=-100, BInRobot==true)}
else:
m=yield {}
def spinToGoal():
m=yield {}
while True:
DegToGoal=toFloat(m[degToGoal])
if is_true(m[BInRobot]):
if (DegToGoal > MaxAngToGoal):
m=yield {Request([spin,forward]): spin>0,
Block: spin<=0}
elif (DegToGoal < -MaxAngToGoal):
m=yield {Request([spin,forward]): spin<0,
Block: spin>=0}
else:
m=yield {Request([spin,forward]): spin==0}
else:
m=yield {}
def forwardMotion():
m=yield {}
while True:
Dist=toFloat(m[dist])
if (Dist>TooClose):
if (Dist<TooFar):
m=yield {Request([spin,forward]): forward == ((Dist-TooClose)/(TooFar-TooClose))*MaxPower}
else:
m=yield {Request([spin,forward]): forward == MaxPower}
else:
if (Dist>(2*TooClose-TooFar)):
m=yield {Request([spin,forward]): forward==((Dist-TooClose)/(TooFar-TooClose))*MaxPower}
else:
m=yield {Request([spin,forward]): forward==-MaxPower}
def initReq():
m=yield {Request([forward,spin,Su,BInRobot,dist, degToBall, degToGoal]):And(forward == 0, spin == 0, Su==0, BInRobot==False,dist==0, degToBall==0, degToGoal==0),
Block: Or(forward != 0, spin != 0, Su!=0, BInRobot==True,dist!=0, degToBall!=0, degToGoal!=0)}
def telemUpdater():
yield {}
while True:
[Dist, DDeg, GDDeg]=getTelem()
yield {Request([dist, degToBall, degToGoal]):And(dist==Dist, degToBall==DDeg,degToGoal==GDDeg),
Block: Or(dist!=Dist, degToBall!=DDeg,degToGoal!=GDDeg)}