class RandomDrive2:
def __init__(self):
self.State = State()
self.State.state(128)
#self.State.state(131)
def full(self):
self.State.state(132)
#def readButton(self):
#return bool(self.State.readState())
#def readButton(self):
#global button
#while True:
#button_state = bool(self.State.readState())
#return button_state
#if(button_state):
#button = not button
def readButton(self):
global button
total_string = self.State.readState()
button_state = bool(total_string)
if (button_state):
button = not button
def readButtonStop(self):
total_string = self.State.readState()
return bool(total_string)
# Right brop is the zeroeth element
def readDrop(self):
total_string = self.State.readDrop()
drops = [bool(total_string[2]), bool(total_string[3])]
return drops
# Tracks drops
def trackDrop(self):
global drop
while True:
drop_state = map(bool, self.readDrop())
if (any(drop_state)):
drop = True
else:
drop = False
# Right bump is the zeroeth element
def readBumps(self):
total_string = self.State.readDrop()
bumps = [bool(total_string[0]), bool(total_string[1])]
return bumps
def readCliffs(self):
total_string = self.State.readCliff()
cliffs = map(bool, total_string)
return cliffs[:-1]
def drive(self):
self.State.driveDirect(170, 170)
def stop(self):
self.State.state(173)
def turn(self, direction):
random_angle = random.randint(-45, 45)
total_angle = math.radians(180 + random_angle)
print(math.degrees(total_angle))
turn_time = total_angle / omega
# turn clockwise
if (direction):
print("Turn clockwise")
self.State.driveDirect(-170, 170)
time.sleep(turn_time)
# turn counterclockwise
if (not direction):
print("Turn counter-clockwise")
self.State.driveDirect(170, -170)
time.sleep(turn_time)
def check(self):
while True:
button_state = self.readButton()
if button:
while True:
button_state = self.readButton()
bump_state = self.readBumps()
cliff_state = self.readCliffs()
print("Button {}".format(button_state))
if (button_state):
print("Button State")
self.stop()
if (drop):
print("SIIIINNNGGG")
self.State.Play(1)
self.stop()
elif (bump_state[0]):
self.turn(False)
elif (bump_state[1]):
self.turn(True)
elif (any(cliff_state)):
print("Cliff State")
self.turn(False)
else:
self.drive()
class RandomDrive3:
# Default constructor - sets it in passive and safe modes, safe is commented out
def __init__(self):
self.State = State()
self.State.state(passive) # passive mode
#self.State.state(safe) # safe mode
def full(self):
self.State.state(full_mode)
# Reads the state of the button and switches the global variable
def readButton(self):
global button
while True:
button_state = bool(self.State.readState())
return button_state
if (button_state):
button = not button
# Reads the wheel drops only, ignores the bumps
# Right drop is the zeroeth element
def readDrop(self):
total_string = self.State.readDrop()
drops = [bool(total_string[2]), bool(total_string[3])]
return drops
# Tracks drops - swtiches the global variable
def trackDrop(self):
global drop
while True:
drop_state = map(bool, self.readDrop())
if (any(drop_state)):
drop = True
else:
drop = False
# Reads the bumps only, ignores the wheel drops
# Right bump is the zeroeth element
def readBumps(self):
total_string = self.State.readDrop()
bumps = [bool(total_string[0]), bool(total_string[1])]
return bumps
# Reads the cliffs, ignores the virtual wall reading
def readCliffs(self):
total_string = self.State.readCliff()
cliffs = map(bool, total_string)
return cliffs[:-1]
# Method to drive direct straight - speed is 170 mm/s
def drive(self):
self.State.driveDirect(positive_velocity, positive_velocity)
# Sets the roomba in stop mode, which terminates all streams
def stop(self):
self.State.state(stop_code)
# Method to turn the roomba, depending on which bumper is hit
def turn(self, direction):
random_angle = random.randint(-45, 45)
total_angle = math.radians(
180 +
random_angle) # turns a random angle between 135 and 225 degress
#print(math.degrees(total_angle))
turn_time = total_angle / omega # calculates the time it takes to turn
# turn clockwise
if (direction):
print("Turn clockwise")
self.State.driveDirect(negative_velocity, positive_velocity)
time.sleep(turn_time)
# turn counterclockwise
if (not direction):
print("Turn counter-clockwise")
self.State.driveDirect(positive_velocity, negative_velocity)
time.sleep(turn_time)
# Method which starts the robot when button is pressed and does turns
def check(self):
while True:
while button: # needs button press to start and pauses when pressed again
bump_state = self.readBumps()
cliff_state = self.readCliffs()
# If the wheel drop - sing and then stop
if (drop):
print("SIIIINNNGGG")
self.State.Play(1)
self.stop()
# These are elifs so that we don't get multiple turns
elif (bump_state[0]): # bumper, so turn
self.turn(False)
elif (bump_state[1]): # bumper, so turn
self.turn(True)
elif (any(cliff_state)): # cliff, so turn
print("Cliff State")
self.turn(False)
else: # if not stopping or turning, drive
self.drive()
class findIR:
def __init__(self):
self.State = State()
self.State.state(128)
self.State.state(131)
def drive(self):
self.State.driveDirect(velocity, velocity)
def Stop(self):
self.State.driveDirect(0,0)
def WallError(self):
wall_reading = self.State.readRightBumper()
error = wall_reading - wall_set
wall_list.append(error)
def WallController(self):
time.sleep(sleep_time)
self.WallError()
if(wall_list[-1] == -1 * wall_set):
return -1 * wall_set
U_p = KP * (wall_list[-1])
U_d = KD * ((wall_list[-1] - wall_list[-2]) / sampling_time)
U = U_p + U_d
if(U < 0):
return -1 * math.sqrt(-1 * U)
return math.sqrt(U)
def WallFollow(self):
state = self.State.readIROmni()
while(state <= 150):
print("IR State in Wall Follow {}".format(state))
U_current = self.WallController()
while(U_current == -400):
self.State.driveDirect(0, 100)
U_current = self.WallController()
self.State.driveDirect(100 + U_current, 100)
state = self.State.readIROmni()
self.whichSide()
def readCenterDock(self):
wall = self.State.readCenterBumper()
print("Wall State {}".format(wall))
if(wall > 240):
return True
return False
def chargingState(self):
charge = roomba.State.isBatteryCharge()
return bool(charge)
def IRerrorCalc(self):
error = self.State.readIROmni() - ir_setpoint
error_list.append(error)
def DockController(self):
time.sleep(sleep_time)
self.IRerrorCalc()
Up = KP * (error_list[-1])
Ud = KD * (error_list[-1] - error_list[-2])
return Up + Ud
def DockDrive(self):
while True:
U_current = self.DockController()
def Dock(self):
while True:
state= self.State.readIROmni()
print("IR State{} ".format(state))
if(self.State.isBatteryCharge() ==2):
self.State.driveDirect(0,0)
elif(state==168):
print("Turn Left")
self.State.driveDirect(90,0)
time.sleep(0.01)
elif(state ==164):
print("Turn Right")
self.State.driveDirect(0,90)
time.sleep(0.01)
elif(state ==172):
while(self.State.readIROmni() ==172):
self.State.driveDirect(100,100)
time.sleep(0.05)
if(self.State.isBatteryCharge()==2):
self.State.driveDirect(0,0)
self.Play()
break
def Play(self):
self.State.state(132)
print("stuff")
self.State.Play(1)
self.State.state(128)
self.State.state(131)
def turn(self):
angle = math.radians(135)
omega = (2 * 170) / 235.0
turn_sleep = (angle / omega)
self.State.driveDirect(-170, 170)
time.sleep(turn_sleep)
def sideDock(self):
self.State.driveDirect(250,200)
time.sleep(2.5)
self.State.driveDirect(0,0)
time.sleep(.5)
self.turn()
self.State.driveDirect(0, 0)
self.Dock()
def whichSide(self):
state = self.State.readIROmni()
print(state)
if(state == 161):
self.sideDock()
else:
self.Dock()
def FollowAndDock(self):
self.WallFollow()
self.whichSide()
