Example #1
0
    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
Example #2
0
class EncoderWrap:
    def __init__(self, tamp):
        self.tamp = tamp
        self.encoderL = Encoder(self.tamp, 22, 23)
        self.encoderR = Encoder(self.tamp, 21, 20)
        self.isRobotMoving = False
        self.prevEncoderL = 0
        self.pervEncoderR = 0
        self.NOT_MOVING_EPSILON = 62  #5 degrees of change

    def resetEncoders(self):
        self.encoderL.write(0)
        self.encoderR.write(0)

    def getDistanceTraveled(self):
        avg = (self.encoderL.val + self.encoderR.val) / 2
        return avg / 360.

    def update(self):
        if (abs(self.encoderL.val - self.prevEncoderL) <
                self.NOT_MOVING_EPSILON
                and abs(self.encoderL.val - self.prevEncoderL) <
                self.NOT_MOVING_EPSILON):
            self.isRobotMoving = False
        else:
            self.isRobotMoving = True
        self.prevEncoderL = self.encoderL.val
        self.prevEncoderR = self.encoderR.val
Example #3
0
 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()
Example #4
0
    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()
Example #5
0
class EncoderWrap:
	def __init__(self,tamp):
		self.tamp=tamp
		self.encoderL = Encoder(self.tamp, 22, 23)
		self.encoderR = Encoder(self.tamp, 21, 20)
		self.isRobotMoving=False
		self.prevEncoderL=0
		self.pervEncoderR=0
		self.NOT_MOVING_EPSILON=62 #5 degrees of change

	def resetEncoders(self):
		self.encoderL.write(0)
		self.encoderR.write(0)

	def getDistanceTraveled(self):
		avg = (self.encoderL.val+self.encoderR.val)/2
		return avg/360.

	def update(self):
		if(abs(self.encoderL.val-self.prevEncoderL)<self.NOT_MOVING_EPSILON and abs(self.encoderL.val-self.prevEncoderL)<self.NOT_MOVING_EPSILON):
			self.isRobotMoving=False
		else:
			self.isRobotMoving=True
		self.prevEncoderL=self.encoderL.val
		self.prevEncoderR=self.encoderR.val
Example #6
0
    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()
Example #7
0
 def __init__(self, tamp):
     self.tamp = tamp
     self.encoderL = Encoder(self.tamp, 22, 23)
     self.encoderR = Encoder(self.tamp, 21, 20)
     self.isRobotMoving = False
     self.prevEncoderL = 0
     self.pervEncoderR = 0
     self.NOT_MOVING_EPSILON = 62  #5 degrees of change
Example #8
0
	def __init__(self,tamp):
		self.tamp=tamp
		self.encoderL = Encoder(self.tamp, 22, 23)
		self.encoderR = Encoder(self.tamp, 21, 20)
		self.isRobotMoving=False
		self.prevEncoderL=0
		self.pervEncoderR=0
		self.NOT_MOVING_EPSILON=62 #5 degrees of change
Example #9
0
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()
Example #10
0
    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()
Example #11
0
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();
Example #12
0
 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)
Example #13
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)
Example #14
0
 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)
Example #15
0
 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)
Example #16
0
 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()
Example #17
0
    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()
        '''
Example #18
0
 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)
Example #19
0
    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()
Example #20
0
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)
Example #21
0
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)
Example #22
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()
Example #23
0
  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."
Example #24
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()
Example #25
0
from sw import pins
from tamproxy.devices import Odometer
from tamproxy.devices import Gyro
from tamproxy.devices import Encoder
from tamproxy import TAMProxy

import time

if __name__ == "__main__":
    tamp = TAMProxy()
    gyro = Gyro(tamp, pins.gyro_cs, integrate=False)
    lEnc = Encoder(tamp, pins.l_encoder_a, pins.l_encoder_b, continuous = False)
    rEnc = Encoder(tamp, pins.r_encoder_a, pins.r_encoder_b, continuous = False)
    odo = Odometer(tamp, lEnc, rEnc, gyro, 0.0)
    
    while True:
        odo.update()
        lEnc.update()
        rEnc.update()
        print odo.val
        print lEnc.val
        print rEnc.val
        time.sleep(0.05)
Example #26
0
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
Example #27
0
    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
Example #28
0
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()

        # 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 loop(self):
        if self.timer.millis() > self.dT * 1000:
            # print("GameTimer:", 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__
            if self.goIntoTimeoutState:
                self.state = TimeoutState()
            elif self.timeoutCounter == 3000:
                self.state = ExploreState()
                self.timeoutCounter = 0
                self.goIntoTimeoutState = False
            else:
                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 = (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)
        blocks = []  #CalculateBlocks(); #what should CalculateBlocks return?
        leftIR = self.leftIR.val
        rightIR = self.rightIR.val
        frontLeftIR = self.frontLeftIR.val
        frontRightIR = self.frontRightIR.val
        # if len(self.leftIRVals) == 100:
        #   print("Reading from averaged values")
        #   self.leftIRVals.append(leftIR)
        #   self.leftIRVals.popleft()
        #   leftIR = sum(leftIRVals)/100
        # if len(self.rightIRVals) == 100:
        #   self.rightIRVals.append(rightIR)
        #   self.rightIRVals.popleft()
        #   rightIR = sum(self.rightIRVals)/100
        # if len(self.frontLeftIRVals) == 100:
        #   self.frontLeftIRVals.append(frontLeftIR)
        #   self.frontLeftIRVals.popleft()
        #   frontLeftIR = sum(self.frontLeftIRVals)/100
        # if len(self.frontRightIRVals) == 100:
        #   self.frontRightIRVals.append(frontRightIR)
        #   self.frontRightIRVals.popleft()
        #   frontRightIR = sum(self.frontRightIRVals)/100

        ret, frame = self.cam.read()
        img = cv2.resize(frame,
                         None,
                         fx=0.25,
                         fy=0.25,
                         interpolation=cv2.INTER_AREA)
        print("VideoFrame captured: ", ret)

        return Inputs(distance_traveled, self.gyro.val, frontRightIR,
                      frontLeftIR, leftIR, rightIR,
                      self.finishedCollectingBlock, img)
        # self.leftIR.val, self.rightIR.val, self.color.r, self.color.g, self.color.b)

        # distance_traveled, theta, frontRightIR, frontLeftIR, leftIR, rightIR

    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)  # originally = 5
            else:
                self.PID(self.desiredAngle - 3)  # originally = 5
        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
        print(derivative)  # check this for timeout?
        # if derivative > x:
        #   self.goIntoTimeoutMode = True
        power = self.P * diff + self.I * self.integral + self.D * derivative  # NOTE: Cap self.D*derivative, use as timeout
        # print("motorLeft: ", min(255, abs(self.motorval + power)))
        # print("motorRight: ", min(255, abs(self.motorval - 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 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))
Example #29
0
    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."
Example #30
0
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)
Example #31
0
    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()
Example #32
0
    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)
Example #33
0
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
Example #34
0
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)
Example #35
0
    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
Example #36
0
    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
Example #37
0
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
Example #38
0
 def setup(self):
     self.encoder = Encoder(self.tamp, *self.pins, continuous=True)
     self.timer = Timer()
Example #39
0
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()

    # 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 loop(self):
    if self.timer.millis() > self.dT*1000:
      # print("GameTimer:", 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__
      if self.goIntoTimeoutState:
        self.state = TimeoutState()
      elif self.timeoutCounter == 3000:
        self.state = ExploreState()
        self.timeoutCounter = 0
        self.goIntoTimeoutState = False
      else:
        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 = (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)
    blocks = [] #CalculateBlocks(); #what should CalculateBlocks return?
    leftIR = self.leftIR.val
    rightIR = self.rightIR.val
    frontLeftIR = self.frontLeftIR.val
    frontRightIR = self.frontRightIR.val
    # if len(self.leftIRVals) == 100:
    #   print("Reading from averaged values")
    #   self.leftIRVals.append(leftIR)
    #   self.leftIRVals.popleft()
    #   leftIR = sum(leftIRVals)/100
    # if len(self.rightIRVals) == 100:
    #   self.rightIRVals.append(rightIR)
    #   self.rightIRVals.popleft()
    #   rightIR = sum(self.rightIRVals)/100
    # if len(self.frontLeftIRVals) == 100:
    #   self.frontLeftIRVals.append(frontLeftIR)
    #   self.frontLeftIRVals.popleft()
    #   frontLeftIR = sum(self.frontLeftIRVals)/100
    # if len(self.frontRightIRVals) == 100:
    #   self.frontRightIRVals.append(frontRightIR)
    #   self.frontRightIRVals.popleft()
    #   frontRightIR = sum(self.frontRightIRVals)/100

    ret, frame = self.cam.read()
    img = cv2.resize(frame,None,fx=0.25, fy=0.25, interpolation = cv2.INTER_AREA)
    print("VideoFrame captured: ", ret)

    return Inputs(distance_traveled, self.gyro.val, frontRightIR, frontLeftIR, leftIR, rightIR, self.finishedCollectingBlock, img)
      # self.leftIR.val, self.rightIR.val, self.color.r, self.color.g, self.color.b)
     
    # distance_traveled, theta, frontRightIR, frontLeftIR, leftIR, rightIR

  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) # originally = 5
      else:
        self.PID(self.desiredAngle - 3 ) # originally = 5
    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
    print(derivative) # check this for timeout?
    # if derivative > x:
    #   self.goIntoTimeoutMode = True
    power = self.P*diff + self.I*self.integral + self.D*derivative # NOTE: Cap self.D*derivative, use as timeout
    # print("motorLeft: ", min(255, abs(self.motorval + power)))
    # print("motorRight: ", min(255, abs(self.motorval - 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 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))