class Robot():
# 100 ms update (stable frequency) / 50 ms update (maximum frequency)
PERIODIC_DELAY = 0.05
stopped = False
def __init__(self):
self.drive = Drive()
self.oi = OperatorInterface(self.drive)
self.drive.setOperatorInterface(self.oi)
def update(self):
self.drive.update()
def stop(self):
self.stopped = True
self.drive.stop()
self.oi.stop()
def periodic(self):
nextTime = time.time()
while (not self.stopped):
try:
self.update()
nextTime = nextTime + self.PERIODIC_DELAY
delay = max(0, nextTime - time.time())
if (delay == 0):
print("--------------THRASH--------------")
time.sleep(delay)
except:
print("Exception in Periodic")
traceback.print_exc()
self.stop()
class Navigation():
def __init__(self):
# this is only changed once the ball is kicked
self.switch_goal = False
self.prev_state = state.none
self.prev_ball = -1
self.prev_goal = -1
# CONDITIONS
self.ball_in_dribbler = False
self.ball_found = False
self.goal_found = False
self.goal_aligned = False
# STATES
self.state = state.none
self.search = search.none
# OBJECTS
self.drive_sys = Drive()
self.dribbler = Dribbler()
self.field = Field()
self.kicker = Kicker()
# MOTOR pwm
self.left_motor = 0
self.right_motor = 0
# range/headings
self.obs_range = [None, None, None]
self.obs_heading = [None, None, None]
self.ball_range = None
self.ball_heading = None
self.goal_range = None
self.goal_heading = None
####################
# GENERIC FUNCTIONS
#
# input: left and right motor pwms
# output: pwm value that gets larger the sharper the robot is turning
# WORK IN PROGRESS
def choose_drib_speed(self):
# find ratio: -1 is spinning on spot, 1 is going straight
ratio = 0
if (self.left_motor == 0 or self.right_motor == 0):
ratio = 0
else:
ratio = min(self.left_motor, self.right_motor) / max(
self.left_motor, self.right_motor)
# convert ratio to: 0 is straight, 2 is spinning on spot
ratio = -ratio + 1
# if motor is going forward (ratio = 0), dribbler is at 50 pwm
# if motor is doing a full spin (ratio = 2), dribbler is at 90 pwm
speed = 60 + (ratio * 100)
return check_speed(speed)
# input: object heading/range
# output: drive and center towards object, with speed slowing down based on proximity
# example: object at 100cm, -16deg:
# left_motor: 50 + 100/8 - -16 = 78.5
# right_motor: 50 + 100/8 + -16 = 46.5
def goto_object_simple(self, speed, ran, head):
self.right_motor = check_speed((MIN_SPEED * speed) + (ran / 5) -
(head))
self.left_motor = check_speed((MIN_SPEED * speed) + (ran / 5) + (head))
self.drive_sys.drive(self.left_motor, self.right_motor)
# input: object target
# output: drive towards object and avoid obstacles
prev_heading = -999
def goto_object(self, speed=1, object='ball'):
'''
def smooth(heading):
if self.prev_heading == -999:
self.prev_heading = heading
else:
if abs(self.prev_heading - heading) > 5:
self.prev_heading -= np.sign(self.prev_heading)*5
else:
self.prev_heading = heading
return self.prev_heading'''
if (self.obs_range[0] is not None
and self.obs_range[0] <= self.ball_range):
self.field.change_state(self.state)
target_heading = self.field.update(
self.obs_range, self.obs_heading, self.ball_range,
self.ball_heading, self.goal_range, self.goal_heading)
if object == 'ball':
target_range = self.ball_range
elif object == 'goal':
target_range = self.goal_range
else:
return
else:
if object == 'ball':
target_range = self.ball_range
target_heading = self.ball_heading
elif object == 'goal':
target_range = self.goal_range
target_heading = self.goal_heading
self.goto_object_simple(speed, target_range, target_heading)
def stop(self):
self.drive_sys.stop()
self.dribbler.stop()
GPIO.cleanup()
####################
# UPDATE FUNCTION
#
# input: vision feed values
# output: true if goal needs to be switched
def update(self, obs, ball, goal):
# return true if goal needs to be switched, so vision can be changed from main function
if (self.switch_goal):
self.switch_goal = False
return True
# VISION INFO
self.obs_range = [
cap_ran(obs[0][0]),
cap_ran(obs[1][0]),
cap_ran(obs[2][0])
]
self.obs_heading = [obs[0][1], obs[1][1], obs[2][1]]
self.ball_range = cap_ran(ball[0])
self.ball_heading = ball[1]
self.goal_range = cap_ran(goal[0])
self.goal_heading = goal[1]
# ACT BASED ON VISION INFO
self.decide_conditions()
self.decide_state()
self.decide_action()
return False
##################
# DECIDE FUNCTIONS
# decides what to do based on the data from update()
# input: vision data
# output: condition bool variables
dribbler_counter = 0
def decide_conditions(self):
# if ball is seen
if (self.ball_range is not None):
self.ball_found = True
self.prev_ball = np.sign(self.ball_heading)
# if ball is within dribbler range/angle
if (self.ball_range < BALL_IN_DRIBBLER_DIST and within_angle(
self.ball_heading, BALL_IN_DRIBBLER_ANGLE)):
self.ball_in_dribbler = True
else:
self.ball_in_dribbler = False
else:
self.ball_found = False
# if goal is seen
if (self.goal_range is not None):
self.goal_found = True
self.prev_goal = np.sign(self.goal_heading)
#if goal is within shooting range/angle
if (self.goal_range < GOAL_DIST
and within_angle(self.goal_heading, GOAL_ANGLE)):
self.goal_aligned = True
else:
self.goal_aligned = False
else:
self.goal_found = False
def change_state(self, state):
self.state = state
# input: condition variables
# output: state value
def decide_state(self):
if (not self.ball_found and not self.ball_in_dribbler):
self.state = state.ball_search
elif (self.ball_found and not self.ball_in_dribbler):
self.state = state.go_to_ball
elif (self.ball_found and self.ball_in_dribbler
and not self.goal_found):
self.state = state.goal_search
if (self.prev_state != self.state):
self.drive_sys.stop()
time.sleep(0.25)
elif (self.ball_found and self.ball_in_dribbler and self.goal_found
and not self.goal_aligned):
self.state = state.go_to_goal
elif (self.ball_found and self.ball_in_dribbler and self.goal_found
and self.goal_aligned):
self.state = state.shoot_ball
else:
self.state = state.ball_search
self.prev_state = self.state
# print(self.state)
# input: state/search value
# output: appropriate function
def decide_action(self):
if (self.state == state.ball_search):
self.ball_search()
else:
self.search_lock = False
if (self.state == state.go_to_ball):
self.go_to_ball()
if (self.state == state.goal_search):
self.goal_search()
if (self.state == state.go_to_goal):
self.go_to_goal()
if (self.state == state.shoot_ball):
self.shoot_ball()
# if state == none
else:
pass
##################
# ACTION FUNCTIONS
#
# input: search value
# output: appropriate ball searching method
search_lock = False
def ball_search(self):
if (not self.search_lock):
if (self.search == search.none):
self.search = search.spin
if (self.search == search.spin):
# will not be true until until spin is complete
#print("searching for ball")
if (self.drive_sys.spin(speed=30,
direction=self.prev_ball,
radius=0,
cycles=10)):
self.search = search.go_to_obstacle
if (self.search == search.go_to_obstacle):
self.search_lock = True
if (self.obs_range[0] is None):
#print("searching for obstacle")
self.drive_sys.spin(speed=30,
direction=self.prev_ball,
radius=0,
cycles=None)
else:
if (self.obs_range[0] > OBSTACLE_DIST):
#print("goi8ng to obstacle")
self.goto_object_simple(1, self.obs_range[0],
self.obs_heading[0])
else:
self.search = search.circle_obstacle
if (self.search == search.circle_obstacle):
#print("circling obstacle")
# will not be true until circle is complete
if (self.circle_obstacle(adjust=100)):
#print("CIRCLE OBSTACLE COMPLETE")
# if after all that the ball is not found, restart the cycle again
self.search = search.none
self.search_lock = False
orientate = True
spin_out_timer = 0
right_wheel = 40
left_wheel = 30
prev_state = 0
adjust_counter = 0
# input: largest obstacle range/heading
# output: circling obstacle
def circle_obstacle(self, adjust=10):
# count how many times the robot has to adjust based on if it saw the object
if (self.obs_range[0] is None and self.prev_state is not None):
self.adjust_counter += 1
self.prev_state = self.obs_range[0]
# if it has reached the limit, reset everything for the next time the function is called
if (self.adjust_counter >= adjust):
self.orientate = True
self.spin_out_timer = 0
self.right_wheel = 40
self.left_wheel = 30
self.prev_state = 0
self.adjust_counter = 0
self.drive_sys.stop()
return True
# if the robot needs to oreintate itself
if (self.orientate and self.obs_range[0] is not None):
self.drive_sys.drive(-30, 30)
self.spin_out_timer = 0
# else if it needs to circle
else:
self.orientate = False
if self.obs_range[0] is not None:
self.spin_out_timer = 0
self.right_wheel = 40
self.left_wheel = 30
if (self.obs_range[0] is None):
self.spin_out_timer += 1
self.right_wheel += 0.4
self.left_wheel -= 0.4
# spin in
if (self.spin_out_timer >= 10):
self.drive_sys.drive(self.right_wheel, self.left_wheel)
# spin out
if (self.spin_out_timer < 10):
self.drive_sys.drive(-40, 40)
return False
# input: ball range/heading, ball_in_dribbler variable
# output: driving to ball, turning dribbler on if in range
# WILL DRIVE STRAIGHT TOWARDS BALL, IGNORING OPBSTACLES
def go_to_ball(self):
# self.goto_object_simple(1, self.ball_range, self.ball_heading)
# REPLACE ^ WITH:
self.goto_object(speed=1, object='ball')
if (self.ball_range < 20):
self.dribbler.start(95)
else:
self.dribbler.stop()
# input:
# output: turning in a circle without losing ball
def goal_search(self):
self.dribbler.start(90)
self.drive_sys.spin(speed=20,
direction=self.prev_goal,
radius=10,
cycles=None)
def go_to_goal(self):
self.dribbler.start(95)
self.goto_object_simple(0.65, self.goal_range, self.goal_heading)
# REPLACE ^ WITH:
# self.goto_object(speed=0.65, object='goal')
def shoot_ball(self):
self.dribbler.stop()
self.kicker.kick()
self.drive_sys.stop()
self.switch_goal = True
class Navigation:
def __init__(self):
self.circle_obstacle = False
self.goto_ball = False
self.ball_captured = False
self.goto_obstacle = False
self.goto_goal = False
self.ball_found = False
self.goal_found = False
self.goal_clear = False
self.spin = False
self.search_obs = False
self.timer = Timer()
self.target_side = -1
self.ball_side = -1
self.goal_side = -1
self.wall_side = -1
self.obs_side = -1
self.speed_mod = 1
self.no_target_count = 0
self.circle_count = 0
self.obs_r = None
self.obs_h = None
self.ball_r = None
self.ball_h = None
self.goal_r = None
self.goal_h = None
self.wall_r = None
self.wall_h = None
self.target_r = None
self.target_h = None
self.target_found = False
self.drive_sys = Drive()
self.dribbler = Dribbler()
self.kicker = Kicker()
obs_buffer = [None, None]
obs_buf_count = 0
def update(self, obs, ball, goal, wall):
if obs[0] is None:
self.obs_buf_count += 1
if(self.obs_buf_count > 2):
self.obs_buffer = [None, None]
self.obs_r = None
self.obs_h = None
else:
self.obs_r = cap_range(self.obs_buffer[0])
self.obs_h = self.obs_buffer[1]
else:
self.obs_buf_count = 0
self.obs_buffer = obs
self.ball_r = cap_range(ball[0])
self.ball_h = ball[1]
self.goal_r = cap_range(goal[0])
self.goal_h = goal[1]
self.wall_r = cap_range(wall[0])
self.wall_h = wall[1]
self.conditions()
self.state()
ml, mr, drib, kick = self.decide()
self.drive_sys.drive(ml, mr)
self.dribbler.start(drib)
if kick:
self.kicker.kick()
self.goal_clear = False
return kick
def conditions(self):
if self.ball_r is not None:
self.ball_side = np.sign(self.ball_h)
self.ball_found = True
self.no_target_count = 0
if(within_angle(self.ball_h, MAX_BALL_ANGLE) and self.ball_r < MIN_BALL_DIST):
self.ball_captured = True
else:
self.ball_captured = False
else:
self.ball_found = False
if self.goal_r is not None:
self.goal_side = np.sign(self.goal_h)
self.goal_found = True
if(within_angle(self.goal_h, MAX_GOAL_ANGLE) and self.goal_r < MIN_GOAL_DIST and self.target_clear()):
self.goal_clear = True
else:
self.goal_clear = False
else:
self.goal_found = False
if self.ball_captured:
self.target_r = self.goal_r
self.target_h = self.goal_h
self.target_side = self.goal_side
if self.goal_found:
self.target_found = True
else:
self.target_found = False
else:
self.target_r = self.ball_r
self.target_h = self.ball_h
self.target_side = self.ball_side
if self.ball_found:
self.target_found = True
else:
self.target_found = False
if self.wall_r is not None:
self.wall_side = np.sign(self.wall_h)
if self.obs_h is not None:
self.obs_side = np.sign(self.obs_h)
counter = 0
def state(self):
if not self.target_found:
print(self.counter)
if self.counter < 50 * self.speed_mod:
print("looking for ball")
self.counter += 1
self.spin = True
elif self.counter >= 50 * self.speed_mod:
self.spin = False
# spin until first obs found
if not self.search_obs:
print(self.obs_r)
if(self.obs_r is None):
self.spin = True
print("looking for obs")
else:
self.spin = False
if(self.obs_r > MIN_OBS_DIST):
print("going to obs")
self.goto_obstacle = True
else:
print("spinning to circle obs")
self.goto_obstacle = False
self.spin = True
self.target_side = -self.wall_side
self.search_obs = True
else:
if(self.counter >= 100 * self.speed_mod):
print("circling obs")
self.spin = False
self.counter += 1
self.circle_obstacle = True
if(self.counter >= 150 * self.speed_mod):
print("circle obs done, spin out")
self.counter += 1
self.circle_obstacle = False
self.spin = True
if(self.counter >= 200 * self.speed_mod):
self.counter = 0
print("done")
self.spin = False
self.circle_obstacle = False
self.search_obs = False
# target seen
else:
print("going to target")
self.counter = 0
# if obstacle is too close
if self.obs_r is not None and self.obs_r < 25:
self.spin = True
self.target_side = -self.obs_side
def decide(self):
ml = 0
mr = 0
drib = 0
kick = False
# most complex situations at the top, least complex at the bottom
if self.circle_obstacle and self.target_found:
ml1, mr1 = turn(self.target_side, 45, 90)
ml2, mr2 = self.goto_target()
if ml1 > mr1:
ml, mr = min(ml2, ml1), max(mr2, mr1)
else:
ml, mr = max(ml2, ml1), min(mr2, mr1)
elif self.target_found:
ml, mr = self.goto_target()
elif self.circle_obstacle:
ml, mr = turn(self.target_side, 45, 90)
elif self.goto_obstacle:
ml, mr = rb_to_pwm(self.obs_r, self.obs_h)
elif self.spin:
ml, mr = spin(self.target_side, 40)
# dribbler adn speed adjusts
if self.ball_r is not None and self.ball_r < 25:
drib = 80
if self.ball_captured:
if self.spin:
ml *= 0.5
mr *= 0.5
self.speed_mod = 2
drib = 100
if self.circle_obstacle:
ml *= 0.65
mr *= 0.65
self.speed_mod = 1.5
drib = 85
else:
ml *= 1
mr *= 1
self.speed_mod = 1
drib = 75
else:
drib = 0
self.speed_mod = 1
if self.goal_clear:
drib = 0
kick = True
ml = 0
mr = 0
return ml, mr, drib, kick
'''
def target_clear_many(self):
def clear(obs_h, obs_r, thresh):
if self.target_r - 10 < obs_r:
return True
else:
if (obs_h - thresh <= self.target_h <= obs_h + thresh):
return False
return True
if self.target_h is None:
return False
for i in range(3):
if self.obs_h[i] is None:
return True
if not clear(self.obs_h[i], self.obs_r[i], 16):
return False
return True'''
def target_clear(self):
if self.obs_r is None:
return True
if self.target_r is None:
return False
if self.target_r - 20 < self.obs_r:
return True
else:
if (self.obs_h - 15 <= self.target_h <= self.obs_h + 15):
return False
return True
def target_between(self):
closest_left_h = -60
closest_left_r = 0
closest_right_h = 60
closest_right_r = 0
ret = [None, None], [None, None]
for i in range(3):
if self.obs_h[i] is None:
continue
if closest_left_h < self.obs_h[i] < self.target_h:
closest_left_h = self.obs_h[i]
closest_left_r = self.obs_r[i]
elif closest_right_h > self.obs_h[i] > self.target_h:
closest_right_h = self.obs_h[i]
closest_right_r = self.obs_r[i]
if closest_left_h != -60:
ret[0][:] = [closest_left_r, closest_left_h]
if closest_right_h != 60:
ret[1][:] = [closest_right_r, closest_right_h]
return ret
'''
def goto_target(self):
left_obs, right_obs = self.target_between()
if left_obs[0] is None and right_obs[0] is None:
#print("clear")
return self.goto_target_simple()
if left_obs[0] is not None and right_obs[0] is not None:
#print("between")
# ball inbetween left and right obs
gap = abs(np.sqrt(left_obs[0]**2 + right_obs[0]**2 - 2*left_obs[0]*right_obs[0]*np.cos(left_obs[1] - right_obs[1])))
dist_dif = - left_obs[0] + right_obs[0]
m1l, m1r = self.goto_target_simple()
m2l, m2r = 0, 0
if gap > 30:
# right obs below left obs
if dist_dif <= 0:
m2l, m2r = rb_to_pwm(left_obs[0], left_obs[1] * (10 + abs(dist_dif)) / 10)
else:
m2l, m2r = rb_to_pwm(right_obs[0], right_obs[1] * (10 + abs(dist_dif)) / 10)
else:
if dist_dif <= 0:
m2l, m2r = rb_to_pwm(right_obs[0], right_obs[1] + 16)
else:
m2l, m2r = rb_to_pwm(left_obs[0], left_obs[1] - 16)
ml = (m1l - m2l) / 2
mr = (m1r - m2r) / 2
return ml, mr
if left_obs[0] is None:
# ball to the right of right obs
#print("right")
heading = max((right_obs[1] - (251 - right_obs[0])), self.target_h)
return rb_to_pwm(self.target_r, heading)
if right_obs[0] is None:
print("left")
# ball to the left of left object
heading = max(left_obs[1] - (251 - left_obs[0]), self.target_h)
return rb_to_pwm(self.target_r, heading)
else:
print("uh oh")'''
def goto_target(self):
def avoid(heading, dist):
left = heading - ((251 - dist) * (175 / 250))
right = heading + ((251 - dist) * (175 / 250))
return left, right
if self.obs_r is None or self.obs_r - 10 > self.target_r:
return self.goto_target_simple()
else:
left_avoid, right_avoid = avoid(self.obs_h, self.obs_r)
if self.target_h <= left_avoid or self.target_h >= right_avoid:
return rb_to_pwm(self.target_r, self.target_h)
elif left_avoid < self.target_h <= self.obs_h:
return rb_to_pwm(self.obs_r, left_avoid)
elif right_avoid > self.target_h > self.obs_h:
return rb_to_pwm(self.obs_r, right_avoid)
else:
return 0, 0
def goto_target_simple(self):
return rb_to_pwm(self.target_r, self.target_h)
def stop(self):
self.drive_sys.stop()
self.dribbler.stop()
GPIO.cleanup()
class MyRobot(wpilib.SimpleRobot):
def __init__(self):
super().__init__()
self.loader = Loader()
self.shooter = Shooter()
self.drive = Drive(config.robotDrive, config.leftJoy, config.hsButton,
config.alignButton)
self.componets = [ self.loader, self.shooter, self.drive ]
def RobotInit(self):
wpilib.Wait(0.25)
shooterEncoder.Start()
def Disabled(self):
while wpilib.IsDisabled():
CheckRestart()
wpilib.Wait(0.01)
def Autonomous(self):
while wpilib.IsAutonomous() and wpilib.IsEnabled():
CheckRestart()
wpilib.Wait(0.01)
def OperatorControl(self):
dog = self.GetWatchdog()
dog.SetEnabled(True)
dog.SetExpiration(0.25)
self.drive.stop()
shooterMotor.Set(0)
tipperMotor.Set(0)
rollerMotor.Set(0)
while self.IsOperatorControl() and self.IsEnabled():
dog.Feed()
CheckRestart()
for componet in self.componets:
componet.tick()
#Teleop Code
#if not self.drive.aligning :
# self.drive.arcadeDrive(leftJoy.GetRawButton(halfSpeed))
shooterMotor.Set(rightJoy.GetY())
## Alignment
#if leftJoy.GetRawButton(allignButton) :
# if not self.drive.aligning:
# self.drive.aligning = True
# robotDrive.StopMotor()
# self.drive.align()
#else:
# self.drive.aligning = False
## Tipper Up and Down
if rightJoy.GetRawButton(tipperUp):
tipperMotor.Set(-1)
elif rightJoy.GetRawButton(tipperDown):
tipperMotor.Set(1)
else:
tipperMotor.Set(0)
## Roller Up and Down
if rightJoy.GetRawButton(rollerUpButton):
rollerMotor.Set(-1)
elif rightJoy.GetRawButton(rollerDownButton):
rollerMotor.Set(1)
else:
rollerMotor.Set(0)
## Loading
if rightJoy.GetRawButton(feederButton):
self.loader.load()
## Shooting
if rightJoy.GetRawButton(latchButton):
self.shooter.unlatch()
## Debug & Tuning
wpilib.Wait(0.01)
class Main:
drive_proc = True
detect_boundary = True
detecting = True
detect_cube_found = False
continue_driving = True
def __init__(self, color):
self.random = Random()
self.drive = Drive()
self.grab = Grab()
self.dd = DistanceDetector()
self.cd = ColorDetector(color)
def turn_ninety_degrees(self):
self.drive.turn(Drive.DIRECTION_LEFT, 88)
def detect_outer_boundary(self):
while self.detecting:
while self.detect_boundary:
detect = False
if not self.detect_cube_found:
detect = self.cd.detect_boundary()
if detect:
break
time.sleep(0.01)
self.continue_driving = False
self.drive.stop()
time.sleep(1)
self.drive.reverse(70)
time.sleep(2)
self.drive.turn(Drive.DIRECTION_LEFT, self.random.randint(60, 180))
self.drive.drive_continuous()
self.continue_driving = True
# def drive_process(self):
# self.drive.drive_forward(275)
# time.sleep(1)
# self.turn_ninety_degrees()
# for i in range(250, 0, -20):
# if not self.drive_proc:
# break
# for j in range(1, 4):
# time.sleep(0.5)
# if not self.drive_proc:
# break
# self.drive.drive_forward(i)
# time.sleep(1)
# if not self.drive_proc:
# break
# self.turn_ninety_degrees()
# self.drive.stop()
def proc(self):
self.drive.drive_continuous()
self.dd.detect()
self.detect_cube_found = True
self.drive.stop()
time.sleep(0.5)
self.grab.lift()
time.sleep(1)
detected = False
attempts = 0
max_attempts = 6
while not detected:
detected = self.cd.detect()
if not self.continue_driving:
time.sleep(0.02)
detected = False
if detected:
break
else:
attempts += 1
if attempts >= max_attempts:
self.drive.reverse(20)
self.grab.sink()
time.sleep(2)
self.drive.turn(Drive.DIRECTION_RIGHT, 80)
self.detect_cube_found = False
return
self.drive.turn(Drive.DIRECTION_LEFT, 5)
time.sleep(0.5)
self.drive.reverse(5)
time.sleep(1)
self.grab.sink()
time.sleep(1)
self.drive.turn(Drive.DIRECTION_RIGHT, attempts * 5)
time.sleep(1)
self.drive.drive_continuous()
self.continue_driving = False
self.detect_cube_found = False