def __init__(self, sim = False): super(Core, self).__init__(sim) StateMachine.__init__(self) self.CC = Chase() self.BC = Behavior() self.block = 0 dsrv = DynamicReconfigureServer(PassingConfig, self.Callback)
def __init__(self, robot_num, sim=False):
super(Core, self).__init__(robot_num, sim)
StateMachine.__init__(self)
self.CC = Chase()
self.AC = Attack()
self.BC = Behavior()
self.sim = sim
def __init__(self, sim=False):
super(Core, self).__init__(sim)
StateMachine.__init__(self)
self.CC = Chase()
self.AC = Attack()
self.BC = Behavior()
self.left_ang = 0
self.dest_angle = 0
dsrv = DynamicReconfigureServer(RobotConfig, self.Callback)
def __init__(self, robot_num, sim = False): super(Core, self).__init__(robot_num, sim) StateMachine.__init__(self) self.CC = Chase() self.AC = Attack() self.BC = Behavior() self.goal_dis = 0 self.tStart = time.time() self.block = 0 dsrv = DynamicReconfigureServer(RobotConfig, self.Callback)
class Core(Robot, StateMachine):
def __init__(self, robot_num, sim=False):
self.CC = Chase()
self.AC = Attack()
self.sim = sim
super(Core, self).__init__(robot_num, sim)
StateMachine.__init__(self)
idle = State('Idle', initial=True)
chase = State('Chase')
attack = State('Attack')
shoot = State('Shoot')
toChase = idle.to(chase) | attack.to(chase) | chase.to.itself()
toIdle = chase.to(idle) | attack.to(idle)
toAttack = chase.to(attack) | attack.to.itself() | shoot.to(attack)
toShoot = attack.to(shoot)
def on_toChase(self, t, side):
o = self.CC.ClassicRounding(t[side]['ang'],\
t['ball']['dis'],\
t['ball']['ang'])
self.RobotCtrl(o['v_x'], o['v_y'], o['v_yaw'])
if self.RobotBallHandle():
self.toAttack(t, side)
else:
pass
def on_toIdle(self):
log("To Idle")
def on_toAttack(self, t, side):
o = self.AC.ClassicAttacking(t[side]['dis'], t[side]['ang'])
self.RobotCtrl(o['v_x'], o['v_y'], o['v_yaw'])
def on_toShoot(self, power, pos):
if self.RobotBallHandle():
self.RobotShoot(power, pos)
else:
print("NOT YET")
def PubCurrentState(self):
self.RobotStatePub(self.current_state.identifier)
def CheckBallHandle(self):
return self.RobotBallHandle()
def __init__(self, sim=False):
super(MyStateMachine, self).__init__(sim)
StateMachine.__init__(self)
self.CC = Chase()
self.AC = Attack()
dsrv = DynamicReconfigureServer(RobotConfig, self.Callback)
class MyStateMachine(Robot, StateMachine):
def __init__(self, sim=False):
super(MyStateMachine, self).__init__(sim)
StateMachine.__init__(self)
self.CC = Chase()
self.AC = Attack()
dsrv = DynamicReconfigureServer(RobotConfig, self.Callback)
def Callback(self, config, level):
self.game_start = config['game_start']
self.test = config['test']
self.our_side = config['our_side']
self.opp_side = 'Blue' if self.our_side == 'Yellow' else 'Yellow'
self.maximum_v = config['maximum_v']
self.minimum_v = config['minimum_v']
self.atk_shoot_ang = config['atk_shoot_ang']
self.atk_shoot_dis = config['atk_shoot_dis']
self.ChangeVelocityRange(config['minimum_v'], config['maximum_v'])
self.ChangeAngularVelocityRange(config['minimum_w'],
config['maximum_w'])
self.ChangeBallhandleCondition(config['ballhandle_dis'],
config['ballhandle_ang'])
return config
idle = State('Idle', initial=True)
chase = State('Chase')
attack = State('Attack')
shoot = State('Shoot')
toIdle = chase.to(idle) | attack.to(idle) | shoot.to(
idle) | idle.to.itself()
toChase = idle.to(chase) | attack.to(chase) | chase.to.itself()
toAttack = attack.to.itself() | shoot.to(attack) | chase.to(attack)
toShoot = attack.to(shoot) | idle.to(shoot)
def on_toIdle(self):
self.MotionCtrl(0, 0, 0)
def on_toChase(self, method="Classic"):
t = self.GetObjectInfo()
side = self.opp_side
if method == "Classic":
x, y, yaw = self.CC.StraightForward(\
t['ball']['dis'],\
t['ball']['ang'])
self.MotionCtrl(x, y, yaw)
def on_toAttack(self, method="Classic"):
t = self.GetObjectInfo()
side = self.opp_side
if method == "Classic":
x, y, yaw = self.AC.ClassicAttacking(t[side]['dis'],
t[side]['ang'])
self.MotionCtrl(x, y, yaw)
def on_toShoot(self, power, pos=1):
self.RobotShoot(power, pos)
def CheckBallHandle(self):
if self.RobotBallHandle():
## Back to normal from Accelerator
self.ChangeVelocityRange(self.minimum_v, self.maximum_v)
self.last_ball_dis = 0
return self.RobotBallHandle()
class Core(Robot, StateMachine):
idle = State('Idle', initial = True)
chase = State('Chase')
attack = State('Attack')
shoot = State('Shoot')
point = State('Point')
movement = State('Movement')
toIdle = chase.to(idle) | attack.to(idle) | movement.to(idle) | point.to(idle) | idle.to.itself()
toChase = idle.to(chase) | attack.to(chase) | chase.to.itself() | movement.to(chase) | point.to(chase)
toAttack = chase.to(attack) | attack.to.itself() | shoot.to(attack) | movement.to(attack)
toShoot = attack.to(shoot)
toMovement = chase.to(movement) | movement.to.itself()
toPoint = point.to.itself() | idle.to(point)
def Callback(self, config, level):
self.game_start = config['game_start']
self.game_state = config['game_state']
self.run_point = config['run_point']
self.our_side = config['our_side']
self.opp_side = 'Blue' if self.our_side == 'Yellow' else 'Yellow'
self.run_x = config['run_x']
self.run_y = config['run_y']
self.run_yaw = config['run_yaw']
self.strategy_mode = config['strategy_mode']
self.maximum_v = config['maximum_v']
self.ChangeVelocityRange(config['minimum_v'], config['maximum_v'])
self.ChangeAngularVelocityRange(config['minimum_w'], config['maximum_w'])
self.ChangeBallhandleCondition(config['ballhandle_dis'], config['ballhandle_ang'])
return config
def __init__(self, robot_num, sim = False):
super(Core, self).__init__(robot_num, sim)
StateMachine.__init__(self)
self.CC = Chase()
self.AC = Attack()
self.BC = Behavior()
self.goal_dis = 0
self.tStart = time.time()
self.block = 0
dsrv = DynamicReconfigureServer(RobotConfig, self.Callback)
def on_toIdle(self):
self.goal_dis = 0
for i in range(0, 10):
self.MotionCtrl(0,0,0)
log("To Idle1")
def on_toChase(self, method = "Classic"):
t = self.GetObjectInfo()
side = self.opp_side
if method == "Classic":
x, y, yaw = self.CC.ClassicRounding(t[side]['ang'],\
t['ball']['dis'],\
t['ball']['ang'])
elif method == "Straight":
x, y, yaw = self.CC.StraightForward(t['ball']['dis'], t['ball']['ang'])
def on_toAttack(self, t, side ,l,a):
x, y, yaw = self.AC.ClassicAttacking(t[side]['dis'], t[side]['ang'], l['ranges'], l['angle']['increment'],a)
# self.Accelerate(1, t, 80)
self.MotionCtrl(x, y, yaw)
log('attack')
def on_toAttack(self, method = "Classic"):
t = self.GetObjectInfo()
side = self.opp_side
if method == "Classic":
x, y, yaw = self.AC.ClassicAttacking(t[side]['dis'], t[side]['ang'])
self.MotionCtrl(x, y, yaw)
# elif method == "Cross_Over":
# x, y, yaw, _ = self.AC.cross_over(t, side, run)
# self.MotionCtrl(x, y, yaw)
def on_toShoot(self, power, pos = 1):
self.RobotShoot(power, pos)
def on_toMovement(self, method):
t = self.GetObjectInfo()
side = self.opp_side
if method == "Orbit":
x, y, yaw = self.BC.Orbit(t[side]['ang'])
self.MotionCtrl(x, y, yaw, True)
def on_toPoint(self):
if self.game_state == "Kick_Off" and self.our_side == "Yellow" :
x, y, yaw, arrived = self.BC.Go2Point(-60, 0, 0)
elif self.game_state == "Kick_Off" and self.our_side == "Blue" :
x, y, yaw, arrived = self.BC.Go2Point(60, 0, 180)
elif self.game_state == "Free_Kick" :
x, y, yaw, arrived = self.BC.Go2Point(100, 100, 90)
elif self.game_state == "Free_Ball" :
x, y, yaw, arrived = self.BC.Go2Point(-100, -100, 180)
elif self.game_state == "Throw_In" :
x, y, yaw, arrived = self.BC.Go2Point(-100, -100, 270)
elif self.game_state == "Coner_Kick":
x, y, yaw, arrived = self.BC.Go2Point(300, 200, 45)
elif self.game_state == "Penalty_Kick" :
x, y, yaw, arrived = self.BC.Go2Point(-100, 100, 135)
elif self.game_state == "Run_Specific_Point" :
x, y, yaw, arrived = self.BC.Go2Point(self.run_x, self.run_y, self.run_yaw)
else:
log("Unknown Game State")
self.MotionCtrl(x, y, yaw)
return arrived
def PubCurrentState(self):
self.RobotStatePub(self.current_state.identifier)
def CheckBallHandle(self):
return self.RobotBallHandle()
def Calculate(self, ntime):
return ntime - self.tStart
def Accelerate(self, do, t, maximum_v = 100):
if do :
if self.goal_dis == 0:
print('goal into')
self.tStart = t['time']
elif t['ball']['dis'] < self.goal_dis:
self.tStart = t['time']
elif t['ball']['dis'] >= self.goal_dis :
a = self.Calculate(t['time'])
if a >= 0.8:
print('accelerating')
self.ChangeVelocityRange(0, maximum_v)
self.goal_dis = t['ball']['dis']
else :
self.ChangeVelocityRange(0, maximum_v)
print('back to normal')
class Core(Robot, StateMachine):
def __init__(self, robot_num, sim=False):
super(Core, self).__init__(robot_num, sim)
StateMachine.__init__(self)
self.CC = Chase()
self.AC = Attack()
self.BC = Behavior()
self.sim = sim
idle = State('Idle', initial=True)
chase = State('Chase')
attack = State('Attack')
shoot = State('Shoot')
orbit = State('Orbit')
point = State('Point')
toIdle = chase.to(idle) | attack.to(idle) | orbit.to(idle) | point.to(
idle) | idle.to.itself()
toChase = idle.to(chase) | attack.to(chase) | chase.to.itself() | orbit.to(
chase) | point.to(chase)
toAttack = chase.to(attack) | attack.to.itself() | shoot.to(
attack) | orbit.to(attack)
toShoot = attack.to(shoot)
toOrbit = chase.to(orbit) | orbit.to.itself()
toPoint = point.to.itself() | idle.to(point)
def on_toIdle(self):
for i in range(0, 10):
self.MotionCtrl(0, 0, 0)
log("To Idle1")
def on_toChase(self, t, side, method="Classic"):
if method == "Classic":
x, y, yaw = self.CC.ClassicRounding(t[side]['ang'],\
t['ball']['dis'],\
t['ball']['ang'])
self.MotionCtrl(x, y, yaw)
elif method == "Straight":
x, y, yaw = self.CC.StraightForward(t['ball']['dis'],
t['ball']['ang'])
self.MotionCtrl(x, y, yaw)
def on_toAttack(self, t, side):
x, y, yaw = self.AC.ClassicAttacking(t[side]['dis'], t[side]['ang'])
self.MotionCtrl(x, y, yaw)
def on_toShoot(self, power, pos):
self.RobotShoot(power, pos)
def on_toOrbit(self, t, side):
x, y, yaw = self.CC.Orbit(t[side]['ang'])
self.MotionCtrl(x, y, yaw, True)
def on_toPoint(self, tx, ty, tyaw):
x, y, yaw, remaining = self.BC.Go2Point(tx, ty, tyaw)
self.MotionCtrl(x, y, yaw)
return remaining
def PubCurrentState(self):
self.RobotStatePub(self.current_state.identifier)
def CheckBallHandle(self):
return self.RobotBallHandle()
class Core(Robot, StateMachine):
last_ball_dis = 0
last_goal_dis = 0
last_time = time.time()
idle = State('Idle', initial=True)
chase = State('Chase')
attack = State('Attack')
shoot = State('Shoot')
point = State('Point')
movement = State('Movement')
toIdle = chase.to(idle) | attack.to(idle) | movement.to(idle) | point.to(
idle) | shoot.to(idle) | idle.to.itself()
toChase = idle.to(chase) | attack.to(
chase) | chase.to.itself() | movement.to(chase) | point.to(chase)
toAttack = attack.to.itself() | shoot.to(attack) | movement.to(
attack) | chase.to(attack) | point.to(attack)
toShoot = attack.to(shoot) | idle.to(shoot) | movement.to(shoot)
toMovement = chase.to(movement) | movement.to.itself() | idle.to(
movement) | point.to(movement)
toPoint = point.to.itself() | idle.to(point) | movement.to(
point) | chase.to(point)
def Callback(self, config, level):
self.game_start = config['game_start']
self.game_state = config['game_state']
self.chase_straight = config['chase_straight']
self.run_point = config['run_point']
self.our_side = config['our_side']
self.opp_side = 'Blue' if self.our_side == 'Yellow' else 'Yellow'
self.run_x = config['run_x']
self.run_y = config['run_y']
self.run_yaw = config['run_yaw']
self.strategy_mode = config['strategy_mode']
self.attack_mode = config['attack_mode']
self.maximum_v = config['maximum_v']
self.orb_attack_ang = config['orb_attack_ang']
self.atk_shoot_ang = config['atk_shoot_ang']
self.shooting_start = config['shooting_start']
self.Change_Plan = config['Change_Plan']
self.atk_shoot_dis = config['atk_shoot_dis']
self.my_role = config['role']
self.accelerate = config['Accelerate']
self.ball_speed = config['ball_pwm']
self.ChangeVelocityRange(config['minimum_v'], config['maximum_v'])
self.ChangeAngularVelocityRange(config['minimum_w'],
config['maximum_w'])
self.ChangeBallhandleCondition(config['ballhandle_dis'],
config['ballhandle_ang'])
self.SetMyRole(self.my_role)
return config
def __init__(self, sim=False):
super(Core, self).__init__(sim)
StateMachine.__init__(self)
self.CC = Chase()
self.AC = Attack()
self.BC = Behavior()
self.left_ang = 0
self.dest_angle = 0
dsrv = DynamicReconfigureServer(RobotConfig, self.Callback)
def on_toIdle(self):
self.goal_dis = 0
for i in range(0, 10):
self.MotionCtrl(0, 0, 0)
log("To Idle1")
def on_toChase(self, method="Classic"):
t = self.GetObjectInfo()
side = self.opp_side
if method == "Classic":
x, y, yaw = self.CC.ClassicRounding(t[side]['ang'],\
t['ball']['dis'],\
t['ball']['ang'])
elif method == "Straight":
x, y, yaw = self.CC.StraightForward(t['ball']['dis'],
t['ball']['ang'])
elif method == "Defense":
x, y, yaw = self.AC.Defense(t['ball']['dis'], t['ball']['ang'])
if self.accelerate:
self.Accelerator(80)
if self.ball_speed:
x = x + t['ball']['speed_pwm_x']
y = y + t['ball']['speed_pwm_y']
self.MotionCtrl(x, y, yaw)
def on_toAttack(self, method="Classic"):
t = self.GetObjectInfo()
side = self.opp_side
l = self.GetObstacleInfo()
if method == "Classic":
x, y, yaw = self.AC.ClassicAttacking(t[side]['dis'],
t[side]['ang'])
elif method == "Cut":
x, y, yaw = self.AC.Cut(t[side]['dis'], t[side]['ang'],
self.run_yaw)
elif method == "Post_up":
if t[side]['dis'] < 50:
t[side]['dis'] = 50
x, y, yaw = self.AC.Post_up(t[side]['dis'],\
t[side]['ang'],\
l['ranges'],\
l['angle']['increment'])
elif method == "Orbit":
x, y, yaw, arrived = self.BC.Orbit(t[side]['ang'])
self.MotionCtrl(x, y, yaw, True)
self.MotionCtrl(x, y, yaw)
def on_toShoot(self, power, pos=1):
self.RobotShoot(power, pos)
def on_toMovement(self, method):
t = self.GetObjectInfo()
position = self.GetRobotInfo()
side = self.opp_side
ourside = self.our_side
l = self.GetObstacleInfo()
#log('move')
if method == "Orbit":
x, y, yaw, arrived = self.BC.Orbit(t[side]['ang'])
self.MotionCtrl(x, y, yaw, True)
elif method == "Relative_ball":
x, y, yaw = self.BC.relative_ball(t[ourside]['dis'],\
t[ourside]['ang'],\
t['ball']['dis'],\
t['ball']['ang'])
self.MotionCtrl(x, y, yaw)
elif method == "Relative_goal":
x, y, yaw = self.BC.relative_goal(t[ourside]['dis'],\
t[ourside]['ang'],\
t['ball']['dis'],\
t['ball']['ang'])
self.MotionCtrl(x, y, yaw)
elif method == "Penalty_Kick":
x, y, yaw = self.BC.PenaltyTurning(side, self.run_yaw,
self.dest_angle)
self.left_ang = abs(yaw)
self.MotionCtrl(x, y, yaw)
elif method == "At_Post_up":
x, y, yaw = self.BC.Post_up(t[side]['dis'],\
t[side]['ang'],\
l['ranges'],\
l['angle']['increment'])
self.MotionCtrl(x, y, yaw)
def on_toPoint(self):
t = self.GetObjectInfo()
our_side = self.our_side
opp_side = self.opp_side
if self.run_yaw == 0:
yaw = t[our_side]['ang']
elif self.run_yaw == 180:
yaw = t[opp_side]['ang']
elif self.run_yaw == -180:
yaw = t['ball']['ang']
else:
yaw = self.run_yaw
x, y, yaw, arrived = self.BC.Go2Point(self.run_x, self.run_y, yaw)
self.MotionCtrl(x, y, yaw)
return arrived
def PubCurrentState(self):
self.RobotStatePub(self.current_state.identifier)
def CheckBallHandle(self):
if self.RobotBallHandle():
## Back to normal from Accelerator
self.ChangeVelocityRange(0, self.maximum_v)
Core.last_ball_dis = 0
return self.RobotBallHandle()
def Accelerator(self, exceed=100):
t = self.GetObjectInfo()
if Core.last_ball_dis == 0:
Core.last_time = time.time()
Core.last_ball_dis = t['ball']['dis']
elif t['ball']['dis'] >= Core.last_ball_dis:
if time.time() - Core.last_time >= 0.8:
self.ChangeVelocityRange(0, exceed)
else:
Core.last_time = time.time()
Core.last_ball_dis = t['ball']['dis']
def change_plan(self):
t = self.GetObjectInfo()
opp_side = self.opp_side
if Core.last_goal_dis == 0:
Core.last_time = time.time()
Core.last_goal_dis = t[opp_side]['dis']
elif t[opp_side]['dis'] >= Core.last_goal_dis:
if time.time() - Core.last_time >= 3:
return True
else:
Core.last_time = time.time()
Core.last_goal_dis = t[opp_side]['dis']
return False
def record_angle(self):
position = self.GetRobotInfo()
self.dest_angle = math.degrees(
position['imu_3d']['yaw']) - self.run_yaw
class Core(Robot, StateMachine):
last_ball_dis = 0
last_time = time.time()
idle = State('Idle', initial = True)
chase = State('Chase')
shoot = State('Shoot')
point = State('Point')
movement = State('Movement')
aim = State('Aim')
toIdle = chase.to(idle) | movement.to(idle) | point.to(idle) | idle.to.itself() | shoot.to(idle)
toChase = idle.to(chase) | chase.to.itself() | movement.to(chase) | point.to(chase)
toShoot = movement.to(shoot) | point.to(shoot) | aim.to(shoot)
toMovement = chase.to(movement) | movement.to.itself() | point.to(movement)
toPoint = point.to.itself() | idle.to(point) | chase.to(point) | movement.to(point) | shoot.to(point)
toAim = point.to(aim) | aim.to.itself()
def Callback(self, config, level):
self.game_level = config['level']
self.game_start = config['game_start']
self.strategy_mode = config['strategy_mode']
self.maximum_v = config['maximum_v']
self.using_orbit = config['using_orbit']
self.passing_power = config['passing_power']
self.target_vision_red = config['target_vision_red']
self.target_vision_yellow = config['target_vision_yellow']
self.target_vision_blue = config['target_vision_blue']
self.target_vision_white = config['target_vision_white']
padding_ball = config['padding_ball']
padding_target = config['padding_target']
adjust_ball1_x = config['adjust_ball1_x']
adjust_ball1_y = config['adjust_ball1_y']
adjust_ball2_x = config['adjust_ball2_x']
adjust_ball2_y = config['adjust_ball2_y']
adjust_ball3_x = config['adjust_ball3_x']
adjust_ball3_y = config['adjust_ball3_y']
adjust_ball4_x = config['adjust_ball4_x']
adjust_ball4_y = config['adjust_ball4_y']
adjust_target1_x = config['adjust_target1_x']
adjust_target1_y = config['adjust_target1_y']
adjust_target2_x = config['adjust_target2_x']
adjust_target2_y = config['adjust_target2_y']
adjust_target3_x = config['adjust_target3_x']
adjust_target3_y = config['adjust_target3_y']
adjust_target4_x = config['adjust_target4_x']
adjust_target4_y = config['adjust_target4_y']
ball1 = (-165 + padding_ball + adjust_ball1_x, 120 + adjust_ball1_y)
ball2 = (-165 + padding_ball + adjust_ball2_x, 40 + adjust_ball2_y)
ball3 = (-165 + padding_ball + adjust_ball3_x, -40 + adjust_ball3_y)
ball4 = (-165 + padding_ball + adjust_ball4_x, -120 + adjust_ball4_y)
target1 = (165 - padding_target + adjust_target1_x, 120 + adjust_target1_y)
target2 = (165 - padding_target + adjust_target2_x, 40 + adjust_target2_y)
target3 = (165 - padding_target + adjust_target3_x, -40 + adjust_target3_y)
target4 = (165 - padding_target + adjust_target4_x, -120 + adjust_target4_y)
self.level1 = {'balls_point': [ball1], 'targets_point': [target1], 'targets_color': ['red']}
self.level2 = {'balls_point': [ball1, ball4], 'targets_point': [target1, target4], 'targets_color': ['red', 'yellow']}
self.level3 = {'balls_point': [ball1, ball2, ball4], 'targets_point': [target4, target2, target1], 'targets_color': ['yellow', 'blue', 'red']}
self.level4 = {'balls_point': [ball2, ball3, ball4, ball1], 'targets_point': [target4, target1, target2, target3], 'targets_color': ['yellow', 'red', 'blue', 'white']}
self.ChangeVelocityRange(config['minimum_v'], config['maximum_v'])
self.ChangeAngularVelocityRange(config['minimum_w'], config['maximum_w'])
self.ChangeBallhandleCondition(config['ballhandle_dis'], config['ballhandle_ang'])
return config
def __init__(self, sim = False):
super(Core, self).__init__(sim)
StateMachine.__init__(self)
self.CC = Chase()
self.BC = Behavior()
self.block = 0
dsrv = DynamicReconfigureServer(PassingConfig, self.Callback)
def on_toIdle(self):
self.goal_dis = 0
for i in range(0, 10):
self.MotionCtrl(0,0,0)
log("To Idle1")
def on_toAim(self):
t = self.GetObjectInfo()
print("Aim to ", Strategy.aim_target)
if Strategy.aim_target == "Red":
ta = t['Red']['ang']
elif Strategy.aim_target == "Blue":
ta = t['Blue']['ang']
elif Strategy.aim_target == "Yellow":
ta = t['Yellow']['ang']
elif Strategy.aim_target == "White":
ta = t['White']['ang']
else:
ta = 0
if abs(ta) < 1:
print("It's okay")
return True
else:
self.MotionCtrl(0, 0, ta)
return False
def on_toChase(self, method = "Straight"):
t = self.GetObjectInfo()
# side = self.opp_side
# if method == "Classic":
# x, y, yaw = self.CC.ClassicRounding(t[side]['ang'],\
# t['ball']['dis'],\
# t['ball']['ang'])
# elif method == "Straight":
x, y, yaw = self.CC.StraightForward(t['ball']['dis'], t['ball']['ang'])
self.MotionCtrl(x, y, yaw)
def on_toShoot(self, power, pos = 1):
self.RobotShoot(power, pos)
def on_toMovement(self, tang):
x, y, yaw, arrived = self.BC.Orbit(tang)
self.MotionCtrl(x, y, yaw, True)
return arrived
def on_toPoint(self, tx, ty, tyaw, rV = 5, rW = 1):
x, y, yaw, arrived = self.BC.Go2Point(tx, ty, tyaw, rV, rW)
self.MotionCtrl(x, y, yaw)
return arrived
def PubCurrentState(self):
self.RobotStatePub(self.current_state.identifier)
def CheckBallHandle(self):
if self.RobotBallHandle():
## Back to normal from Accelerator
self.ChangeVelocityRange(0, self.maximum_v)
Core.last_ball_dis = 0
return self.RobotBallHandle()
def Accelerator(self, exceed = 100):
t = self.GetObjectInfo()
if Core.last_ball_dis == 0:
Core.last_time = time.time()
Core.last_ball_dis = t['ball']['dis']
elif t['ball']['dis'] >= Core.last_ball_dis:
if time.time() - Core.last_time >= 0.8:
self.ChangeVelocityRange(0, exceed)
else:
Core.last_time = time.time()
Core.last_ball_dis = t['ball']['dis']
