def setup(self):
#self.motorLeft = Motor(self.tamp, 21, 20)
#self.motorRight = Motor(self.tamp, 23, 22)
self.motorLeft = Motor(self.tamp, 20, 21)
self.motorRight = Motor(self.tamp, 22, 23)
self.motorLeft.write(1,0)
self.motorRight.write(1,0)
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)
# TODO: set encoder to 0
self.timer = Timer()
self.gyro = Gyro(self.tamp, 10)
self.P = 5
self.I = 0 # should be negative
self.D = 0
self.dT = .03
self.motorval = 25 #50
self.last_diff = 0
self.integral = 0
self.desired = self.gyro.val #+ 45 # to drive in a straight line
self.encoderLeft.start_continuous()
self.encoderRight.start_continuous()
class KitBot(ROSSketch):
"""ROS Node that controls the KitBod via the Teensy and tamproxy"""
# PIN MAPPINGS
LMOTOR_PINS = (2, 3) # DIR, PWM
RMOTOR_PINS = (4, 5) # DIR, PWM
def setup(self):
"""One-time method that sets up the robot, like in Arduino"""
# Create the motor objects
self.lmotor = Motor(self.tamp, *self.LMOTOR_PINS)
self.rmotor = Motor(self.tamp, *self.RMOTOR_PINS)
# Create a subscriber to listen for drive motor commands
self.drive_sub = rospy.Subscriber(
'drive_cmd', kitware.msg.DriveCMD, self.drive_callback)
def loop(self):
"""Method that loops at a fast rate, like in Arduino"""
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))
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))
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
class EncoderRead(SyncedSketch):
encoderPins = 28, 27
motorPwm = 6 #green
motorDir = 7 #yellow
def setup(self):
self.encoder = Encoder(self.tamp, *self.encoderPins, continuous=True)
self.motor = Motor(self.tamp, self.motorDir, self.motorPwm)
self.miscTimer = Timer()
self.timer = Timer()
self.rotations = 0
self.lastRot = 0
self.motor.write(0, 100)
def loop(self):
if self.timer.millis() > 100:
print "enc", self.encoder.val
self.timer.reset()
if self.encoder.val > 5.0 * 3200:
print "Stopped at", self.encoder.val
self.motor.write(1,0)
self.stop()
'''
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):
self.motor = Motor(self.tamp, 11, 9)
self.tof1 = TimeOfFlight(self.tamp, 20, 1)
self.tof2 = TimeOfFlight(self.tamp, 22, 2)
self.tof1.enable()
self.tof2.enable()
def setup(self):
self.state = self.STRAIGHT_STATE
self.leftMotor = Motor(self.tamp, LEFT_MOTOR_DIRECTION, LEFT_MOTOR_PWM)
self.rightMotor = Motor(self.tamp, RIGHT_MOTOR_DIRECTION,
RIGHT_MOTOR_PWM)
self.bumper_RF = DigitalInput(self.tamp, BUMP_SENSOR_R_FRONT)
self.bumper_LF = DigitalInput(self.tamp, BUMP_SENSOR_L_FRONT)
self.bumper_RS = DigitalInput(self.tamp, BUMP_SENSOR_R_SIDE)
self.bumper_LS = DigitalInput(self.tamp, BUMP_SENSOR_L_SIDE)
self.comp_timer = Timer()
self.straight_timer = Timer()
self.reverse_timer = Timer()
self.turn_timer = Timer()
self.bump_timer = Timer()
self.turn_direction = LEFT
self.turn_pwm = 50
def setup(self):
self.state = self.STRAIGHT_STATE
self.leftMotor = Motor(self.tamp, LEFT_MOTOR_DIRECTION, LEFT_MOTOR_PWM)
self.rightMotor = Motor(self.tamp, RIGHT_MOTOR_DIRECTION, RIGHT_MOTOR_PWM)
self.bumper_RF = DigitalInput(self.tamp, BUMP_SENSOR_R_FRONT)
self.bumper_LF = DigitalInput(self.tamp, BUMP_SENSOR_L_FRONT)
self.bumper_RS = DigitalInput(self.tamp, BUMP_SENSOR_R_SIDE)
self.bumper_LS = DigitalInput(self.tamp, BUMP_SENSOR_L_SIDE)
self.comp_timer = Timer()
self.straight_timer = Timer()
self.reverse_timer = Timer()
self.turn_timer = Timer()
self.bump_timer = Timer()
self.turn_direction = LEFT
self.turn_pwm = 50
def setup(self):
#self.motorLeft = Motor(self.tamp, 21, 20)
#self.motorRight = Motor(self.tamp, 23, 22)
self.motorLeft = Motor(self.tamp, 20, 21)
self.motorRight = Motor(self.tamp, 22, 23)
self.motorLeft.write(1, 0)
self.motorRight.write(1, 0)
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)
# TODO: set encoder to 0
self.timer = Timer()
self.gyro = Gyro(self.tamp, 10)
self.P = 5
self.I = 0 # should be negative
self.D = 0
self.dT = .03
self.motorval = 25 #50
self.last_diff = 0
self.integral = 0
self.desired = self.gyro.val #+ 45 # to drive in a straight line
self.encoderLeft.start_continuous()
self.encoderRight.start_continuous()
def setup(self):
self.steps = 0
# Motor object representing the conveyor belt motor.
self.motor = Motor(self.tamp, 7, 6)
# Encoder object for the conveyor belt motor.
self.encoder = Encoder(self.tamp, 28, 27)
self.timer = Timer()
self.motor.write(DOWN, 75)
def setup(self):
self.encoder = Encoder(self.tamp, *self.encoderPins, continuous=True)
self.motor = Motor(self.tamp, self.motorDir, self.motorPwm)
self.miscTimer = Timer()
self.timer = Timer()
self.rotations = 0
self.lastRot = 0
self.motor.write(0, 100)
def setup(self):
"""One-time method that sets up the robot, like in Arduino"""
# Create the motor objects
self.lmotor = Motor(self.tamp, *self.LMOTOR_PINS)
self.rmotor = Motor(self.tamp, *self.RMOTOR_PINS)
# Create a subscriber to listen for drive motor commands
self.drive_sub = rospy.Subscriber('drive_cmd', kitware.msg.DriveCMD, self.drive_callback)
def setup(self):
self.motor = Motor(self.tamp, 24, 25)
self.motor.write(1, 0)
self.delta = 1
self.motorval = 0
self.timer = Timer()
self.counter = 0
self.spin_forwards = True
class MotorWrite(SyncedSketch):
def setup(self):
# Motor object representing the conveyor belt motor.
self.conveyorMotor = Motor(self.tamp, 7, 6)
#self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION, HUGS_MOTOR_CONTROLLER_PWM)
self.start()
def loop(self):
self.conveyorMotor.write(1, CONVEYOR_POWER) #0 for up, 1 for down
def setup(self):
self.motor1 = Motor(self.tamp, 2, 3)
self.motor1.write(1,0)
self.motor2 = Motor(self.tamp, 5, 4)
self.motor2.write(1,0)
self.delta = 1
self.motorval = 70
self.timer = Timer()
self.steps = 0
class MotorWrite(SyncedSketch):
def setup(self):
# Motor object representing the conveyor belt motor.
self.conveyorMotor = Motor(self.tamp, 7, 6)
#self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION, HUGS_MOTOR_CONTROLLER_PWM)
self.start()
def loop(self):
self.conveyorMotor.write(1, CONVEYOR_POWER) #0 for up, 1 for down
def setup(self):
self.timer = Timer()
self.ttt = Timer()
self.timeout = 5000
# Motor object representing the conveyor belt motor.
self.conveyorMotor = Motor(self.tamp, 7, 6)
# Encoder object for the conveyor belt motor.
self.conveyorEncoder = Encoder(self.tamp, 28, 27)
self.blocksPickedUp = 0
self.startThing()
def setup(self):
self.left = Motor(self.tamp, 5, 4)
self.right = Motor(self.tamp, 2, 3)
hugs = Motor(self.tamp, 8, 9)
ir0 = LRIR(self.tamp,14)
ir1 = LRIR(self.tamp,15)
ir2 = LRIR(self.tamp,16)
ir3 = LRIR(self.tamp,17)
self.movement = WallFollow(self.left, self.right, Timer(), ir0, ir1, ir2, ir3)
self.timer = Timer()
self.wintimer = Timer()
def setup(self):
# 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)
# Timer object to moderate checking for intake errors.
self.intakeTimer = Timer()
# Are the intake motors going forward? True if so, False if reversing.
self.intakeDirection = False
# Start the intake motor.
self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
class Simple(Sketch):
def setup(self):
self.motor = Motor(self.tamp, 11, 9)
self.tof1 = TimeOfFlight(self.tamp, 20, 1)
self.tof2 = TimeOfFlight(self.tamp, 22, 2)
self.tof1.enable()
self.tof2.enable()
def loop(self):
sleep(0.01)
print self.tof1.dist, self.tof2.dist
self.motor.write(1, 50)
def setup(self):
timer = Timer()
timeoutTimer = Timer()
leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION,
LEFT_DRIVE_CONTROLLER_PWM)
rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION,
RIGHT_DRIVE_CONTROLLER_PWM)
intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION,
HUGS_MOTOR_CONTROLLER_PWM)
color = Color(self.tamp)
self.check = CheckModule(timer, timeoutTimer, leftMotor, rightMotor,
intakeMotor, color)
self.check.start()
def setup(self):
self.left = Motor(self.tamp, 5, 4)
self.right = Motor(self.tamp, 2, 3)
hugs = Motor(self.tamp, 8, 9)
ir0 = LRIR(self.tamp, 14)
ir1 = LRIR(self.tamp, 15)
ir2 = LRIR(self.tamp, 16)
ir3 = LRIR(self.tamp, 17)
self.movement = WallFollow(self.left, self.right, Timer(), ir0, ir1,
ir2, ir3)
self.timer = Timer()
self.wintimer = Timer()
class TestWallFollow(SyncedSketch):
def setup(self):
self.left = Motor(self.tamp, 5, 4)
self.right = Motor(self.tamp, 2, 3)
hugs = Motor(self.tamp, 8, 9)
ir0 = LRIR(self.tamp,14)
ir1 = LRIR(self.tamp,15)
ir2 = LRIR(self.tamp,16)
ir3 = LRIR(self.tamp,17)
self.movement = WallFollow(self.left, self.right, Timer(), ir0, ir1, ir2, ir3)
self.timer = Timer()
self.wintimer = Timer()
def loop(self):
if self.wintimer.millis() < 19600:
if self.timer.millis() > 100:
self.timer.reset()
print self.movement.distance()
# Intended behavior: bot will follow wall
# IR return distance, 50 speed
self.movement.followWall(self.movement.distance(),-FORWARD_SPEED)
elif self.wintimer.millis() < 19900:
self.left.write( 0, 145)
self.right.write( 1, 145)
else:
self.wintimer.reset()
self.left.write(0,0)
self.right.write(0,0)
def setup(self):
self.timer = Timer()
# 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)
class TestWallFollow(SyncedSketch):
def setup(self):
self.left = Motor(self.tamp, 5, 4)
self.right = Motor(self.tamp, 2, 3)
hugs = Motor(self.tamp, 8, 9)
ir0 = LRIR(self.tamp, 14)
ir1 = LRIR(self.tamp, 15)
ir2 = LRIR(self.tamp, 16)
ir3 = LRIR(self.tamp, 17)
self.movement = WallFollow(self.left, self.right, Timer(), ir0, ir1,
ir2, ir3)
self.timer = Timer()
self.wintimer = Timer()
def loop(self):
if self.wintimer.millis() < 19600:
if self.timer.millis() > 100:
self.timer.reset()
print self.movement.distance()
# Intended behavior: bot will follow wall
# IR return distance, 50 speed
self.movement.followWall(self.movement.distance(),
-FORWARD_SPEED)
elif self.wintimer.millis() < 19900:
self.left.write(0, 145)
self.right.write(1, 145)
else:
self.wintimer.reset()
self.left.write(0, 0)
self.right.write(0, 0)
class MotorWrite(Sketch):
def setup(self):
self.motor = Motor(self.tamp, 2, 3)
self.motor.write(1, 0)
self.delta = 1
self.motorval = 0
self.timer = Timer()
def loop(self):
if self.timer.millis() > 10:
self.timer.reset()
if abs(self.motorval) == 255:
self.delta = -self.delta
self.motorval += self.delta
self.motor.write(self.motorval > 0, abs(self.motorval))
class MotorWrite(Sketch):
def setup(self):
self.motor = Motor(self.tamp, 2, 3)
self.motor.write(1,0)
self.delta = 1
self.motorval = 0
self.timer = Timer()
def loop(self):
if (self.timer.millis() > 10):
self.timer.reset()
if abs(self.motorval) == 255: self.delta = -self.delta
self.motorval += self.delta
self.motor.write(self.motorval>0, abs(self.motorval))
class BeltMove(SyncedSketch):
def setup(self):
# Motor object representing the conveyor belt motor.
self.limSwitch = DigitalInput(self.tamp, 23)
self.conveyorMotor = Motor(self.tamp, 7, 6)
self.timer = Timer()
def loop(self):
if self.timer.millis() > 100:
self.timer.reset()
if self.limSwitch.val:
self.conveyorMotor.write(UP, 0)
self.stop()
else:
self.conveyorMotor.write(DOWN, 80)
def setup(self):
"""One-time method that sets up the robot, like in Arduino"""
# Create the motor and short IR objects
self.lmotor = Motor(self.tamp, *self.LMOTOR_PINS)
self.rmotor = Motor(self.tamp, *self.RMOTOR_PINS)
self.irs = [AnalogInput(self.tamp, p) for p in self.IR_PINS]
# Create a publisher to publish short IR sensor readings
self.ir_pub = rospy.Publisher('ir_read',
kitware.msg.IRRead,
queue_size=10)
# Create a subscriber to listen for drive motor commands
self.drive_sub = rospy.Subscriber('drive_cmd', kitware.msg.DriveCMD,
self.drive_callback)
class BeltMove(SyncedSketch):
def setup(self):
# Motor object representing the conveyor belt motor.
self.limSwitch = DigitalInput(self.tamp, 23)
self.conveyorMotor = Motor(self.tamp, 7, 6)
self.timer = Timer()
def loop(self):
if self.timer.millis() > 100:
self.timer.reset()
if self.limSwitch.val:
self.conveyorMotor.write(UP, 0)
self.stop()
else:
self.conveyorMotor.write(DOWN, 80)
def __init__(self, tamp):
self.tamp = tamp
#motor left
self.motorL = Motor(self.tamp, 2, 4)
self.motorL.write(1, 0)
#motor right
self.motorR = Motor(self.tamp, 1, 3)
self.motorR.write(1, 0)
#arm servo
self.arm = Arm(self.tamp)
#sorting servo
self.sorter = Sorter(self.tamp)
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 #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorL.write(1,0) #motor right self.motorR = Motor(self.tamp,1, 3) self.motorR.write(1,0) #arm servo self.arm = Arm(self.tamp) #sorting servo self.sorter = Sorter(self.tamp)
def setup(self):
# initialize sensors, settings, start timers, etc.
self.gameTimer = Timer()
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)
print "Motors connected."
self.timer = Timer()
self.gyro = Gyro(self.tamp, 9)
print "Gyro connected."
self.theta = self.gyro.val
self.dT = .01
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.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()
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)
print "Motors connected."
self.timer = Timer()
self.gyro = Gyro(self.tamp, 9)
print "Gyro connected."
self.theta = self.gyro.val
self.dT = .01
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.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.steps = 0
# Motor object representing the conveyor belt motor.
self.motor = Motor(self.tamp, 7, 6)
# Encoder object for the conveyor belt motor.
self.encoder = Encoder(self.tamp, 28, 27)
self.timer = Timer()
self.motor.write(DOWN, 75)
def setup(self):
self.encoder = Encoder(self.tamp, *self.encoderPins, continuous=True)
self.motor = Motor(self.tamp, self.motorDir, self.motorPwm)
self.miscTimer = Timer()
self.timer = Timer()
self.rotations = 0
self.lastRot = 0
self.motor.write(0, 100)
def setup(self):
self.motor = Motor(self.tamp, 24, 25)
self.motor.write(1,0)
self.delta = 1
self.motorval = 0
self.timer = Timer()
self.counter = 0
self.spin_forwards = True
def setup(self):
limSwitch = DigitalInput(self.tamp, CONVEYOR_LIMIT_SWITCH)
conveyorMotor = Motor(self.tamp, BELT_MOTOR_CONTROLLER_DIRECTION,
BELT_MOTOR_CONTROLLER_PWM)
conveyorEncoder = Encoder(self.tamp, BELT_MOTOR_ENCODER_YELLOW,
BELT_MOTOR_ENCODER_WHITE)
self.pickup = PickupModule(Timer(), limSwitch, conveyorMotor,
conveyorEncoder)
self.pickup.start()
class EncoderRead(SyncedSketch):
def setup(self):
self.steps = 0
# Motor object representing the conveyor belt motor.
self.motor = Motor(self.tamp, 7, 6)
# Encoder object for the conveyor belt motor.
self.encoder = Encoder(self.tamp, 28, 27)
self.timer = Timer()
self.motor.write(DOWN, 75)
def loop(self):
if self.timer.millis() > 100:
self.timer.reset()
self.steps += 1
print self.steps, self.encoder.val
if self.steps > 25:
self.motor.write(DOWN, 75)
self.stop()
def setup(self):
self.TicpIn = 4480 / 2.875
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
print "initial angle:" + str(self.initAngle)
self.prevGyro = 0
self.drift = -0.02875
self.totalDrift = 0
self.drifts = []
self.PID = PID(5, 4, 0.2)
self.fwdVel = 0
self.timer = Timer()
"""
def setup(self):
self.TicpIn = 4480 / 2.875
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
print "initial angle:" + str(self.initAngle)
self.prevGyro = 0
self.drift = -.02875
self.totalDrift = 0
self.drifts = []
self.PID = PID(5, 4, .2)
self.fwdVel = 0
self.timer = Timer()
'''
class EncoderRead(SyncedSketch):
def setup(self):
self.steps = 0
# Motor object representing the conveyor belt motor.
self.motor = Motor(self.tamp, 7, 6)
# Encoder object for the conveyor belt motor.
self.encoder = Encoder(self.tamp, 28, 27)
self.timer = Timer()
self.motor.write(DOWN, 75)
def loop(self):
if self.timer.millis() > 100:
self.timer.reset()
self.steps += 1
print self.steps, self.encoder.val
if self.steps > 25:
self.motor.write(DOWN, 75)
self.stop()
def setup(self):
# 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)
# Timer object to moderate checking for intake errors.
self.intakeTimer = Timer()
# Are the intake motors going forward? True if so, False if reversing.
self.intakeDirection = False
# Start the intake motor.
self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
def setup(self):
self.timer = Timer()
self.ttt = Timer()
self.timeout = 5000
# Motor object representing the conveyor belt motor.
self.conveyorMotor = Motor(self.tamp, 7, 6)
# Encoder object for the conveyor belt motor.
self.conveyorEncoder = Encoder(self.tamp, 28, 27)
self.blocksPickedUp = 0
self.startThing()
class KitBot(ROSSketch):
"""ROS Node that controls the KitBod via the Teensy and tamproxy"""
# PIN MAPPINGS
LMOTOR_PINS = (2, 3) # DIR, PWM
RMOTOR_PINS = (4, 5) # DIR, PWM
IR_PINS = [14, 15, 16]
def setup(self):
"""One-time method that sets up the robot, like in Arduino"""
# Create the motor and short IR objects
self.lmotor = Motor(self.tamp, *self.LMOTOR_PINS)
self.rmotor = Motor(self.tamp, *self.RMOTOR_PINS)
self.irs = [AnalogInput(self.tamp, p) for p in self.IR_PINS]
# Create a publisher to publish short IR sensor readings
self.ir_pub = rospy.Publisher('ir_read',
kitware.msg.IRRead,
queue_size=10)
# Create a subscriber to listen for drive motor commands
self.drive_sub = rospy.Subscriber('drive_cmd', kitware.msg.DriveCMD,
self.drive_callback)
def loop(self):
"""Method that loops at a fast rate, like in Arduino"""
# Create an IR read message containing the short IR analog values
ir_msg = kitware.msg.IRRead()
ir_msg.data = [ir.val for ir in self.irs]
# Publish the IR readings
self.ir_pub.publish(ir_msg)
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))
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))
class Actuators: def __init__(self,tamp): self.tamp=tamp #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorL.write(1,0) #motor right self.motorR = Motor(self.tamp,1, 3) self.motorR.write(1,0) #arm servo self.arm = Arm(self.tamp) #sorting servo self.sorter = Sorter(self.tamp) def update(self): self.arm.update() self.sorter.update()
class Actuators:
def __init__(self, tamp):
self.tamp = tamp
#motor left
self.motorL = Motor(self.tamp, 2, 4)
self.motorL.write(1, 0)
#motor right
self.motorR = Motor(self.tamp, 1, 3)
self.motorR.write(1, 0)
#arm servo
self.arm = Arm(self.tamp)
#sorting servo
self.sorter = Sorter(self.tamp)
def update(self):
self.arm.update()
self.sorter.update()
class MotorWrite(Sketch):
def setup(self):
self.motor = Motor(self.tamp, 24, 25)
self.motor.write(1,0)
self.delta = 1
self.motorval = 0
self.timer = Timer()
self.counter = 0
self.spin_forwards = True
def loop(self):
if (self.timer.millis() > 10):
self.timer.reset()
# if abs(self.motorval) == 255: self.delta = -self.delta
# self.motorval += self.delta
# self.motor.write(self.motorval>0, abs(self.motorval))
if(self.counter >= 2000 and self.spin_forwards):
self.counter = 0
self.spin_forwards = False
elif(self.counter >= 500 and not self.spin_forwards):
self.counter = 0
self.spin_forwards = True
self.motor.write(self.spin_forwards, 250)
self.counter += 10
class HugTest(SyncedSketch):
def setup(self):
# 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)
# Timer object to moderate checking for intake errors.
self.intakeTimer = Timer()
# Are the intake motors going forward? True if so, False if reversing.
self.intakeDirection = False
# Start the intake motor.
self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
def loop(self):
self.checkForIntakeErrors()
def checkForIntakeErrors(self, checkTime = 1000, reverseTime = 3000):
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 MyRobot(SyncedSketch):
runtime = 300000 #ms
def setup(self):
# initialize sensors, settings, start timers, etc.
self.gameTimer = Timer()
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)
print "Motors connected."
self.timer = Timer()
self.gyro = Gyro(self.tamp, 9)
print "Gyro connected."
self.theta = self.gyro.val
self.dT = .01
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.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:
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
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)
def processOutputs(self, Outputs):
# TODO Missing servo outputs
if (Outputs.driving == True):
self.motorval = 50 #25?
else:
self.motorval = 0
if (Outputs.isDiscardingBlock == True):
self.motorval = -50
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 + 5)
else:
self.PID(self.desiredAngle - 5)
else:
self.PID(self.desiredAngle)
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)))
class PID(SyncedSketch):
def setup(self):
#self.motorLeft = Motor(self.tamp, 21, 20)
#self.motorRight = Motor(self.tamp, 23, 22)
self.motorLeft = Motor(self.tamp, 20, 21)
self.motorRight = Motor(self.tamp, 22, 23)
self.motorLeft.write(1,0)
self.motorRight.write(1,0)
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)
# TODO: set encoder to 0
self.timer = Timer()
self.gyro = Gyro(self.tamp, 10)
self.P = 5
self.I = 0 # should be negative
self.D = 0
self.dT = .03
self.motorval = 25 #50
self.last_diff = 0
self.integral = 0
self.desired = self.gyro.val #+ 45 # to drive in a straight line
self.encoderLeft.start_continuous()
self.encoderRight.start_continuous()
def loop(self):
# Set encoder to 0 after turning.
# To turn in place, set bias (i.e. motorval to 0)
if self.timer.millis() > self.dT*1000:
self.timer.reset()
gyroVal = self.gyro.val
print gyroVal, self.gyro.stxatus
# TODO: encoderVal
estimated = gyroVal # TODO: calculate estimated with encoder
diff = self.desired-estimated
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
power = min(40, power)
self.motorLeft.write((self.motorval + power) > 0, min(255, abs(self.motorval + power)))
self.motorRight.write((self.motorval - power) > 0, min(255, abs(self.motorval - power)))
print "EncoderLeft: " + str(self.encoderLeft.val)
print "EncoderRight: " + str(self.encoderRight.val)
def stop(self):
self.motorLeft.write(1, 0)
self.motorRight.write(1, 0)
# print self.motorval
super(PID,self).stop()
self.tamp.clear_devices();
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 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 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.
class TestBelt(SyncedSketch):
def setup(self):
self.timer = Timer()
self.ttt = Timer()
self.timeout = 5000
# Motor object representing the conveyor belt motor.
self.conveyorMotor = Motor(self.tamp, 7, 6)
# Encoder object for the conveyor belt motor.
self.conveyorEncoder = Encoder(self.tamp, 28, 27)
self.blocksPickedUp = 0
self.startThing()
def loop(self):
if self.ttt.millis() > 100:
print self.conveyorEncoder.val
self.ttt.reset()
self.runThing()
## Set up the beginning of the pickup process.
def startThing(self):
self.conveyorEncoder.write(0)
self.conveyorMotor.write(UP, CONVEYOR_POWER)
self.timer.reset()
## Pick up a block from the block capture mechanism.
#
# Move the conveyor belt upwards until the encoders indicate that
# the block has moved far enough. Then move the conveyor belt back.
#
# @return The value of the next module to return to.
def runThing(self):
# Allow timeout.
if self.timer.millis() > self.timeout:
print "Timed out from PICKUP to FIND"
self.stop()
encval = self.conveyorEncoder.val
# Move up the conveyor belt until it hits the encoder limit.
if encval > CONVEYOR_ENCODER_LIMIT:
self.conveyorMotor.write(DOWN, CONVEYOR_POWER)
else:
self.conveyorMotor.write(UP, CONVEYOR_POWER)
# Stop the motor when it gets to the bottom.
if encval < 0 and self.timer.millis() > 1000:
self.conveyorMotor.write(False, 0)
self.blocksPickedUp += 1
# Switch modules
if self.blocksPickedUp >= 4:
print "Going from PICKUP to DROPOFF"
self.stop()
else:
print "Going from PICKUP to FIND"
self.stop()
return
def setup(self):
self.motor = Motor(self.tamp, 2, 3)
self.motor.write(1, 0)
self.delta = 1
self.motorval = 0
self.timer = Timer()
def setup(self):
# Motor object representing the conveyor belt motor.
self.conveyorMotor = Motor(self.tamp, 7, 6)
#self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION, HUGS_MOTOR_CONTROLLER_PWM)
self.start()
