def setup(self):
# Servo object representing the door.
self.openValue = 100 # Value in degrees the servo should be when the door is open.
self.closedValue = 172 # Value in degrees the servo should be when the door is closed.
self.servo = Servo(self.tamp, 25)
self.start()
def setup(self):
self.servo = Servo(self.tamp, self.SERVO_PIN)
self.servo.write(0)
self.servoval = 0
self.delta = 1
self.timer = Timer()
self.end = False
def setup(self):
self.LEFT_TOWER = 85
self.RIGHT_TOWER = 24
self.CENTER = 53
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
self.servo = Servo(self.tamp, 23) #pin TBD
self.red_detected = False
self.green_detected = False
self.servo.write(self.CENTER)
self.counter = 0
self.servo_bottom = Servo(self.tamp,22)
self.servo_bottom.write(34)
self.motor = Motor(self.tamp, 24, 25)
self.motor.write(True, 250)
self.jammed = False
self.conveyor_encoder = Encoder(self.tamp, *self.pins, continuous=True)
self.prev_encoder_value = 0
self.conveyor_encoder.update()
self.timer = Timer()
self.sorter_delta = 50
def setup(self):
# ROS Setup
rospy.init_node('driver')
rospy.Subscriber("driver_commands", String, callback)
# self.tof_pub = rospy.Publisher('tof_readings', String, queue_size=10)
# Device Setup
self.motor1 = Motor(self.tamp, MOTOR1_DIR, MOTOR1_PWM)
self.motor2 = Motor(self.tamp, MOTOR2_DIR, MOTOR2_PWM)
self.encoder1 = Encoder(self.tamp, *ENCODER1, continuous=True)
self.encoder2 = Encoder(self.tamp, *ENCODER2, continuous=True)
# self.encoder1_value = 0
# self.encoder2_value = 0
# TimeOfFlight Sensors
# self.tof1 = TimeOfFlight(self.tamp, TOF1, 1, continuous=False)
# self.tof2 = TimeOfFlight(self.tamp, TOF2, 2, continuous=False)
# self.tof1.enable()
# self.tof2.enable()
# self.tof1_value = 0
# self.tof2_value = 0
# Servo for the Release
self.servo = Servo(self.tamp, SERVO)
self.servo.write(SERVO_CLOSE)
# Controller Params
self.pid = PID(p=P, i=I, d=D)
self.target1 = 0
self.target2 = 0
self.servo_commands = Queue()
class Arm: def __init__(self,tamp): self.tamp=tamp self.servo = Servo(self.tamp, 9) self.servo.bottom = 10 #5 for metal self.servo.top = 150 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.armState="None" self.nextArmState="None" def pickUpBlock(self): self.armState="Up" self.nextArmState="Down" def moveArmUp(self): if self.servoval <= self.servo.top: self.delta = self.servo.speed self.armState="Up" else: self.setPause(.25) self.armState="Pause" #self.armState=self.nextArmState #self.nextArmState="None" def moveArmDown(self): if self.servoval > self.servo.bottom: self.delta = -self.servo.speed self.armState="Down" else: self.armState=self.nextArmState self.nextArmState="None" def setPause(self,pauseLength): self.startPauseTime=time.time() self.pauseLength=pauseLength def pauseArm(self): if(time.time()-self.startPauseTime>self.pauseLength): self.armState=self.nextArmState self.nextArmState="None" self.delta=0 def update(self): if self.armState!="None": if self.armState=="Up": self.moveArmUp() elif self.armState=="Down": self.moveArmDown() elif(self.armState=="Pause"): self.pauseArm() self.servoval += self.delta if(self.servoval<0): self.servoval=0 self.servo.write(abs(self.servoval))
class Arm:
def __init__(self, tamp):
self.tamp = tamp
self.servo = Servo(self.tamp, 9)
self.servo.bottom = 10 #5 for metal
self.servo.top = 150
self.servo.speed = 30
self.servo.write(self.servo.bottom)
self.servoval = self.servo.bottom
self.delta = 0
self.armState = "None"
self.nextArmState = "None"
def pickUpBlock(self):
self.armState = "Up"
self.nextArmState = "Down"
def moveArmUp(self):
if self.servoval <= self.servo.top:
self.delta = self.servo.speed
self.armState = "Up"
else:
self.setPause(.25)
self.armState = "Pause"
#self.armState=self.nextArmState
#self.nextArmState="None"
def moveArmDown(self):
if self.servoval > self.servo.bottom:
self.delta = -self.servo.speed
self.armState = "Down"
else:
self.armState = self.nextArmState
self.nextArmState = "None"
def setPause(self, pauseLength):
self.startPauseTime = time.time()
self.pauseLength = pauseLength
def pauseArm(self):
if (time.time() - self.startPauseTime > self.pauseLength):
self.armState = self.nextArmState
self.nextArmState = "None"
self.delta = 0
def update(self):
if self.armState != "None":
if self.armState == "Up":
self.moveArmUp()
elif self.armState == "Down":
self.moveArmDown()
elif (self.armState == "Pause"):
self.pauseArm()
self.servoval += self.delta
if (self.servoval < 0):
self.servoval = 0
self.servo.write(abs(self.servoval))
def __init__(self, tamp):
self.tamp = tamp
self.servo = Servo(self.tamp, 9)
self.servo.bottom = 5
self.servo.top = 180
self.servo.speed = 30
self.servo.write(self.servo.bottom)
self.servoval = self.servo.bottom
self.delta = 0
self.armState = "None"
self.nextArmState = "None"
def setup(self):
pygame.init()
self.screen = pygame.display.set_mode((300, 300))
self.servo = Servo(self.tamp, self.SERVO_PIN)
self.servo.write(0)
self.servoval = 0
self.delta = 1
self.timer = Timer()
self.end = False
self.wait = False
def __init__(self, tamp, servo_pin, lower, upper, up_traj, down_traj):
"""
lower: servo us corresponding to 0 angle
upper: servo us corresponding to 180 angle
up_traj: list of (angle, dt) tuples for up trajectory
down_traj: list of (angle, dt) tuples for down trajectory
"""
self.servo = Servo(tamp, servo_pin, lower, upper)
self.servo.write(0)
self.up_traj = up_traj
self.down_traj = down_traj
class ServoWrite(Sketch):
"""Cycles a servo back and forth between 1050us and 1950us pulse widths (most servos are 1000-2000)"""
def setup(self):
self.servo = Servo(self.tamp, 10)
self.servo.write(2200)
self.timer = Timer()
self.val = 2200
def loop(self):
raw_input()
self.val += 10
print self.val
self.servo.write(self.val)
class moveDoor(SyncedSketch):
def setup(self):
# Servo object representing the door.
self.openValue = 100 # Value in degrees the servo should be when the door is open.
self.closedValue = 172 # Value in degrees the servo should be when the door is closed.
self.servo = Servo(self.tamp, 25)
self.start()
def loop(self):
# COMMENT 1 OF THESE
#self.servo.write(self.openValue) # Use to open door
self.servo.write(self.closedValue) # Use to close door
class moveDoor(SyncedSketch):
def setup(self):
# Servo object representing the door.
self.openValue = 100 # Value in degrees the servo should be when the door is open.
self.closedValue = 172 # Value in degrees the servo should be when the door is closed.
self.servo = Servo(self.tamp, 25)
self.start()
def loop(self):
# COMMENT 1 OF THESE
#self.servo.write(self.openValue) # Use to open door
self.servo.write(self.closedValue) # Use to close door
def setup(self):
self.LEFT_TOWER = 83.4
self.RIGHT_TOWER = 27
self.CENTER = 54
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
self.servo = Servo(self.tamp, 23) #pin TBD
self.red_detected = False
self.green_detected = False
self.servo.write(self.CENTER)
self.counter = 0
self.timer = Timer()
def __init__(self, tamp):
self.tamp = tamp
self.servo = Servo(self.tamp, 5)
self.servo.center = 90
self.servo.right = 25
self.servo.left = 165
self.servo.speed = 15
self.servo.rightJostle = 60
self.servo.leftJostle = 120
self.servo.write(self.servo.center)
self.sorterval = self.servo.center
self.dsorter = 0
self.sorterState = "None"
self.startPauseTime = 0
self.pauseLength = 0
def setup(self):
# Servo object representing the door.
self.openValue = 100 # Value in degrees the servo should be when the door is open.
self.closedValue = 172 # Value in degrees the servo should be when the door is closed.
self.servo = Servo(self.tamp, 25)
self.start()
def setup(self):
self.servo = Servo(self.tamp, self.SERVO_PIN)
self.servo.write(0)
self.servoval = 0
self.delta = 1
self.timer = Timer()
self.end = False
class Arm:
def __init__(self, tamp):
self.tamp = tamp
self.servo = Servo(self.tamp, 9)
self.servo.bottom = 5
self.servo.top = 180
self.servo.speed = 30
self.servo.write(self.servo.bottom)
self.servoval = self.servo.bottom
self.delta = 0
self.armState = "None"
self.nextArmState = "None"
def pickUpBlock(self):
self.armState = "Up"
self.nextArmState = "Down"
def moveArmUp(self):
if self.servoval <= self.servo.top:
self.delta = self.servo.speed
self.armState = "Up"
else:
self.armState = self.nextArmState
self.nextArmState = "None"
def moveArmDown(self):
if self.servoval > self.servo.bottom:
self.delta = -self.servo.speed
self.armState = "Down"
else:
self.armState = self.nextArmState
self.nextArmState = "None"
def update(self):
if self.armState != "None":
if self.armState == "Up":
self.moveArmUp()
elif self.armState == "Down":
self.moveArmDown()
self.servoval += self.delta
if (self.servoval < 0):
self.servoval = 0
self.servo.write(abs(self.servoval))
class Arm: def __init__(self,tamp): self.tamp=tamp self.servo = Servo(self.tamp, 9) self.servo.bottom = 5 self.servo.top = 180 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.armState="None" self.nextArmState="None" def pickUpBlock(self): self.armState="Up" self.nextArmState="Down" def moveArmUp(self): if self.servoval <= self.servo.top: self.delta = self.servo.speed self.armState="Up" else: self.armState=self.nextArmState self.nextArmState="None" def moveArmDown(self): if self.servoval > self.servo.bottom: self.delta = -self.servo.speed self.armState="Down" else: self.armState=self.nextArmState self.nextArmState="None" def update(self): if self.armState!="None": if self.armState=="Up": self.moveArmUp() elif self.armState=="Down": self.moveArmDown() self.servoval += self.delta if(self.servoval<0): self.servoval=0 self.servo.write(abs(self.servoval))
class ServoWrite(Sketch):
"""Cycles a servo back and forth between 0 and 180 degrees. However,
these degrees are not guaranteed accurate, and each servo's range of valid
microsecond pulses is different"""
SERVO_PIN = 9
def setup(self):
pygame.init()
self.screen = pygame.display.set_mode((300, 300))
self.servo = Servo(self.tamp, self.SERVO_PIN)
self.servo.write(0)
self.servoval = 0
self.delta = 1
self.timer = Timer()
self.end = False
self.wait = False
def loop(self):
for event in pygame.event.get():
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_UP:
self.servoval = 180
if event.key == pygame.K_DOWN:
self.servoval = 0
if event.key == pygame.K_SPACE:
self.servoval = 90
if (self.timer.millis() > 10):
self.timer.reset()
if self.servoval >= 180:
self.delta = -1 # down
elif self.servoval <= 0:
self.delta = 1 # up
elif self.wait == True:
self.delta = 0
# self.servoval += self.delta
print self.servoval
self.servo.write(abs(self.servoval))
def __init__(self,tamp): self.tamp=tamp self.servo = Servo(self.tamp, 9) self.servo.bottom = 5 self.servo.top = 180 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.armState="None" self.nextArmState="None"
class ServoWrite(Sketch):
"""Cycles a servo back and forth between 0 and 180 degrees. However,
these degrees are not guaranteed accurate, and each servo's range of valid
microsecond pulses is different"""
SERVO_PIN = 25
def setup(self):
self.servo = Servo(self.tamp, self.SERVO_PIN)
self.servo.write(0)
self.servoval = 0
self.delta = 1
self.timer = Timer()
self.end = False
def loop(self):
self.servo.write(180)
if self.timer.millis() > 1000:
self.stop()
'''
def setup(self):
pygame.init()
self.screen = pygame.display.set_mode((300, 300))
self.servo = Servo(self.tamp, self.SERVO_PIN)
self.servo.write(0)
self.servoval = 0
self.delta = 1
self.timer = Timer()
self.end = False
self.wait = False
class Arm(HardwareDevice):
"""
A loose wrapper of a servo device, that deals with the up and down
trajectories we need
"""
def __init__(self, tamp, servo_pin, lower, upper, up_traj, down_traj):
"""
lower: servo us corresponding to 0 angle
upper: servo us corresponding to 180 angle
up_traj: list of (angle, dt) tuples for up trajectory
down_traj: list of (angle, dt) tuples for down trajectory
"""
self.servo = Servo(tamp, servo_pin, lower, upper)
self.servo.write(0)
self.up_traj = up_traj
self.down_traj = down_traj
def up(self):
for angle, dt in self.up_traj:
self.servo.write(angle)
time.sleep(dt)
def down(self):
for angle, dt in self.down_traj:
self.servo.write(angle)
time.sleep(dt)
class ServoWrite(Sketch):
"""Cycles a servo back and forth between 0 and 180 degrees. However,
these degrees are not guaranteed accurate, and each servo's range of valid
microsecond pulses is different"""
SERVO_PIN = 9
def setup(self):
pygame.init()
self.screen = pygame.display.set_mode((300, 300))
self.servo = Servo(self.tamp, self.SERVO_PIN)
self.servo.write(0)
self.servoval = 0
self.delta = 1
self.timer = Timer()
self.end = False
self.wait = False
def loop(self):
for event in pygame.event.get():
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_UP:
self.servoval = 180
if event.key == pygame.K_DOWN:
self.servoval = 0
if event.key == pygame.K_SPACE:
self.servoval = 90
if (self.timer.millis() > 10):
self.timer.reset()
if self.servoval >= 180:
self.delta = -1 # down
elif self.servoval <= 0:
self.delta = 1 # up
elif self.wait == True:
self.delta = 0
# self.servoval += self.delta
print self.servoval
self.servo.write(abs(self.servoval))
class ServoWrite(Sketch):
"""Cycles a servo back and forth between 0 and 180 degrees. However,
these degrees are not guaranteed accurate, and each servo's range of valid
microsecond pulses is different"""
SERVO_PIN = 9
def setup(self):
self.servo = Servo(self.tamp, self.SERVO_PIN)
self.servo.write(0)
self.servoval = 0
self.delta = 1
self.timer = Timer()
self.end = False
def loop(self):
if (self.timer.millis() > 10):
self.timer.reset()
if self.servoval >= 180: self.delta = -1
elif self.servoval <= 0: self.delta = 1
self.servoval += self.delta
print self.servoval
self.servo.write(abs(self.servoval))
def setup(self):
self.ttt = Timer()
self.timer = Timer()
self.loopTimer = Timer()
self.servo = Servo(self.tamp, SERVO_PIN)
self.motorRight = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION,
RIGHT_DRIVE_CONTROLLER_PWM)
self.motorLeft = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION,
LEFT_DRIVE_CONTROLLER_PWM)
self.encoder = Encoder(self.tamp, RIGHT_DRIVE_ENCODER_YELLOW,
RIGHT_DRIVE_ENCODER_WHITE)
self.dropoff = DropoffModule(self.timer, self.servo, self.motorRight,
self.motorLeft, self.encoder)
self.dropoff.start()
def __init__(self, tamp): self.tamp=tamp self.servo = Servo(self.tamp, 5) self.servo.center = 90 self.servo.right = 25 self.servo.left = 165 self.servo.speed = 15 self.servo.rightJostle = 60 self.servo.leftJostle = 120 self.servo.write(self.servo.center) self.sorterval = self.servo.center self.dsorter = 0 self.sorterState = "None" self.startPauseTime=0 self.pauseLength=0
class Silo(HardwareDevice):
def __init__(self, tamp):
self.latch = Servo(tamp, pins.stack_latch, 1830, 1130)
self.latch.write(0)
def open(self):
self.latch.write(180)
time.sleep(0.5)
def close(self):
self.latch.write(0)
time.sleep(0.5)
class SortBlocks(SyncedSketch):
def setup(self):
self.LEFT_TOWER = 83.4
self.RIGHT_TOWER = 27
self.CENTER = 54
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
self.servo = Servo(self.tamp, 23) #pin TBD
self.red_detected = False
self.green_detected = False
self.servo.write(self.CENTER)
self.counter = 0
self.timer = Timer()
def loop(self):
if self.timer.millis() > 200:
self.timer.reset()
# print self.color.r, self.color.g, self.color.b, self.color.c
# print self.color.colorTemp, self.color.lux
#if not self.red_detected and not self.green_detected and self.color.r > 1.3 * self.color.g and self.color.r > 1.3 * self.color.b:
detect_red = self.color.r > 1.3*self.color.g
detect_green = self.color.g > 1.3*self.color.r
sum_val = self.color.r+self.color.g+self.color.b
if detect_red and sum_val > 300:
self.servo.write(self.LEFT_TOWER)
self.counter = 0
#elif not self.red_detected and not self.green_detected and self.color.g > 1.3 * self.color.r and self.color.g > 1.3 * self.color.b:
elif detect_green and sum_val > 300:
self.servo.write(self.RIGHT_TOWER)
self.counter = 0
elif self.counter > 400:
self.servo.write(self.CENTER)
self.counter += 100
class ServoWrite(Sketch):
"""Cycles a servo back and forth between 1050us and 1950us pulse widths (most servos are 1000-2000)"""
def setup(self):
self.servo = Servo(self.tamp, 9)
self.servo.write(1050)
self.timer = Timer()
self.end = False
def loop(self):
if (self.timer.millis() > 2000):
self.timer.reset()
if self.end:
self.servo.write(1050)
else:
self.servo.write(1950)
self.end = not self.end
class ServoWrite(Sketch):
"""Cycles a servo back and forth between 1050us and 1950us pulse widths (most servos are 1000-2000)"""
def setup(self):
self.servo = Servo(self.tamp, 9)
self.servo.write(1050)
self.timer = Timer()
self.end = False
def loop(self):
if (self.timer.millis() > 2000):
self.timer.reset()
if self.end:
self.servo.write(1050)
else:
self.servo.write(1950)
self.end = not self.end
def setup(self):
# initialize sensors, settings, start timers, etc.
self.gameTimer = Timer()
self.motorGripper = Motor(self.tamp, 23, 3)
self.motorLeft = Motor(self.tamp, 7, 22)
self.motorRight = Motor(self.tamp, 0, 21)
self.motorval = 0
self.motorLeft.write(1,0)
self.motorRight.write(1,0)
self.motorGripper.write(1,0)
self.currentGripperLevel = 2
print "Motors connected."
self.servoDoor = Servo(self.tamp, 20)
self.servovalDoor = self.DOOR_CLOSE_POS
self.servoDoor.write(self.DOOR_CLOSE_POS)
self.timerDoor = Timer()
self.servoGripper = Servo(self.tamp, 10)
self.servovalGripper = self.GRIPPER_CLOSE_POS
self.servoGripper.write(self.servovalGripper)
self.timerGripper = Timer()
print "Servos connected."
self.timer = Timer()
self.gyro = Gyro(self.tamp, 9)
print "Gyro connected."
self.theta = self.gyro.val
self.dT = .01
self.cam = cv2.VideoCapture(0)
print "Camera connected."
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
frontLeftIR_pin = 14
self.frontLeftIR = AnalogInput(self.tamp, frontLeftIR_pin)
frontRightIR_pin = 15
self.frontRightIR = AnalogInput(self.tamp, frontRightIR_pin)
leftIR_pin = 16
self.leftIR = AnalogInput(self.tamp, leftIR_pin)
rightIR_pin = 17
self.rightIR = AnalogInput(self.tamp, rightIR_pin)
# Initialize PID Values
self.P = 10
self.I = 0 #5
self.D = 0
self.last_diff = 0
self.integral = 0
self.desiredAngle = self.theta
self.finishedCollectingBlock = False
self.finishedDiscardingBlock = False
self.timer = Timer()
self.state = ExploreState()
# self.state = CollectBlockState()
self.blocksCollected = 0
self.leftIRVals = deque([])
self.rightIRVals = deque([])
self.frontRightIRVals = deque([])
self.frontLeftIRVals = deque([])
# Starts the robot
print "Robot setup complete."
def setup(self):
# initialize sensors, settings, start timers, etc.
self.gameTimer = Timer()
# Pins (7,22); (3,23); (0,21) work.
# Problem was with the Double Motor controller.
self.motorGripper = Motor(self.tamp, 23, 3)
self.motorLeft = Motor(self.tamp, 7, 22)
self.motorRight = Motor(self.tamp, 0, 21) # good
self.motorval = 0
self.motorLeft.write(1,0)
self.motorRight.write(1,0)
self.motorGripper.write(1,0)
self.currentGripperLevel = 2
print "Motors connected."
self.servoDoor = Servo(self.tamp, 20)
self.servovalDoor = self.DOOR_CLOSE_POS
self.servoDoor.write(self.DOOR_CLOSE_POS)
self.timerDoor = Timer()
self.servoGripper = Servo(self.tamp, 10)
self.servovalGripper = self.GRIPPER_CLOSE_POS
self.servoGripper.write(self.servovalGripper)
self.timerGripper = Timer()
print "Servos connected."
left_pins = 6,5
right_pins = 3,4
# Encoder doesn't work when after gyro
self.encoderLeft = Encoder(self.tamp, 6,5, continuous=False)
self.encoderRight = Encoder(self.tamp, 3,4, continuous=True)
print "Encoders connected."
# TODO: set encoder to 0
self.timer = Timer()
self.gyro = Gyro(self.tamp, 9)
print "Gyro connected."
self.theta = self.gyro.val
self.dT = .01
self.cam = cv2.VideoCapture(0)
print "Camera connected."
# self.color = Color(self.tamp,
# integrationTime=Color.INTEGRATION_TIME_101MS,
# gain=Color.GAIN_1X)
self.encoderLeft.start_continuous()
self.encoderRight.start_continuous()
frontLeftIR_pin = 14
self.frontLeftIR = AnalogInput(self.tamp, frontLeftIR_pin)
frontRightIR_pin = 15
self.frontRightIR = AnalogInput(self.tamp, frontRightIR_pin)
leftIR_pin = 16
self.leftIR = AnalogInput(self.tamp, leftIR_pin)
rightIR_pin = 17
self.rightIR = AnalogInput(self.tamp, rightIR_pin)
# Initialize PID Values
self.P = 10
self.I = 5
self.D = 5
self.last_diff = 0
self.integral = 0
self.desiredAngle = self.theta
self.finishedCollectingBlock = False
self.finishedDiscardingBlock = False
self.timer = Timer()
self.state = ExploreState()
# self.state = CollectBlockState()
self.blocksCollected = 0
self.leftIRVals = deque([])
self.rightIRVals = deque([])
self.frontRightIRVals = deque([])
self.frontLeftIRVals = deque([])
self.goIntoTimeoutState = False
self.timeoutCounter = 0
# Starts the robot
print "Robot setup complete."
class SortBlocks(SyncedSketch):
pins = 31, 32
def setup(self):
self.LEFT_TOWER = 85
self.RIGHT_TOWER = 24
self.CENTER = 53
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
self.servo = Servo(self.tamp, 23) #pin TBD
self.red_detected = False
self.green_detected = False
self.servo.write(self.CENTER)
self.counter = 0
self.servo_bottom = Servo(self.tamp,22)
self.servo_bottom.write(34)
self.motor = Motor(self.tamp, 24, 25)
self.motor.write(True, 250)
self.jammed = False
self.conveyor_encoder = Encoder(self.tamp, *self.pins, continuous=True)
self.prev_encoder_value = 0
self.conveyor_encoder.update()
self.timer = Timer()
self.sorter_delta = 50
def loop(self):
if self.timer.millis() > 300:
self.timer.reset()
#self.conveyor_encoder.update()
encoder_value = self.conveyor_encoder.val
#print self.prev_encoder_value
#print encoder_value
if(encoder_value == self.prev_encoder_value):
self.jammed = True
else:
self.jammed = False
self.prev_encoder_value = encoder_value
self.motor.write(not self.jammed,250)
# print self.color.r, self.color.g, self.color.b, self.color.c
# print self.color.colorTemp, self.color.lux
#if not self.red_detected and not self.green_detected and self.color.r > 1.3 * self.color.g and self.color.r > 1.3 * self.color.b:
detect_red = self.color.r > 1.3*self.color.g
detect_green = self.color.g > 1.3*self.color.r
sum_val = self.color.r+self.color.g+self.color.b
if detect_red and sum_val > 300:
self.servo.write(self.LEFT_TOWER)
self.counter = 0
#elif not self.red_detected and not self.green_detected and self.color.g > 1.3 * self.color.r and self.color.g > 1.3 * self.color.b:
elif detect_green and sum_val > 300:
self.servo.write(self.RIGHT_TOWER)
self.counter = 0
elif self.counter > 400:
self.servo.write(self.CENTER)
# else:
# self.servo.write(self.CENTER + self.sorter_delta)
# self.sorter_delta *= -1
self.counter += 100
class Robot(SyncedSketch):
def setup(self):
####################
#### EE SETUP ####
####################
# Motor object representing the left motor.
self.leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION, LEFT_DRIVE_CONTROLLER_PWM)
# Encoder object for the left motor.
self.leftEncoder = Encoder(self.tamp, LEFT_DRIVE_ENCODER_YELLOW, LEFT_DRIVE_ENCODER_WHITE)
# Motor object representing the right motor.
self.rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION, RIGHT_DRIVE_CONTROLLER_PWM)
# Encoder object for the right motor.
self.rightEncoder = Encoder(self.tamp, RIGHT_DRIVE_ENCODER_YELLOW, RIGHT_DRIVE_ENCODER_WHITE)
# Motor object representing the intake mechanism motors.
self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION, HUGS_MOTOR_CONTROLLER_PWM)
# Encoder object for the intake motor.
self.intakeEncoder = Encoder(self.tamp, HUGS_MOTOR_ENCODER_YELLOW, HUGS_MOTOR_ENCODER_WHITE)
# Motor object representing the conveyor belt motor.
self.conveyorMotor = Motor(self.tamp, BELT_MOTOR_CONTROLLER_DIRECTION, BELT_MOTOR_CONTROLLER_PWM)
# Encoder object for the conveyor belt motor.
self.conveyorEncoder = Encoder(self.tamp, BELT_MOTOR_ENCODER_YELLOW, BELT_MOTOR_ENCODER_WHITE)
# Long range IR sensors
self.irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL)
self.irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR)
self.irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL)
self.irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR)
# Color sensor
self.color = Color(self.tamp)
# Limit switches
self.conveyorLimSwitch = DigitalInput(self.tamp, CONVEYOR_LIMIT_SWITCH)
self.blockLimSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH)
# Servo controlling the door of the collection chamber.
self.backDoorServo = Servo(self.tamp, SERVO_PIN)
# Make sure the door is closed
self.backDoorServo.write(SERVO_CLOSE)
# The switch that tells the program that the competition has started
self.competitionModeSwitch = DigitalInput(self.tamp, COMPETITION_MODE)
#################################
#### INTERNAL MODULE SETUP ####
#################################
# Timer object to moderate checking for intake errors.
self.intakeTimer = Timer()
# Are the intake motors reversing? True if so, False if going forwards.
self.intakeDirection = False
# Start the intake motor.
self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
# Wall Follow object
self.wallFollow = WallFollow(self.leftMotor, self.rightMotor, Timer(), self.irFL, self.irFR, self.irBL, self.irBR)
# A timer to make sure timesteps are only 10 times/second
self.timestepTimer = Timer()
# Timer object describing how long the current module has been running.
self.moduleTimer = Timer()
# Timer for the whole game
self.gameTimer = Timer()
# Runs the PICKUP process
self.pickup = PickupModule(self.moduleTimer, self.conveyorLimSwitch, self.conveyorMotor, self.conveyorEncoder)
# Runs the DROPOFF process
self.dropoff = DropoffModule(self.moduleTimer, self.backDoorServo, self.rightMotor, self.leftMotor, self.rightEncoder)
# Runs the FOLLOW process TODO: Fix forward to actually mean forward.
self.follow = FollowModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.wallFollow, FORWARD_SPEED, self.blockLimSwitch)
# Runs the CHECK process. TODO: pass in proper timers.
self.check = CheckModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.color)
# Waits for the game to start
self.off = OffModule(self.gameTimer, self.competitionModeSwitch)
# Describes which stage of the program is running.
self.module = MODULE_OFF
def loop(self):
if self.timestepTimer.millis() > 100:
self.timestepTimer.reset()
#print "Module Number", self.module
state = -1
if self.module == MODULE_OFF:
state = self.off.run()
elif self.module == MODULE_CHECK:
state = self.check.run()
elif self.module == MODULE_PICKUP:
state = self.pickup.run()
elif self.module == MODULE_DROPOFF:
self.module = MODULE_FOLLOW
state = self.follow.run()
elif self.module == MODULE_FOLLOW:
state = self.follow.run()
else:
print "Attempting to run nonexistent module"
self.stop()
self.updateState(state)
if self.gameTimer.millis() % 10000 == 0:
print "Game Time: ", self.gameTimer.millis()
if self.gameTimer.millis() > GAME_LENGTH - 500:
self.backDoorServo.write(SERVO_OPEN)
self.leftMotor.write(0,0)
self.rightMotor.write(0,0)
if self.gameTimer.millis() > GAME_LENGTH:
self.module = MODULE_END
self.stop()
return
## Switch module if necessary.
def updateState(self, module):
if self.module == module:
return
elif module == MODULE_OFF:
self.off.start()
self.module = MODULE_OFF
return
elif module == MODULE_CHECK:
self.check.start()
self.module = MODULE_CHECK
return
elif module == MODULE_PICKUP:
self.pickup.start()
self.module = MODULE_PICKUP
return
elif module == MODULE_DROPOFF:
self.dropoff.start()
self.module = MODULE_DROPOFF
return
elif module == MODULE_FOLLOW:
self.follow.start()
self.module = MODULE_FOLLOW
return
else:
print "Attempting to switch to nonexistent module"
self.stop()
## Make sure that the intake motor does not stall.
# If so, reverse the intake motors.
#
# @param checkTime Time in ms between checking stalls.
# @param reverseTime Time in ms that the intake motors will reverse if needed.
def checkForIntakeErrors(self, checkTime = 100, reverseTime = 800):
if self.intakeDirection: # We are moving forward.
if self.intakeTimer.millis() > checkTime:
self.intakeTimer.reset()
if self.intakeEncoder.val < INTAKE_ENCODER_LIMIT: # if we're stalled
self.intakeDirection = True
self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
else: # if we're not stalled
self.intakeEncoder.write(0)
else: # We are reversing the motors.
if self.intakeTimer.millis() > reverseTime:
self.intakeTimer.reset()
self.intakeDirection = False
self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
self.intakeEncoder.write(0)
self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
class PIDDrive(SyncedSketch):
ss_pin = 10 #gyro select pin
# image_pipe = './image'
# if not os.path.exists(image_pipe):
# os.mkfifo(image_pipe)
# image_fd = os.open(image_pipe, os.O_RDONLY)
def setup(self):
self.TicpIn = 4480/2.875
self.ir_array = Ir_array(self.tamp, 16, 15, 14, 40, 11, 10)
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
self.uIR = DigitalInput(self.tamp, 17)
self.uIR.val = 1
self.servo = Servo(self.tamp, 9)
self.servo.bottom = 0
self.servo.top = 200
self.servo.speed = 30
self.servo.write(self.servo.bottom)
self.servoval = self.servo.bottom
self.delta = 0
self.sorter = Servo(self.tamp, 5)
self.sorter.center = 90
self.sorter.right = 25
self.sorter.left = 165
self.sorter.speed = 15
self.sorter.write(self.sorter.center)
self.sorterval = self.sorter.center
self.dsorter = 0
self.encoderL = Encoder(self.tamp, 22, 23)
self.encoderR = Encoder(self.tamp, 21, 20)
self.init_time = time.time()
# self.encoderL = Encoder(self.tamp, 23, 22)
# self.encoderR = Encoder(self.tamp, 21, 20)
#motor left
self.motorL = Motor(self.tamp, 2, 4)
self.motorLdrive = 0
self.motorL.write(1,0)
#self.deltaL = 1
#self.motorvalL = 0
#motor right
self.motorR = Motor(self.tamp,1, 3)
self.motorRdrive = 0
self.motorR.write(1,0)
#self.deltaR = 1
#self.motorvalR = 0
#gyro
self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
self.initAngle = self.gyro.val
self.newAngle = 0
self.blockAngle = 0
self.blockDistance = 0
# print "initial angle:"+str(self.initAngle)
self.Follow = 'Right'
self.IRs = {'Left': [0, 1, 2], 'Right': [5, 4, 3]}
self.prevGyro = 0
self.drift = -.02875
self.totalDrift = 0
self.drifts = []
self.PID=PID(.5, 1, 0.15)
self.IRPID = PID(10, 5, .15)
self.fwdVel = 30
self.turnVel = 0
self.MoveArm, self.Top, self.Bottom = False, 0, 0
self.Sort = 0
self.SortPause = 0
self.State = 1
self.timer = Timer()
def loop(self):
#state 0 - wait for first no block, do nothing
#transition: no block -> state 1
#state 1 - look for block
#transition: found block -> state 2
#sate 2 - drive over block
#transition: ir triggered -> state 3
#sate 3 - pick up block
#transition: color sensor done -> sate 1
if self.timer.millis() > 100:
# print 'Color'
# print self.color.c
# print self.color.r, self.color.g
if self.color.c > 800 and self.Sort == 0:
if self.color.r > self.color.g:
self.Sort = 1
else:
self.Sort = 2
# print self.uIR.val
if self.uIR.val == 0:
# self.MoveArm = True
self.State = 0
if self.MoveArm:
if self.Bottom == 2 and self.Top == 1:
self.delta = 0
self.servoval = self.servo.bottom
self.servo.write(self.servo.bottom)
self.Bottom, self.Top = 0, 0
self.MoveArm = False
elif self.servoval >= self.servo.top:
time.sleep(1)
self.delta = -self.servo.speed
self.Top = 1
elif self.servoval <= self.servo.bottom:
self.delta = self.servo.speed
self.Bottom = 1
if self.Top == 1:
self.Bottom = 2
if self.Sort == 1:
if self.sorterval < self.sorter.left:
self.dsorter = self.sorter.speed
elif self.sorterval >= self.sorter.left:
self.dsorter = 0
self.Sort = 3
self.SortPause = time.time()
if self.Sort == 2:
if self.sorterval > self.sorter.right:
self.dsorter = -self.sorter.speed
elif self.sorterval <= self.sorter.right:
self.dsorter = 0
self.Sort = 3
self.SortPause = time.time()
if self.Sort == 3:
if time.time() - self.SortPause > 1:
self.sorterval = self.sorter.center
self.Sort = 0
self.sorterval += self.dsorter
self.sorter.write(abs(self.sorterval))
self.servoval += self.delta
self.servo.write(abs(self.servoval))
self.initAngle += self.turnVel
self.timer.reset()
#response = self.rp.read()
#print "Got response %s" % response
# Valid gyro status is [0,1], see datasheet on ST1:ST0 bits
cAngle=self.gyro.val - self.totalDrift #corrected angle
# print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift
self.prevGyro=self.gyro.val
self.totalDrift+=self.drift
# message = os.read(self.image_fd, 20)
# if ParseMessage(message):
# self.blockDistance, self.blockAngle = ParseMessage(message)
# print 'Angle Dif: ' + str(cAngle-self.initAngle) + '\tPID RESULT: '+ str(pidResult)
# print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str(self.encoderL.val)
# print 'AVG: ' + str((self.encoderR.val + self.encoderL.val)/2.)
self.ir_array.update()
ir = self.ir_array.ir_value
ir90 = self.IRs[self.Follow][0]
ir30 = self.IRs[self.Follow][1]
avg = (ir[self.IRs[self.Follow][0]]+ir[self.IRs[self.Follow][1]]/2)/2
min_val = min(ir[self.IRs[self.Follow][0]], ir[self.IRs[self.Follow][1]])
if avg != float('inf'):
pidResult= -self.IRPID.valuePID(4, avg)
elif min_val != float('inf'):
pidResult= -self.IRPID.valuePID(4, min_val)
else:
pidResult = -20
if ir[self.IRs[self.Follow][2]] < 4:
pidResult = 20
# self.fwdVel = 0
else:
self.fwdVel = 30
pidResult = self.PID.valuePID(self.initAngle, cAngle)
self.motorLdrive = self.fwdVel - pidResult
self.motorRdrive = self.fwdVel + pidResult
print 'Distance Traveled : ' + str(self.getDistanceTraveled())
self.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive))
self.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive))
'''
print "ok"
#response = self.rp.read()
#print "Got response %s" % response
'''
def ParseMessage(message):
if message:
if message[:2] == 'no':
blockDistance, blockAngle = 0, 0
return None
else:
try:
blockDistance, blockAngle = [number[:6] for number in message.split(',')]
except:
blockDistance, blockAngle = 0, 0
return blockDistance, blockAngle
else:
return None
def getDistanceTraveled(self):
"""returns Distance traveled in inches.
Call resetEncoders, drive forward until you've reached your destination"""
print 'Left: ' + str(self.encoderL.val)
print 'Right: ' + str(self.encoderR.val)
avg = (self.encoderL.val + self.encoderR.val)/2
return avg/360.
def setup(self):
####################
#### EE SETUP ####
####################
# Motor object representing the left motor.
self.leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION,
LEFT_DRIVE_CONTROLLER_PWM)
# Encoder object for the left motor.
self.leftEncoder = Encoder(self.tamp, LEFT_DRIVE_ENCODER_YELLOW,
LEFT_DRIVE_ENCODER_WHITE)
# Motor object representing the right motor.
self.rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION,
RIGHT_DRIVE_CONTROLLER_PWM)
# Encoder object for the right motor.
self.rightEncoder = Encoder(self.tamp, RIGHT_DRIVE_ENCODER_YELLOW,
RIGHT_DRIVE_ENCODER_WHITE)
# Motor object representing the intake mechanism motors.
self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION,
HUGS_MOTOR_CONTROLLER_PWM)
# Encoder object for the intake motor.
self.intakeEncoder = Encoder(self.tamp, HUGS_MOTOR_ENCODER_YELLOW,
HUGS_MOTOR_ENCODER_WHITE)
# Motor object representing the conveyor belt motor.
self.conveyorMotor = Motor(self.tamp, BELT_MOTOR_CONTROLLER_DIRECTION,
BELT_MOTOR_CONTROLLER_PWM)
# Encoder object for the conveyor belt motor.
self.conveyorEncoder = Encoder(self.tamp, BELT_MOTOR_ENCODER_YELLOW,
BELT_MOTOR_ENCODER_WHITE)
# Long range IR sensors
self.irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL)
self.irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR)
self.irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL)
self.irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR)
# Color sensor
self.color = Color(self.tamp)
# Limit switches
self.conveyorLimSwitch = DigitalInput(self.tamp, CONVEYOR_LIMIT_SWITCH)
self.blockLimSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH)
# Servo controlling the door of the collection chamber.
self.backDoorServo = Servo(self.tamp, SERVO_PIN)
# Make sure the door is closed
self.backDoorServo.write(SERVO_CLOSE)
# The switch that tells the program that the competition has started
self.competitionModeSwitch = DigitalInput(self.tamp, COMPETITION_MODE)
#################################
#### INTERNAL MODULE SETUP ####
#################################
# Timer object to moderate checking for intake errors.
self.intakeTimer = Timer()
# Are the intake motors reversing? True if so, False if going forwards.
self.intakeDirection = False
# Start the intake motor.
self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
# Wall Follow object
self.wallFollow = WallFollow(self.leftMotor, self.rightMotor, Timer(),
self.irFL, self.irFR, self.irBL,
self.irBR)
# A timer to make sure timesteps are only 10 times/second
self.timestepTimer = Timer()
# Timer object describing how long the current module has been running.
self.moduleTimer = Timer()
# Timer for the whole game
self.gameTimer = Timer()
# Runs the PICKUP process
self.pickup = PickupModule(self.moduleTimer, self.conveyorLimSwitch,
self.conveyorMotor, self.conveyorEncoder)
# Runs the DROPOFF process
self.dropoff = DropoffModule(self.moduleTimer, self.backDoorServo,
self.rightMotor, self.leftMotor,
self.rightEncoder)
# Runs the FOLLOW process TODO: Fix forward to actually mean forward.
self.follow = FollowModule(self.moduleTimer, self.leftMotor,
self.rightMotor, self.intakeMotor,
self.wallFollow, FORWARD_SPEED,
self.blockLimSwitch)
# Runs the CHECK process. TODO: pass in proper timers.
self.check = CheckModule(self.moduleTimer, self.leftMotor,
self.rightMotor, self.intakeMotor, self.color)
# Waits for the game to start
self.off = OffModule(self.gameTimer, self.competitionModeSwitch)
# Describes which stage of the program is running.
self.module = MODULE_OFF
def setup(self):
#Pygame stuff
pygame.init()
self.screen = pygame.display.set_mode((300, 300))
self.TicpIn = 4480/2.875
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
self.uIR = DigitalInput(self.tamp, 17)
self.uIR.val = 1
self.servo = Servo(self.tamp, 9)
self.servo.bottom = 0
self.servo.top = 200
self.servo.speed = 30
self.servo.write(self.servo.bottom)
self.servoval = self.servo.bottom
self.delta = 0
self.sorter = Servo(self.tamp, 5)
self.sorter.center = 90
self.sorter.right = 25
self.sorter.left = 165
self.sorter.speed = 15
self.sorter.write(self.sorter.center)
self.sorterval = self.sorter.center
self.dsorter = 0
self.encoderL = Encoder(self.tamp, 22, 23)
self.encoderR = Encoder(self.tamp, 21, 20)
self.init_time = time.time()
# self.encoderL = Encoder(self.tamp, 23, 22)
# self.encoderR = Encoder(self.tamp, 21, 20)
#motor left
self.motorL = Motor(self.tamp, 2, 4)
self.motorLdrive = 0
self.motorL.write(1,0)
#self.deltaL = 1
#self.motorvalL = 0
#motor right
self.motorR = Motor(self.tamp,1, 3)
self.motorRdrive = 0
self.motorR.write(1,0)
#self.deltaR = 1
#self.motorvalR = 0
#gyro
self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
self.initAngle = self.gyro.val
self.newAngle = 0
self.blockAngle = 0
self.blockDistance = 0
print "initial angle:"+str(self.initAngle)
self.prevGyro = 0
self.drift = -.02875
self.totalDrift = 0
self.drifts = []
self.PID=PID(.5, 1, 0.15)
self.fwdVel = 0
self.turnVel = 0
self.turn = 0
self.MoveArm, self.Top, self.Bottom = False, 0, 0
self.Sort = 0
self.SortPause = 0
self.State = 1
self.timer = Timer()
def setup(self):
"""One-time method that sets up the robot, like in Arduino"""
##--------------------------------------------##
##--------- DRIVING MOTORS (+encoder) --------##
##--------------------------------------------##
# Create the motor objects
self.lmotor = Motor(self.tamp, *LMOTOR_PINS)
self.rmotor = Motor(self.tamp, *RMOTOR_PINS)
# Create a subscriber to listen for drive motor commands
rospy.Subscriber('drive_cmd', kitware.msg.DriveCMD,
self.drive_callback)
## --------- ##
if ENCODER:
# encoder objects
self.lmotor_encoder = Encoder(self.tamp,
*LMOTOR_ENCODER_PINS,
continuous=True)
self.rmotor_encoder = Encoder(self.tamp,
*RMOTOR_ENCODER_PINS,
continuous=True)
# Publishers for encoder values
self.pub_lmotor_encoder_val = rospy.Publisher('lmotor_encoder_val',
Int64,
queue_size=10)
self.pub_rmotor_encoder_val = rospy.Publisher('rmotor_encoder_val',
Int64,
queue_size=10)
##--------------------------------------------##
##--------------- INTAKE MOTOR ---------------##
##--------------------------------------------##
# Create the motor object
self.intake_motor = Motor(self.tamp, *INTAKE_MOTOR_PINS)
# Create a subscriber to listen for intake motor commands (passively continuous)
rospy.Subscriber('intake_cmd', kitware.msg.DriveCMD,
self.intake_callback)
##--------------------------------------------##
##-------------- DISPENSER SERVO -------------##
##--------------------------------------------##
# Setup servo
self.servo = Servo(self.tamp, SERVO_PIN)
self.servo.write(0)
self.servo_val = 0
self.delta = DISPENSOR_SPEED
self.end = False
# Subscriber to listen for drive motor commands
rospy.Subscriber('dispenser_servo_speed', Int64,
self.dispenser_servo_callback)
# Publisher for current servo value
self.pub_dispenser_servo_val = rospy.Publisher('dispenser_servo_val',
Int64,
queue_size=10)
##--------------------------------------------##
##--------- DISPENSER MOTOR (+encoder) -------##
##--------------------------------------------##
# Create the motor object
self.dispenser_motor = Motor(self.tamp, *DISPENSER_MOTOR_PINS)
# Create a subscriber to listen for dispenser motor commands
rospy.Subscriber('dispenser_cmd', kitware.msg.DriveCMD,
self.dispenser_motor_callback)
## --------- ##
if ENCODER:
# encoder objects
self.dispenser_motor_encoder = Encoder(
self.tamp, *DISPENSER_MOTOR_ENCODER_PINS, continuous=True)
# Publishers for encoder values
self.pub_dispenser_motor_encoder_val = rospy.Publisher(
'dispenser_motor_encoder_val', Int64, queue_size=10)
##--------------------------------------------##
##---------- General ---------##
##--------------------------------------------##
# timer for loop()
self.timer = Timer()
# reset all motor encoders
self.lmotor_encoder.val = 0
self.rmotor_encoder.val = 0
self.dispenser_motor_encoder.val = 0
class SemiBot(ROSSketch):
"""ROS Node that controls the SemiBot via the Teensy and tamproxy"""
def setup(self):
"""One-time method that sets up the robot, like in Arduino"""
##--------------------------------------------##
##--------- DRIVING MOTORS (+encoder) --------##
##--------------------------------------------##
# Create the motor objects
self.lmotor = Motor(self.tamp, *LMOTOR_PINS)
self.rmotor = Motor(self.tamp, *RMOTOR_PINS)
# Create a subscriber to listen for drive motor commands
rospy.Subscriber('drive_cmd', kitware.msg.DriveCMD,
self.drive_callback)
## --------- ##
if ENCODER:
# encoder objects
self.lmotor_encoder = Encoder(self.tamp,
*LMOTOR_ENCODER_PINS,
continuous=True)
self.rmotor_encoder = Encoder(self.tamp,
*RMOTOR_ENCODER_PINS,
continuous=True)
# Publishers for encoder values
self.pub_lmotor_encoder_val = rospy.Publisher('lmotor_encoder_val',
Int64,
queue_size=10)
self.pub_rmotor_encoder_val = rospy.Publisher('rmotor_encoder_val',
Int64,
queue_size=10)
##--------------------------------------------##
##--------------- INTAKE MOTOR ---------------##
##--------------------------------------------##
# Create the motor object
self.intake_motor = Motor(self.tamp, *INTAKE_MOTOR_PINS)
# Create a subscriber to listen for intake motor commands (passively continuous)
rospy.Subscriber('intake_cmd', kitware.msg.DriveCMD,
self.intake_callback)
##--------------------------------------------##
##-------------- DISPENSER SERVO -------------##
##--------------------------------------------##
# Setup servo
self.servo = Servo(self.tamp, SERVO_PIN)
self.servo.write(0)
self.servo_val = 0
self.delta = DISPENSOR_SPEED
self.end = False
# Subscriber to listen for drive motor commands
rospy.Subscriber('dispenser_servo_speed', Int64,
self.dispenser_servo_callback)
# Publisher for current servo value
self.pub_dispenser_servo_val = rospy.Publisher('dispenser_servo_val',
Int64,
queue_size=10)
##--------------------------------------------##
##--------- DISPENSER MOTOR (+encoder) -------##
##--------------------------------------------##
# Create the motor object
self.dispenser_motor = Motor(self.tamp, *DISPENSER_MOTOR_PINS)
# Create a subscriber to listen for dispenser motor commands
rospy.Subscriber('dispenser_cmd', kitware.msg.DriveCMD,
self.dispenser_motor_callback)
## --------- ##
if ENCODER:
# encoder objects
self.dispenser_motor_encoder = Encoder(
self.tamp, *DISPENSER_MOTOR_ENCODER_PINS, continuous=True)
# Publishers for encoder values
self.pub_dispenser_motor_encoder_val = rospy.Publisher(
'dispenser_motor_encoder_val', Int64, queue_size=10)
##--------------------------------------------##
##---------- General ---------##
##--------------------------------------------##
# timer for loop()
self.timer = Timer()
# reset all motor encoders
self.lmotor_encoder.val = 0
self.rmotor_encoder.val = 0
self.dispenser_motor_encoder.val = 0
##--------- GENERAL ---------##
def loop(self):
"""Method that loops at a fast rate, like in Arduino (isn't looping???)"""
#rospy.loginfo("looping!")
if self.timer.millis() > 100:
self.timer.reset()
self.publish_servo_val()
self.publish_encoder_val()
pass
def speed_to_dir_pwm(self, speed):
"""Converts floating point speed (-1.0 to 1.0) to dir and pwm values"""
speed = max(min(speed, 1), -1)
return speed > 0, int(abs(speed * 255))
##--------- ENCODERS ---------##
def publish_encoder_val(self):
if ENCODER:
self.pub_lmotor_encoder_val.publish(self.lmotor_encoder.val)
self.pub_rmotor_encoder_val.publish(self.rmotor_encoder.val)
self.pub_dispenser_motor_encoder_val.publish(
self.dispenser_motor_encoder.val)
##--------- DRIVING MOTORS ---------##
def drive_callback(self, msg):
"""Processes a new drive command and controls motors appropriately"""
self.lmotor.write(*self.speed_to_dir_pwm(msg.l_speed))
self.rmotor.write(*self.speed_to_dir_pwm(msg.r_speed))
##--------- INTAKE MOTOR ---------##
def intake_callback(self, msg):
"""Processes a new drive command and controls motors appropriately"""
self.intake_motor.write(*self.speed_to_dir_pwm(
msg.l_speed)) # use lspeed
##--------- DISPENSER SERVO ---------##
def publish_servo_val(self):
self.pub_dispenser_servo_val.publish(self.servo_val)
def dispenser_servo_callback(self, msg):
# write to servo
self.delta = msg.data
self.servo_val += self.delta
self.servo.write(abs(self.servo_val))
##--------- DISPENSER MOTOR ---------##
def dispenser_motor_callback(self, msg):
"""Processes a new drive command and controls motors appropriately"""
self.dispenser_motor.write(*self.speed_to_dir_pwm(
msg.l_speed)) # use LEFT motor: l_speed
class PIDDrive(SyncedSketch):
ss_pin = 10 #gyro select pin
image_pipe = './image'
if not os.path.exists(image_pipe):
os.mkfifo(image_pipe)
image_fd = os.open(image_pipe, os.O_RDONLY)
def setup(self):
#Pygame stuff
pygame.init()
self.screen = pygame.display.set_mode((300, 300))
self.TicpIn = 4480/2.875
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
self.uIR = DigitalInput(self.tamp, 17)
self.uIR.val = 1
self.servo = Servo(self.tamp, 9)
self.servo.bottom = 0
self.servo.top = 200
self.servo.speed = 30
self.servo.write(self.servo.bottom)
self.servoval = self.servo.bottom
self.delta = 0
self.sorter = Servo(self.tamp, 5)
self.sorter.center = 90
self.sorter.right = 25
self.sorter.left = 165
self.sorter.speed = 15
self.sorter.write(self.sorter.center)
self.sorterval = self.sorter.center
self.dsorter = 0
self.encoderL = Encoder(self.tamp, 22, 23)
self.encoderR = Encoder(self.tamp, 21, 20)
self.init_time = time.time()
# self.encoderL = Encoder(self.tamp, 23, 22)
# self.encoderR = Encoder(self.tamp, 21, 20)
#motor left
self.motorL = Motor(self.tamp, 2, 4)
self.motorLdrive = 0
self.motorL.write(1,0)
#self.deltaL = 1
#self.motorvalL = 0
#motor right
self.motorR = Motor(self.tamp,1, 3)
self.motorRdrive = 0
self.motorR.write(1,0)
#self.deltaR = 1
#self.motorvalR = 0
#gyro
self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
self.initAngle = self.gyro.val
self.newAngle = 0
self.blockAngle = 0
self.blockDistance = 0
print "initial angle:"+str(self.initAngle)
self.prevGyro = 0
self.drift = -.02875
self.totalDrift = 0
self.drifts = []
self.PID=PID(.5, 1, 0.15)
self.fwdVel = 0
self.turnVel = 0
self.turn = 0
self.MoveArm, self.Top, self.Bottom = False, 0, 0
self.Sort = 0
self.SortPause = 0
self.State = 1
self.timer = Timer()
def loop(self):
#state 0 - wait for first no block, do nothing
#transition: no block -> state 1
#state 1 - look for block
#transition: found block -> state 2
#sate 2 - drive over block
#transition: ir triggered -> state 3
#sate 3 - pick up block
#transition: color sensor done -> sate 1
message = os.read(self.image_fd, 20)
if message:
# print("Recieved: '%s'" % message)
if message[:2] == 'no':
self.blockAngle = 0
if self.State == 0:
self.State = 1
if self.State == 2:
self.State = 3
else:
if self.State != 0:
self.State = 2
try:
self.blockDistance, self.blockAngle = [number[:6] for number in message.split(',')]
except:
self.blockAngle = 0
self.blockAngle = float(self.blockAngle)
if self.timer.millis() > 100:
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.display.quit()
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_LEFT:
self.turnVel = -10
if event.key == pygame.K_RIGHT:
self.turnVel = 10
if event.key == pygame.K_UP:
self.fwdVel = 100
if event.key == pygame.K_DOWN:
self.fwdVel = -100
if event.key == pygame.K_1:
self.Sort = 1
if event.key == pygame.K_2:
self.Sort = 2
if event.key == pygame.K_SPACE:
self.MoveArm = True
if event.type == pygame.KEYUP:
if event.key == pygame.K_LEFT or event.key == pygame.K_RIGHT:
self.turnVel = 0
self.turn = 0
if event.key == pygame.K_UP or event.key == pygame.K_DOWN:
self.fwdVel = 0
print 'Color'
print self.color.c
print self.color.r, self.color.g
print
if self.color.c > 800 and self.Sort == 0:
if self.color.r > self.color.g:
self.Sort = 1
else:
self.Sort = 2
# print self.uIR.val
if self.uIR.val == 0:
self.MoveArm = True
self.State = 0
if self.MoveArm:
if self.Bottom == 2 and self.Top == 1:
self.delta = 0
self.servoval = self.servo.bottom
self.servo.write(self.servo.bottom)
self.Bottom, self.Top = 0, 0
self.MoveArm = False
elif self.servoval >= self.servo.top:
time.sleep(1)
self.delta = -self.servo.speed
self.Top = 1
elif self.servoval <= self.servo.bottom:
self.delta = self.servo.speed
self.Bottom = 1
if self.Top == 1:
self.Bottom = 2
if self.Sort == 1:
if self.sorterval < self.sorter.left:
self.dsorter = self.sorter.speed
elif self.sorterval >= self.sorter.left:
self.dsorter = 0
self.Sort = 3
self.SortPause = time.time()
if self.Sort == 2:
if self.sorterval > self.sorter.right:
self.dsorter = -self.sorter.speed
elif self.sorterval <= self.sorter.right:
self.dsorter = 0
self.Sort = 3
self.SortPause = time.time()
if self.Sort == 3:
if time.time() - self.SortPause > 1:
self.sorterval = self.sorter.center
self.Sort = 0
self.sorterval += self.dsorter
self.sorter.write(abs(self.sorterval))
self.servoval += self.delta
self.servo.write(abs(self.servoval))
self.turn += self.turnVel
self.timer.reset()
#response = self.rp.read()
#print "Got response %s" % response
# Valid gyro status is [0,1], see datasheet on ST1:ST0 bits
cAngle=self.gyro.val - self.totalDrift #corrected angle
# print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift
self.prevGyro=self.gyro.val
self.totalDrift+=self.drift
# print 'State: ' + str(self.Searching)
self.newAngle = cAngle + self.blockAngle
print self.blockAngle
pidResult=self.PID.valuePID(cAngle, cAngle+self.blockAngle+self.turn)
# print 'Angle Dif: ' + str(cAngle-self.initAngle) + '\tPID RESULT: '+ str(pidResult)
# print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str(self.encoderL.val)
# print 'AVG: ' + str((self.encoderR.val + self.encoderL.val)/2.)
self.motorLdrive = self.fwdVel - pidResult
self.motorRdrive = self.fwdVel + pidResult
self.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive))
self.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive))
'''
def setup(self):
self.servo = Servo(self.tamp, 10)
self.servo.write(2200)
self.timer = Timer()
self.val = 2200
def setup(self):
self.TicpIn = 4480/2.875
self.ir_array = Ir_array(self.tamp, 16, 15, 14, 40, 11, 10)
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
self.uIR = DigitalInput(self.tamp, 17)
self.uIR.val = 1
self.servo = Servo(self.tamp, 9)
self.servo.bottom = 0
self.servo.top = 200
self.servo.speed = 30
self.servo.write(self.servo.bottom)
self.servoval = self.servo.bottom
self.delta = 0
self.sorter = Servo(self.tamp, 5)
self.sorter.center = 90
self.sorter.right = 25
self.sorter.left = 165
self.sorter.speed = 15
self.sorter.write(self.sorter.center)
self.sorterval = self.sorter.center
self.dsorter = 0
self.encoderL = Encoder(self.tamp, 22, 23)
self.encoderR = Encoder(self.tamp, 21, 20)
self.init_time = time.time()
# self.encoderL = Encoder(self.tamp, 23, 22)
# self.encoderR = Encoder(self.tamp, 21, 20)
#motor left
self.motorL = Motor(self.tamp, 2, 4)
self.motorLdrive = 0
self.motorL.write(1,0)
#self.deltaL = 1
#self.motorvalL = 0
#motor right
self.motorR = Motor(self.tamp,1, 3)
self.motorRdrive = 0
self.motorR.write(1,0)
#self.deltaR = 1
#self.motorvalR = 0
#gyro
self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
self.initAngle = self.gyro.val
self.newAngle = 0
self.blockAngle = 0
self.blockDistance = 0
# print "initial angle:"+str(self.initAngle)
self.Follow = 'Right'
self.IRs = {'Left': [0, 1, 2], 'Right': [5, 4, 3]}
self.prevGyro = 0
self.drift = -.02875
self.totalDrift = 0
self.drifts = []
self.PID=PID(.5, 1, 0.15)
self.IRPID = PID(10, 5, .15)
self.fwdVel = 30
self.turnVel = 0
self.MoveArm, self.Top, self.Bottom = False, 0, 0
self.Sort = 0
self.SortPause = 0
self.State = 1
self.timer = Timer()
class MyRobot(SyncedSketch):
runtime = 180000 #ms
DOOR_OPEN_POS = 40
DOOR_CLOSE_POS = 144
GRIPPER_OPEN_POS = 0
GRIPPER_CLOSE_POS = 180
GRIPPER_DOWN = 1
GRIPPER_UP = 0
def setup(self):
# initialize sensors, settings, start timers, etc.
self.gameTimer = Timer()
self.motorGripper = Motor(self.tamp, 23, 3)
self.motorLeft = Motor(self.tamp, 7, 22)
self.motorRight = Motor(self.tamp, 0, 21)
self.motorval = 0
self.motorLeft.write(1,0)
self.motorRight.write(1,0)
self.motorGripper.write(1,0)
self.currentGripperLevel = 2
print "Motors connected."
self.servoDoor = Servo(self.tamp, 20)
self.servovalDoor = self.DOOR_CLOSE_POS
self.servoDoor.write(self.DOOR_CLOSE_POS)
self.timerDoor = Timer()
self.servoGripper = Servo(self.tamp, 10)
self.servovalGripper = self.GRIPPER_CLOSE_POS
self.servoGripper.write(self.servovalGripper)
self.timerGripper = Timer()
print "Servos connected."
self.timer = Timer()
self.gyro = Gyro(self.tamp, 9)
print "Gyro connected."
self.theta = self.gyro.val
self.dT = .01
self.cam = cv2.VideoCapture(0)
print "Camera connected."
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
frontLeftIR_pin = 14
self.frontLeftIR = AnalogInput(self.tamp, frontLeftIR_pin)
frontRightIR_pin = 15
self.frontRightIR = AnalogInput(self.tamp, frontRightIR_pin)
leftIR_pin = 16
self.leftIR = AnalogInput(self.tamp, leftIR_pin)
rightIR_pin = 17
self.rightIR = AnalogInput(self.tamp, rightIR_pin)
# Initialize PID Values
self.P = 10
self.I = 0 #5
self.D = 0
self.last_diff = 0
self.integral = 0
self.desiredAngle = self.theta
self.finishedCollectingBlock = False
self.finishedDiscardingBlock = False
self.timer = Timer()
self.state = ExploreState()
# self.state = CollectBlockState()
self.blocksCollected = 0
self.leftIRVals = deque([])
self.rightIRVals = deque([])
self.frontRightIRVals = deque([])
self.frontLeftIRVals = deque([])
# Starts the robot
print "Robot setup complete."
def loop(self):
if self.timer.millis() > self.dT*1000:
# print("self.gameTimer", self.gameTimer)
# print(self.gameTimer.millis())
if (self.gameTimer.millis() > self.runtime - 5000): # 5 seconds left in the game
self.openDoorAndBuildTower()
inputs = self.readSensors()
process = self.state.process(inputs)
print "Process: " + process.__class__.__name__
# print(self.gyro.val)
self.state = process.get_next_state()
self.processOutputs(process.get_outputs())
self.timer.reset()
def readSensors(self):
# Calculate the distance traveled, change in theta, and then reset sensors
distance_traveled = 0 #(self.encoderLeft.val + self.encoderRight.val) / 2.0
#encoder_omega = self.encoderLeft.val - self.encoderRight.val
# print('frontRightIR: ', self.frontRightIR.val)
# print("frontLeftIR: ", self.frontLeftIR.val)
# print("leftIR: ", self.leftIR.val)
# print("rightIR: ", self.rightIR.val)
# Camera
ret, frame = self.cam.read()
img = cv2.resize(frame,None,fx=0.25, fy=0.25, interpolation = cv2.INTER_AREA)
blocks = CalculateBlocks(img); #what should CalculateBlocks return?
leftIR = self.leftIR.val
rightIR = self.rightIR.val
frontLeftIR = self.frontLeftIR.val
frontRightIR = self.frontRightIR.val
return Inputs(distance_traveled, self.gyro.val, frontRightIR, frontLeftIR, leftIR, rightIR, self.finishedCollectingBlock, blocks, self.color.r, self.color.g, self.color.b)
def processOutputs(self, Outputs):
# TODO Missing servo outputs
if (Outputs.driving == True):
self.motorval = 50 #25?
else:
self.motorval = 0
if (Outputs.turning == True):
# if we turn, then update self.desiredAngle
self.desiredAngle = self.gyro.val
if (Outputs.turn_clockwise == True):
self.PID(self.desiredAngle + 3)
else:
self.PID(self.desiredAngle - 3)
else:
# self.PID(self.gyro.val)
self.PID(self.desiredAngle)
if Outputs.isCollectingBlock and not self.finishedCollectingBlock:
# Gripper is at level 2 and closed
if self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_CLOSE_POS:
self.closeOrOpenGripper()
# Gripper is at level 2 and open
elif self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_OPEN_POS:
self.moveGripper()
# Gripper is at level 1 and open
elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_OPEN_POS:
self.closeOrOpenGripper()
# Gripper is at level 1 and closed
elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_CLOSE_POS:
self.moveGripper()
# self.finishedCollectingBlock = True
time.sleep(2)
self.blocksCollected += 1
if (self.blocksCollected == 4):
self.finishedCollectingBlock = True
if Outputs.isDiscardingBlock and not self.finishedDiscardingBlock:
# Gripper level 2, closed
if self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_CLOSE_POS:
self.moveGripper()
# Gripper level 1, closed
elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_CLOSE_POS:
self.closeOrOpenGripper()
# Gripper level 1, open
elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_OPEN_POS:
self.moveGripper()
# Gripper level 2, open
elif self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_OPEN_POS:
self.closeOrOpenGripper()
self.finishedDiscardingBlock = True
def PID(self, desired_theta):
# Set encoder to 0 after turning.
# To turn in place, set bias (i.e. motorval to 0)
estimated = self.gyro.val # TODO: calculate estimated with encoder
# print(self.gyro.val)
diff = desired_theta - estimated
# print diff
self.integral += diff * self.dT
derivative = (diff - self.last_diff)/self.dT
power = self.P*diff + self.I*self.integral + self.D*derivative # NOTE: Cap self.D*derivative, use as timeout
# print("motorLeft: ", min(75, abs(self.motorval + power)))
# print("motorRight: ", min(75, abs(self.motorval - power)))
self.motorLeft.write((self.motorval + power)>0, min(75, abs(self.motorval + power)))
self.motorRight.write((self.motorval - power)>0, min(75, abs(self.motorval - power)))
# print "EncoderLeft: " + str(self.encoderLeft.val)
# print "EncoderRight: " + str(self.encoderRight.val)
def moveGripper(self):
if self.currentGripperLevel == 1:
self.motorGripper.write(self.GRIPPER_UP, 100)
timeToSleep = 1.3
if (self.blocksCollected == 1):
timeToSleep = 1.4
elif (self.blocksCollected == 2):
timeToSleep = 1.55
elif (self.blocksCollected == 3):
timeToSleep = 1.7
elif self.currentGripperLevel == 2:
self.motorGripper.write(self.GRIPPER_DOWN, 100)
timeToSleep = 0.9
time.sleep(timeToSleep)
self.motorGripper.write(1,0)
if self.currentGripperLevel == 1:
self.currentGripperLevel = 2
else:
self.currentGripperLevel = 1
def closeOrOpenGripper(self):
if (self.servovalGripper > self.GRIPPER_OPEN_POS):
while(self.servovalGripper > self.GRIPPER_OPEN_POS):
if (self.timerGripper.millis() > 1):
self.timerGripper.reset()
self.servovalGripper -= 1
self.servoGripper.write(abs(self.servovalGripper))
elif (self.servovalGripper < self.GRIPPER_CLOSE_POS):
while(self.servovalGripper < self.GRIPPER_CLOSE_POS):
if (self.timerGripper.millis() > 1):
self.timerGripper.reset()
self.servovalGripper += 1
self.servoGripper.write(abs(self.servovalGripper))
time.sleep(.1)
def openDoorAndBuildTower(self):
self.moveGripper() # should be blocking
self.closeOrOpenGripper()
while(self.servovalDoor > self.DOOR_OPEN_POS):
if (self.timerDoor.millis() > 10):
self.timerDoor.reset()
self.servovalDoor -= 1
# print self.servovalDoor
self.servoDoor.write(abs(self.servovalDoor))
class pythonDriver(SyncedSketch):
ss_pin = 10 #gyro select pin
# also 11, 12, and 13
color_pins = 18, 19
uIR_pin = 17
Lencoder_pins = 22, 23
Rencoder_pins = 21, 20
arm_pin = 9
sorter_pin = 5
Rmoter_pins = 1, 3
Lmotor_pins = 2, 4
pipes = './gyro', './IR'
for path in pipes:
if not os.path.exists(path):
os.mkfifo(path)
def setup(self):
self.init_time = time.time()
self.functions = {}
self.servo = Servo(self.tamp,self.arm_pin)
# self.servo.write(20)
# self.servoval = 20
# self.delta = 0
self.functions['servoARM'] = lambda angle: self.servo.write(angle)
self.sorter = Servo(self.tamp, self.sorter_pin)
# self.sorter.center = 98
# self.sorter.right = 20
# self.sorter.left = 170
# self.sorter.speed = 30
self.functions['servoSORT'] = lambda angle: self.sorter.write(angle)
self.encoderL = Encoder(self.tamp, *self.Lencoder_pins)
self.encoderR = Encoder(self.tamp, *self.Rencoder_pins)
#motor left
self.motorL = Motor(self.tamp, *self.Lmotor_pins)
self.motorLdrive = 0
self.motorL.write(1,0)
#self.deltaL = 1
#self.motorvalL = 0
self.functions['left'] = lambda speed: self.motorL.write(speed<0, abs(speed))
#motor right
self.motorR = Motor(self.tamp, *self.Rmoter_pins)
self.motorRdrive = 0
self.motorR.write(1,0)
#self.deltaR = 1
#self.motorvalR = 0
self.functions['right'] = lambda speed: self.motorR.write(speed<0, abs(speed))
#gyro
self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
self.initAngle = self.gyro.val
print "initial angle:"+str(self.initAngle)
self.prevGyro = 0
self.drift = -.02875
self.totalDrift = 0
self.drifts = []
self.PID=PID(2,1,0.15)
self.fwdVel = 0
self.turnVel = 0
self.maxVel = 40
self.Distance = 0
self.MoveArm, self.Top, self.Bottom = False, 0, 0
self.Sort = 0
self.SortPause = 0
self.timer = Timer()
#connect to pipe
pipe_path = "./robot"
try:
os.mkfifo(pipe_path)
except OSError:
print "error"
self.pipe_fd = os.open(pipe_path, os.O_RDONLY | os.O_NONBLOCK)
def loop(self):
if self.timer.millis() > 100:
#if get message
#read message
'''Alternative Command Names:
left
right
servoARM
servoSORT
'''
response = os.read(self.pipe_fd,100)
print "Got response %s" % response
#rp.close()
print "no message"
value, function = response.split(' ')
functions[function](int(value))
'''
if response == "LEFT":
self.turnVel = -5
print 'Got Key'
if response == "RIGHT":
self.turnVel = 5
if response == "UP":
self.fwdVel = 40
if response == "DOWN":
self.fwdVel = -40
if response == "SPACE":
self.MoveArm = True
#stop message
if response =="STOP":
self.turVel=0
self.fwdVel=0
if event.key == 'SORT':
if self.Sort == 0:
self.Sort = 1
if event.key == pygame.K_2:
if self.Sort == 0:
self.Sort = 2
if response == "LEFT" or response == "RIGHT":
self.turnVel = 0
if response == "UP" or response == "DOWN":
self.fwdVel = 0
if self.MoveArm:
if self.Bottom == 2 and self.Top == 1:
self.delta = 0
self.servoval = 20
self.servo.write(20)
self.Bottom, self.Top = 0, 0
self.MoveArm = False
elif self.servoval >= 170:
self.delta = -30
self.Top = 1
elif self.servoval <= 30:
self.delta = 30
self.Bottom = 1
if self.Top == 1:
self.Bottom = 2
self.servoval += self.delta
self.servo.write(abs(self.servoval))
print 'delta: ' + str(self.delta)
print 'servoval: ' + str(self.servoval)
print 'MoveArm: ' + str(self.MoveArm)
self.initAngle += self.turnVel
self.timer.reset()
#response = self.rp.read()
#print "Got response %s" % response
# Valid gyro status is [0,1], see datasheet on ST1:ST0 bit'''
cAngle=self.gyro.val - self.totalDrift #corrected angle
#print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift
'''
class Driver(Sketch):
def setup(self):
# ROS Setup
rospy.init_node('driver')
rospy.Subscriber("driver_commands", String, callback)
# self.tof_pub = rospy.Publisher('tof_readings', String, queue_size=10)
# Device Setup
self.motor1 = Motor(self.tamp, MOTOR1_DIR, MOTOR1_PWM)
self.motor2 = Motor(self.tamp, MOTOR2_DIR, MOTOR2_PWM)
self.encoder1 = Encoder(self.tamp, *ENCODER1, continuous=True)
self.encoder2 = Encoder(self.tamp, *ENCODER2, continuous=True)
# self.encoder1_value = 0
# self.encoder2_value = 0
# TimeOfFlight Sensors
# self.tof1 = TimeOfFlight(self.tamp, TOF1, 1, continuous=False)
# self.tof2 = TimeOfFlight(self.tamp, TOF2, 2, continuous=False)
# self.tof1.enable()
# self.tof2.enable()
# self.tof1_value = 0
# self.tof2_value = 0
# Servo for the Release
self.servo = Servo(self.tamp, SERVO)
self.servo.write(SERVO_CLOSE)
# Controller Params
self.pid = PID(p=P, i=I, d=D)
self.target1 = 0
self.target2 = 0
self.servo_commands = Queue()
def rotate(self, angle):
target_rotation = (angle / 360.0) * ROBOT_RADIUS / WHEEL_RADIUS
target_ticks = FULL_TICKS * TURN_COEFFICIENT * target_rotation
self._rotate(target_ticks)
def _rotate(self, ticks):
self.target1 = self.encoder1.val + MOTOR1_DIRECTION * ticks
self.target2 = self.encoder2.val + MOTOR2_DIRECTION * ticks
def move(self, distance):
target_rotation = distance / (2 * np.pi * WHEEL_RADIUS)
target_ticks = target_rotation * FULL_TICKS
self.target1 = self.encoder1.val - MOTOR1_DIRECTION * target_ticks
self.target2 = self.encoder2.val + MOTOR2_DIRECTION * target_ticks
def release(self):
self.servo_commands.put(SERVO_OPEN)
sleep(SERVO_WAIT)
self.servo_commands.put(SERVO_CLOSE)
def loop(self):
sleep(SLEEP_TIME)
# Send Sensor Requests
# self.tamp.send_request(self.encoder1.id,
# self.encoder1.READ_CODE,
# self._handle_encoder1_update,
# continuous=False,
# weight=1)
# self.tamp.send_request(self.encoder2.id,
# self.encoder2.READ_CODE,
# self._handle_encoder2_update,
# continuous=False,
# weight=1)
# self.tamp.send_request(self.tof1.id,
# self.tof1.READ_CODE,
# self._handle_tof1_update,
# continuous=False,
# weight=1)
# self.tamp.send_request(self.tof2.id,
# self.tof2.READ_CODE,
# self._handle_tof2_update,
# continuous=False,
# weight=1)
if self.servo_commands.qsize() != 0:
self.servo.write(self.servo_commands.get())
# Main Control Logic
diff1 = self.target1 - self.encoder1.val
diff2 = self.target2 - self.encoder2.val
if abs(diff1) < CONTROLLER_THRESHOLD: diff1 = 0
if abs(diff2) < CONTROLLER_THRESHOLD: diff2 = 0
write(self.motor1, MOTOR1_DIRECTION, self.pid(feedback=diff1))
write(self.motor2, MOTOR2_DIRECTION, self.pid(feedback=diff2))
def setup(self):
self.init_time = time.time()
self.functions = {}
self.servo = Servo(self.tamp,self.arm_pin)
# self.servo.write(20)
# self.servoval = 20
# self.delta = 0
self.functions['servoARM'] = lambda angle: self.servo.write(angle)
self.sorter = Servo(self.tamp, self.sorter_pin)
# self.sorter.center = 98
# self.sorter.right = 20
# self.sorter.left = 170
# self.sorter.speed = 30
self.functions['servoSORT'] = lambda angle: self.sorter.write(angle)
self.encoderL = Encoder(self.tamp, *self.Lencoder_pins)
self.encoderR = Encoder(self.tamp, *self.Rencoder_pins)
#motor left
self.motorL = Motor(self.tamp, *self.Lmotor_pins)
self.motorLdrive = 0
self.motorL.write(1,0)
#self.deltaL = 1
#self.motorvalL = 0
self.functions['left'] = lambda speed: self.motorL.write(speed<0, abs(speed))
#motor right
self.motorR = Motor(self.tamp, *self.Rmoter_pins)
self.motorRdrive = 0
self.motorR.write(1,0)
#self.deltaR = 1
#self.motorvalR = 0
self.functions['right'] = lambda speed: self.motorR.write(speed<0, abs(speed))
#gyro
self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
self.initAngle = self.gyro.val
print "initial angle:"+str(self.initAngle)
self.prevGyro = 0
self.drift = -.02875
self.totalDrift = 0
self.drifts = []
self.PID=PID(2,1,0.15)
self.fwdVel = 0
self.turnVel = 0
self.maxVel = 40
self.Distance = 0
self.MoveArm, self.Top, self.Bottom = False, 0, 0
self.Sort = 0
self.SortPause = 0
self.timer = Timer()
#connect to pipe
pipe_path = "./robot"
try:
os.mkfifo(pipe_path)
except OSError:
print "error"
self.pipe_fd = os.open(pipe_path, os.O_RDONLY | os.O_NONBLOCK)
class Robot(SyncedSketch):
def setup(self):
####################
#### EE SETUP ####
####################
# Motor object representing the left motor.
self.leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION,
LEFT_DRIVE_CONTROLLER_PWM)
# Encoder object for the left motor.
self.leftEncoder = Encoder(self.tamp, LEFT_DRIVE_ENCODER_YELLOW,
LEFT_DRIVE_ENCODER_WHITE)
# Motor object representing the right motor.
self.rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION,
RIGHT_DRIVE_CONTROLLER_PWM)
# Encoder object for the right motor.
self.rightEncoder = Encoder(self.tamp, RIGHT_DRIVE_ENCODER_YELLOW,
RIGHT_DRIVE_ENCODER_WHITE)
# Motor object representing the intake mechanism motors.
self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION,
HUGS_MOTOR_CONTROLLER_PWM)
# Encoder object for the intake motor.
self.intakeEncoder = Encoder(self.tamp, HUGS_MOTOR_ENCODER_YELLOW,
HUGS_MOTOR_ENCODER_WHITE)
# Motor object representing the conveyor belt motor.
self.conveyorMotor = Motor(self.tamp, BELT_MOTOR_CONTROLLER_DIRECTION,
BELT_MOTOR_CONTROLLER_PWM)
# Encoder object for the conveyor belt motor.
self.conveyorEncoder = Encoder(self.tamp, BELT_MOTOR_ENCODER_YELLOW,
BELT_MOTOR_ENCODER_WHITE)
# Long range IR sensors
self.irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL)
self.irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR)
self.irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL)
self.irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR)
# Color sensor
self.color = Color(self.tamp)
# Limit switches
self.conveyorLimSwitch = DigitalInput(self.tamp, CONVEYOR_LIMIT_SWITCH)
self.blockLimSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH)
# Servo controlling the door of the collection chamber.
self.backDoorServo = Servo(self.tamp, SERVO_PIN)
# Make sure the door is closed
self.backDoorServo.write(SERVO_CLOSE)
# The switch that tells the program that the competition has started
self.competitionModeSwitch = DigitalInput(self.tamp, COMPETITION_MODE)
#################################
#### INTERNAL MODULE SETUP ####
#################################
# Timer object to moderate checking for intake errors.
self.intakeTimer = Timer()
# Are the intake motors reversing? True if so, False if going forwards.
self.intakeDirection = False
# Start the intake motor.
self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
# Wall Follow object
self.wallFollow = WallFollow(self.leftMotor, self.rightMotor, Timer(),
self.irFL, self.irFR, self.irBL,
self.irBR)
# A timer to make sure timesteps are only 10 times/second
self.timestepTimer = Timer()
# Timer object describing how long the current module has been running.
self.moduleTimer = Timer()
# Timer for the whole game
self.gameTimer = Timer()
# Runs the PICKUP process
self.pickup = PickupModule(self.moduleTimer, self.conveyorLimSwitch,
self.conveyorMotor, self.conveyorEncoder)
# Runs the DROPOFF process
self.dropoff = DropoffModule(self.moduleTimer, self.backDoorServo,
self.rightMotor, self.leftMotor,
self.rightEncoder)
# Runs the FOLLOW process TODO: Fix forward to actually mean forward.
self.follow = FollowModule(self.moduleTimer, self.leftMotor,
self.rightMotor, self.intakeMotor,
self.wallFollow, FORWARD_SPEED,
self.blockLimSwitch)
# Runs the CHECK process. TODO: pass in proper timers.
self.check = CheckModule(self.moduleTimer, self.leftMotor,
self.rightMotor, self.intakeMotor, self.color)
# Waits for the game to start
self.off = OffModule(self.gameTimer, self.competitionModeSwitch)
# Describes which stage of the program is running.
self.module = MODULE_OFF
def loop(self):
if self.timestepTimer.millis() > 100:
self.timestepTimer.reset()
#print "Module Number", self.module
state = -1
if self.module == MODULE_OFF:
state = self.off.run()
elif self.module == MODULE_CHECK:
state = self.check.run()
elif self.module == MODULE_PICKUP:
state = self.pickup.run()
elif self.module == MODULE_DROPOFF:
self.module = MODULE_FOLLOW
state = self.follow.run()
elif self.module == MODULE_FOLLOW:
state = self.follow.run()
else:
print "Attempting to run nonexistent module"
self.stop()
self.updateState(state)
if self.gameTimer.millis() % 10000 == 0:
print "Game Time: ", self.gameTimer.millis()
if self.gameTimer.millis() > GAME_LENGTH - 500:
self.backDoorServo.write(SERVO_OPEN)
self.leftMotor.write(0, 0)
self.rightMotor.write(0, 0)
if self.gameTimer.millis() > GAME_LENGTH:
self.module = MODULE_END
self.stop()
return
## Switch module if necessary.
def updateState(self, module):
if self.module == module:
return
elif module == MODULE_OFF:
self.off.start()
self.module = MODULE_OFF
return
elif module == MODULE_CHECK:
self.check.start()
self.module = MODULE_CHECK
return
elif module == MODULE_PICKUP:
self.pickup.start()
self.module = MODULE_PICKUP
return
elif module == MODULE_DROPOFF:
self.dropoff.start()
self.module = MODULE_DROPOFF
return
elif module == MODULE_FOLLOW:
self.follow.start()
self.module = MODULE_FOLLOW
return
else:
print "Attempting to switch to nonexistent module"
self.stop()
## Make sure that the intake motor does not stall.
# If so, reverse the intake motors.
#
# @param checkTime Time in ms between checking stalls.
# @param reverseTime Time in ms that the intake motors will reverse if needed.
def checkForIntakeErrors(self, checkTime=100, reverseTime=800):
if self.intakeDirection: # We are moving forward.
if self.intakeTimer.millis() > checkTime:
self.intakeTimer.reset()
if self.intakeEncoder.val < INTAKE_ENCODER_LIMIT: # if we're stalled
self.intakeDirection = True
self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
else: # if we're not stalled
self.intakeEncoder.write(0)
else: # We are reversing the motors.
if self.intakeTimer.millis() > reverseTime:
self.intakeTimer.reset()
self.intakeDirection = False
self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
self.intakeEncoder.write(0)
self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
def setup(self):
####################
#### EE SETUP ####
####################
# Motor object representing the left motor.
self.leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION, LEFT_DRIVE_CONTROLLER_PWM)
# Encoder object for the left motor.
self.leftEncoder = Encoder(self.tamp, LEFT_DRIVE_ENCODER_YELLOW, LEFT_DRIVE_ENCODER_WHITE)
# Motor object representing the right motor.
self.rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION, RIGHT_DRIVE_CONTROLLER_PWM)
# Encoder object for the right motor.
self.rightEncoder = Encoder(self.tamp, RIGHT_DRIVE_ENCODER_YELLOW, RIGHT_DRIVE_ENCODER_WHITE)
# Motor object representing the intake mechanism motors.
self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION, HUGS_MOTOR_CONTROLLER_PWM)
# Encoder object for the intake motor.
self.intakeEncoder = Encoder(self.tamp, HUGS_MOTOR_ENCODER_YELLOW, HUGS_MOTOR_ENCODER_WHITE)
# Motor object representing the conveyor belt motor.
self.conveyorMotor = Motor(self.tamp, BELT_MOTOR_CONTROLLER_DIRECTION, BELT_MOTOR_CONTROLLER_PWM)
# Encoder object for the conveyor belt motor.
self.conveyorEncoder = Encoder(self.tamp, BELT_MOTOR_ENCODER_YELLOW, BELT_MOTOR_ENCODER_WHITE)
# Long range IR sensors
self.irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL)
self.irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR)
self.irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL)
self.irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR)
# Color sensor
self.color = Color(self.tamp)
# Limit switches
self.conveyorLimSwitch = DigitalInput(self.tamp, CONVEYOR_LIMIT_SWITCH)
self.blockLimSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH)
# Servo controlling the door of the collection chamber.
self.backDoorServo = Servo(self.tamp, SERVO_PIN)
# Make sure the door is closed
self.backDoorServo.write(SERVO_CLOSE)
# The switch that tells the program that the competition has started
self.competitionModeSwitch = DigitalInput(self.tamp, COMPETITION_MODE)
#################################
#### INTERNAL MODULE SETUP ####
#################################
# Timer object to moderate checking for intake errors.
self.intakeTimer = Timer()
# Are the intake motors reversing? True if so, False if going forwards.
self.intakeDirection = False
# Start the intake motor.
self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
# Wall Follow object
self.wallFollow = WallFollow(self.leftMotor, self.rightMotor, Timer(), self.irFL, self.irFR, self.irBL, self.irBR)
# A timer to make sure timesteps are only 10 times/second
self.timestepTimer = Timer()
# Timer object describing how long the current module has been running.
self.moduleTimer = Timer()
# Timer for the whole game
self.gameTimer = Timer()
# Runs the PICKUP process
self.pickup = PickupModule(self.moduleTimer, self.conveyorLimSwitch, self.conveyorMotor, self.conveyorEncoder)
# Runs the DROPOFF process
self.dropoff = DropoffModule(self.moduleTimer, self.backDoorServo, self.rightMotor, self.leftMotor, self.rightEncoder)
# Runs the FOLLOW process TODO: Fix forward to actually mean forward.
self.follow = FollowModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.wallFollow, FORWARD_SPEED, self.blockLimSwitch)
# Runs the CHECK process. TODO: pass in proper timers.
self.check = CheckModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.color)
# Waits for the game to start
self.off = OffModule(self.gameTimer, self.competitionModeSwitch)
# Describes which stage of the program is running.
self.module = MODULE_OFF
class PIDDrive(SyncedSketch):
ss_pin = 10 #gyro select pin
image_pipe = './image'
if not os.path.exists(image_pipe):
os.mkfifo(image_pipe)
image_fd = os.open(image_pipe, os.O_RDONLY)
def setup(self):
#Pygame stuff
pygame.init()
self.screen = pygame.display.set_mode((300, 300))
self.TicpIn = 4480 / 2.875
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
self.uIR = DigitalInput(self.tamp, 17)
self.uIR.val = 1
self.servo = Servo(self.tamp, 9)
self.servo.bottom = 0
self.servo.top = 200
self.servo.speed = 30
self.servo.write(self.servo.bottom)
self.servoval = self.servo.bottom
self.delta = 0
self.sorter = Servo(self.tamp, 5)
self.sorter.center = 90
self.sorter.right = 25
self.sorter.left = 165
self.sorter.speed = 15
self.sorter.write(self.sorter.center)
self.sorterval = self.sorter.center
self.dsorter = 0
self.encoderL = Encoder(self.tamp, 22, 23)
self.encoderR = Encoder(self.tamp, 21, 20)
self.init_time = time.time()
# self.encoderL = Encoder(self.tamp, 23, 22)
# self.encoderR = Encoder(self.tamp, 21, 20)
#motor left
self.motorL = Motor(self.tamp, 2, 4)
self.motorLdrive = 0
self.motorL.write(1, 0)
#self.deltaL = 1
#self.motorvalL = 0
#motor right
self.motorR = Motor(self.tamp, 1, 3)
self.motorRdrive = 0
self.motorR.write(1, 0)
#self.deltaR = 1
#self.motorvalR = 0
#gyro
self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
self.initAngle = self.gyro.val
self.newAngle = 0
self.blockAngle = 0
self.blockDistance = 0
print "initial angle:" + str(self.initAngle)
self.prevGyro = 0
self.drift = -.02875
self.totalDrift = 0
self.drifts = []
self.PID = PID(.5, 1, 0.15)
self.fwdVel = 0
self.turnVel = 0
self.turn = 0
self.MoveArm, self.Top, self.Bottom = False, 0, 0
self.Sort = 0
self.SortPause = 0
self.State = 1
self.timer = Timer()
def loop(self):
#state 0 - wait for first no block, do nothing
#transition: no block -> state 1
#state 1 - look for block
#transition: found block -> state 2
#sate 2 - drive over block
#transition: ir triggered -> state 3
#sate 3 - pick up block
#transition: color sensor done -> sate 1
message = os.read(self.image_fd, 20)
if message:
# print("Recieved: '%s'" % message)
if message[:2] == 'no':
self.blockAngle = 0
if self.State == 0:
self.State = 1
if self.State == 2:
self.State = 3
else:
if self.State != 0:
self.State = 2
try:
self.blockDistance, self.blockAngle = [
number[:6] for number in message.split(',')
]
except:
self.blockAngle = 0
self.blockAngle = float(self.blockAngle)
if self.timer.millis() > 100:
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.display.quit()
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_LEFT:
self.turnVel = -10
if event.key == pygame.K_RIGHT:
self.turnVel = 10
if event.key == pygame.K_UP:
self.fwdVel = 100
if event.key == pygame.K_DOWN:
self.fwdVel = -100
if event.key == pygame.K_1:
self.Sort = 1
if event.key == pygame.K_2:
self.Sort = 2
if event.key == pygame.K_SPACE:
self.MoveArm = True
if event.type == pygame.KEYUP:
if event.key == pygame.K_LEFT or event.key == pygame.K_RIGHT:
self.turnVel = 0
self.turn = 0
if event.key == pygame.K_UP or event.key == pygame.K_DOWN:
self.fwdVel = 0
print 'Color'
print self.color.c
print self.color.r, self.color.g
print
if self.color.c > 800 and self.Sort == 0:
if self.color.r > self.color.g:
self.Sort = 1
else:
self.Sort = 2
# print self.uIR.val
if self.uIR.val == 0:
self.MoveArm = True
self.State = 0
if self.MoveArm:
if self.Bottom == 2 and self.Top == 1:
self.delta = 0
self.servoval = self.servo.bottom
self.servo.write(self.servo.bottom)
self.Bottom, self.Top = 0, 0
self.MoveArm = False
elif self.servoval >= self.servo.top:
time.sleep(1)
self.delta = -self.servo.speed
self.Top = 1
elif self.servoval <= self.servo.bottom:
self.delta = self.servo.speed
self.Bottom = 1
if self.Top == 1:
self.Bottom = 2
if self.Sort == 1:
if self.sorterval < self.sorter.left:
self.dsorter = self.sorter.speed
elif self.sorterval >= self.sorter.left:
self.dsorter = 0
self.Sort = 3
self.SortPause = time.time()
if self.Sort == 2:
if self.sorterval > self.sorter.right:
self.dsorter = -self.sorter.speed
elif self.sorterval <= self.sorter.right:
self.dsorter = 0
self.Sort = 3
self.SortPause = time.time()
if self.Sort == 3:
if time.time() - self.SortPause > 1:
self.sorterval = self.sorter.center
self.Sort = 0
self.sorterval += self.dsorter
self.sorter.write(abs(self.sorterval))
self.servoval += self.delta
self.servo.write(abs(self.servoval))
self.turn += self.turnVel
self.timer.reset()
#response = self.rp.read()
#print "Got response %s" % response
# Valid gyro status is [0,1], see datasheet on ST1:ST0 bits
cAngle = self.gyro.val - self.totalDrift #corrected angle
# print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift
self.prevGyro = self.gyro.val
self.totalDrift += self.drift
# print 'State: ' + str(self.Searching)
self.newAngle = cAngle + self.blockAngle
print self.blockAngle
pidResult = self.PID.valuePID(cAngle,
cAngle + self.blockAngle + self.turn)
# print 'Angle Dif: ' + str(cAngle-self.initAngle) + '\tPID RESULT: '+ str(pidResult)
# print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str(self.encoderL.val)
# print 'AVG: ' + str((self.encoderR.val + self.encoderL.val)/2.)
self.motorLdrive = self.fwdVel - pidResult
self.motorRdrive = self.fwdVel + pidResult
self.motorL.write(self.motorLdrive < 0, abs(self.motorLdrive))
self.motorR.write(self.motorRdrive < 0, abs(self.motorRdrive))
'''
def setup(self):
self.servo = Servo(self.tamp, 9)
self.servo.write(1050)
self.timer = Timer()
self.end = False
class Sorter: def __init__(self, tamp): self.tamp=tamp self.servo = Servo(self.tamp, 5) self.servo.center = 90 self.servo.right = 25 self.servo.left = 165 self.servo.speed = 15 self.servo.jostleSpeed = 40 self.servo.rightJostle = 60 self.servo.leftJostle = 120 self.servo.write(self.servo.center) self.sorterval = self.servo.center self.dsorter = 0 self.sorterState = "None" self.startPauseTime=0 self.pauseLength=0 def moveSorterLeft(self): if self.sorterval < self.servo.left: self.sorterState="Left" self.dsorter = self.servo.speed elif self.sorterval >= self.servo.left: self.setPause(1) self.sorterState="Pause" self.nextSorterState="Center" self.dsorter = 0 def moveSorterRight(self): if self.sorterval > self.servo.right: self.sorterState="Right" self.dsorter = -self.servo.speed elif self.sorterval <= self.servo.right: self.setPause(1) self.sorterState="Pause" self.nextSorterState="Center" self.dsorter = 0 def moveSorterCenter(self): self.sorterState="None" self.sorterval = self.servo.center def jostleServoLeft(self): if self.sorterval < self.servo.leftJostle: self.sorterState="LeftJostle" self.dsorter = self.servo.jostleSpeed elif self.sorterval >= self.servo.leftJostle: self.sorterState="RightJostle" self.dsorter = 0 def jostleServoRight(self): if self.sorterval > self.servo.rightJostle: self.sorterState="RightJostle" self.dsorter = -self.servo.jostleSpeed elif self.sorterval <= self.servo.rightJostle: self.sorterState="Center" self.nextSorterState="None" self.dsorter = 0 #jostle moves the servo back and forth in a way that does not allow the block to fall into either tower #it is meant to "de-choke" the funnel if a block has become stuck def jostle(self): self.sorterState ="LeftJostle" def setPause(self,pauseLength): self.startPauseTime=time.time() self.pauseLength=pauseLength def pauseSorter(self): if(time.time()-self.startPauseTime>self.pauseLength): self.sorterState=self.nextSorterState self.nextSorterState="None" def update(self): if(self.sorterState!="None"): if(self.sorterState=="Left"): self.moveSorterLeft() elif(self.sorterState=="Right"): self.moveSorterRight() elif(self.sorterState=="Center"): self.moveSorterCenter() elif(self.sorterState=="Pause"): self.pauseSorter() elif self.sorterState=="LeftJostle": self.jostleServoLeft() elif self.sorterState =="RightJostle": self.jostleServoRight() self.sorterval += self.dsorter self.servo.write(abs(self.sorterval))
def setup(self):
#Pygame stuff
pygame.init()
self.screen = pygame.display.set_mode((300, 300))
self.TicpIn = 4480 / 2.875
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
self.uIR = DigitalInput(self.tamp, 17)
self.uIR.val = 1
self.servo = Servo(self.tamp, 9)
self.servo.bottom = 0
self.servo.top = 200
self.servo.speed = 30
self.servo.write(self.servo.bottom)
self.servoval = self.servo.bottom
self.delta = 0
self.sorter = Servo(self.tamp, 5)
self.sorter.center = 90
self.sorter.right = 25
self.sorter.left = 165
self.sorter.speed = 15
self.sorter.write(self.sorter.center)
self.sorterval = self.sorter.center
self.dsorter = 0
self.encoderL = Encoder(self.tamp, 22, 23)
self.encoderR = Encoder(self.tamp, 21, 20)
self.init_time = time.time()
# self.encoderL = Encoder(self.tamp, 23, 22)
# self.encoderR = Encoder(self.tamp, 21, 20)
#motor left
self.motorL = Motor(self.tamp, 2, 4)
self.motorLdrive = 0
self.motorL.write(1, 0)
#self.deltaL = 1
#self.motorvalL = 0
#motor right
self.motorR = Motor(self.tamp, 1, 3)
self.motorRdrive = 0
self.motorR.write(1, 0)
#self.deltaR = 1
#self.motorvalR = 0
#gyro
self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
self.initAngle = self.gyro.val
self.newAngle = 0
self.blockAngle = 0
self.blockDistance = 0
print "initial angle:" + str(self.initAngle)
self.prevGyro = 0
self.drift = -.02875
self.totalDrift = 0
self.drifts = []
self.PID = PID(.5, 1, 0.15)
self.fwdVel = 0
self.turnVel = 0
self.turn = 0
self.MoveArm, self.Top, self.Bottom = False, 0, 0
self.Sort = 0
self.SortPause = 0
self.State = 1
self.timer = Timer()
