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):
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):
# 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 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):
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):
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 __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 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):
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 setup(self):
self.servo = Servo(self.tamp, 10)
self.servo.write(2200)
self.timer = Timer()
self.val = 2200
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):
# 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."
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
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):
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 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):
self.servo = Servo(self.tamp, 9)
self.servo.write(1050)
self.timer = Timer()
self.end = False
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 __init__(self, tamp):
self.latch = Servo(tamp, pins.stack_latch, 1830, 1130)
self.latch.write(0)
