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 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
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.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 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
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 __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 __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
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.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 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): # 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() # 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)
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.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): 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()
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 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 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)
def setup(self): self.ttt = Timer() self.timer = Timer() self.loopTimer = Timer() self.servo = Servo(self.tamp, SERVO_PIN) self.motorRight = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION, RIGHT_DRIVE_CONTROLLER_PWM) self.motorLeft = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION, LEFT_DRIVE_CONTROLLER_PWM) self.encoder = Encoder(self.tamp, RIGHT_DRIVE_ENCODER_YELLOW, RIGHT_DRIVE_ENCODER_WHITE) self.dropoff = DropoffModule(self.timer, self.servo, self.motorRight, self.motorLeft, self.encoder) self.dropoff.start()
def setup(self): # initialize sensors, settings, start timers, etc. self.gameTimer = Timer() # Pins (7,22); (3,23); (0,21) work. # Problem was with the Double Motor controller. self.motorGripper = Motor(self.tamp, 23, 3) self.motorLeft = Motor(self.tamp, 7, 22) self.motorRight = Motor(self.tamp, 0, 21) # good self.motorval = 0 self.motorLeft.write(1,0) self.motorRight.write(1,0) self.motorGripper.write(1,0) self.currentGripperLevel = 2 print "Motors connected." self.servoDoor = Servo(self.tamp, 20) self.servovalDoor = self.DOOR_CLOSE_POS self.servoDoor.write(self.DOOR_CLOSE_POS) self.timerDoor = Timer() self.servoGripper = Servo(self.tamp, 10) self.servovalGripper = self.GRIPPER_CLOSE_POS self.servoGripper.write(self.servovalGripper) self.timerGripper = Timer() print "Servos connected." left_pins = 6,5 right_pins = 3,4 # Encoder doesn't work when after gyro self.encoderLeft = Encoder(self.tamp, 6,5, continuous=False) self.encoderRight = Encoder(self.tamp, 3,4, continuous=True) print "Encoders connected." # TODO: set encoder to 0 self.timer = Timer() self.gyro = Gyro(self.tamp, 9) print "Gyro connected." self.theta = self.gyro.val self.dT = .01 self.cam = cv2.VideoCapture(0) print "Camera connected." # self.color = Color(self.tamp, # integrationTime=Color.INTEGRATION_TIME_101MS, # gain=Color.GAIN_1X) self.encoderLeft.start_continuous() self.encoderRight.start_continuous() frontLeftIR_pin = 14 self.frontLeftIR = AnalogInput(self.tamp, frontLeftIR_pin) frontRightIR_pin = 15 self.frontRightIR = AnalogInput(self.tamp, frontRightIR_pin) leftIR_pin = 16 self.leftIR = AnalogInput(self.tamp, leftIR_pin) rightIR_pin = 17 self.rightIR = AnalogInput(self.tamp, rightIR_pin) # Initialize PID Values self.P = 10 self.I = 5 self.D = 5 self.last_diff = 0 self.integral = 0 self.desiredAngle = self.theta self.finishedCollectingBlock = False self.finishedDiscardingBlock = False self.timer = Timer() self.state = ExploreState() # self.state = CollectBlockState() self.blocksCollected = 0 self.leftIRVals = deque([]) self.rightIRVals = deque([]) self.frontRightIRVals = deque([]) self.frontLeftIRVals = deque([]) self.goIntoTimeoutState = False self.timeoutCounter = 0 # Starts the robot print "Robot setup complete."
def setup(self): #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()
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)
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): #################### #### EE SETUP #### #################### # Motor object representing the left motor. self.leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION, LEFT_DRIVE_CONTROLLER_PWM) # Encoder object for the left motor. self.leftEncoder = Encoder(self.tamp, LEFT_DRIVE_ENCODER_YELLOW, LEFT_DRIVE_ENCODER_WHITE) # Motor object representing the right motor. self.rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION, RIGHT_DRIVE_CONTROLLER_PWM) # Encoder object for the right motor. self.rightEncoder = Encoder(self.tamp, RIGHT_DRIVE_ENCODER_YELLOW, RIGHT_DRIVE_ENCODER_WHITE) # Motor object representing the intake mechanism motors. self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION, HUGS_MOTOR_CONTROLLER_PWM) # Encoder object for the intake motor. self.intakeEncoder = Encoder(self.tamp, HUGS_MOTOR_ENCODER_YELLOW, HUGS_MOTOR_ENCODER_WHITE) # Motor object representing the conveyor belt motor. self.conveyorMotor = Motor(self.tamp, BELT_MOTOR_CONTROLLER_DIRECTION, BELT_MOTOR_CONTROLLER_PWM) # Encoder object for the conveyor belt motor. self.conveyorEncoder = Encoder(self.tamp, BELT_MOTOR_ENCODER_YELLOW, BELT_MOTOR_ENCODER_WHITE) # Long range IR sensors self.irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL) self.irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR) self.irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL) self.irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR) # Color sensor self.color = Color(self.tamp) # Limit switches self.conveyorLimSwitch = DigitalInput(self.tamp, CONVEYOR_LIMIT_SWITCH) self.blockLimSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH) # Servo controlling the door of the collection chamber. self.backDoorServo = Servo(self.tamp, SERVO_PIN) # Make sure the door is closed self.backDoorServo.write(SERVO_CLOSE) # The switch that tells the program that the competition has started self.competitionModeSwitch = DigitalInput(self.tamp, COMPETITION_MODE) ################################# #### INTERNAL MODULE SETUP #### ################################# # Timer object to moderate checking for intake errors. self.intakeTimer = Timer() # Are the intake motors reversing? True if so, False if going forwards. self.intakeDirection = False # Start the intake motor. self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) # Wall Follow object self.wallFollow = WallFollow(self.leftMotor, self.rightMotor, Timer(), self.irFL, self.irFR, self.irBL, self.irBR) # A timer to make sure timesteps are only 10 times/second self.timestepTimer = Timer() # Timer object describing how long the current module has been running. self.moduleTimer = Timer() # Timer for the whole game self.gameTimer = Timer() # Runs the PICKUP process self.pickup = PickupModule(self.moduleTimer, self.conveyorLimSwitch, self.conveyorMotor, self.conveyorEncoder) # Runs the DROPOFF process self.dropoff = DropoffModule(self.moduleTimer, self.backDoorServo, self.rightMotor, self.leftMotor, self.rightEncoder) # Runs the FOLLOW process TODO: Fix forward to actually mean forward. self.follow = FollowModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.wallFollow, FORWARD_SPEED, self.blockLimSwitch) # Runs the CHECK process. TODO: pass in proper timers. self.check = CheckModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.color) # Waits for the game to start self.off = OffModule(self.gameTimer, self.competitionModeSwitch) # Describes which stage of the program is running. self.module = MODULE_OFF
class 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))
def setup(self): # initialize sensors, settings, start timers, etc. self.gameTimer = Timer() # Pins (7,22); (3,23); (0,21) work. # Problem was with the Double Motor controller. self.motorGripper = Motor(self.tamp, 23, 3) self.motorLeft = Motor(self.tamp, 7, 22) self.motorRight = Motor(self.tamp, 0, 21) # good self.motorval = 0 self.motorLeft.write(1, 0) self.motorRight.write(1, 0) self.motorGripper.write(1, 0) self.currentGripperLevel = 2 print "Motors connected." self.servoDoor = Servo(self.tamp, 20) self.servovalDoor = self.DOOR_CLOSE_POS self.servoDoor.write(self.DOOR_CLOSE_POS) self.timerDoor = Timer() self.servoGripper = Servo(self.tamp, 10) self.servovalGripper = self.GRIPPER_CLOSE_POS self.servoGripper.write(self.servovalGripper) self.timerGripper = Timer() print "Servos connected." left_pins = 6, 5 right_pins = 3, 4 # Encoder doesn't work when after gyro self.encoderLeft = Encoder(self.tamp, 6, 5, continuous=False) self.encoderRight = Encoder(self.tamp, 3, 4, continuous=True) print "Encoders connected." # TODO: set encoder to 0 self.timer = Timer() self.gyro = Gyro(self.tamp, 9) print "Gyro connected." self.theta = self.gyro.val self.dT = .01 self.cam = cv2.VideoCapture(0) print "Camera connected." # self.color = Color(self.tamp, # integrationTime=Color.INTEGRATION_TIME_101MS, # gain=Color.GAIN_1X) self.encoderLeft.start_continuous() self.encoderRight.start_continuous() frontLeftIR_pin = 14 self.frontLeftIR = AnalogInput(self.tamp, frontLeftIR_pin) frontRightIR_pin = 15 self.frontRightIR = AnalogInput(self.tamp, frontRightIR_pin) leftIR_pin = 16 self.leftIR = AnalogInput(self.tamp, leftIR_pin) rightIR_pin = 17 self.rightIR = AnalogInput(self.tamp, rightIR_pin) # Initialize PID Values self.P = 10 self.I = 5 self.D = 5 self.last_diff = 0 self.integral = 0 self.desiredAngle = self.theta self.finishedCollectingBlock = False self.finishedDiscardingBlock = False self.timer = Timer() self.state = ExploreState() # self.state = CollectBlockState() self.blocksCollected = 0 self.leftIRVals = deque([]) self.rightIRVals = deque([]) self.frontRightIRVals = deque([]) self.frontLeftIRVals = deque([]) self.goIntoTimeoutState = False self.timeoutCounter = 0 # Starts the robot print "Robot setup complete."
class Robot(SyncedSketch): def setup(self): #################### #### EE SETUP #### #################### # Motor object representing the left motor. self.leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION, LEFT_DRIVE_CONTROLLER_PWM) # Encoder object for the left motor. self.leftEncoder = Encoder(self.tamp, LEFT_DRIVE_ENCODER_YELLOW, LEFT_DRIVE_ENCODER_WHITE) # Motor object representing the right motor. self.rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION, RIGHT_DRIVE_CONTROLLER_PWM) # Encoder object for the right motor. self.rightEncoder = Encoder(self.tamp, RIGHT_DRIVE_ENCODER_YELLOW, RIGHT_DRIVE_ENCODER_WHITE) # Motor object representing the intake mechanism motors. self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION, HUGS_MOTOR_CONTROLLER_PWM) # Encoder object for the intake motor. self.intakeEncoder = Encoder(self.tamp, HUGS_MOTOR_ENCODER_YELLOW, HUGS_MOTOR_ENCODER_WHITE) # Motor object representing the conveyor belt motor. self.conveyorMotor = Motor(self.tamp, BELT_MOTOR_CONTROLLER_DIRECTION, BELT_MOTOR_CONTROLLER_PWM) # Encoder object for the conveyor belt motor. self.conveyorEncoder = Encoder(self.tamp, BELT_MOTOR_ENCODER_YELLOW, BELT_MOTOR_ENCODER_WHITE) # Long range IR sensors self.irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL) self.irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR) self.irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL) self.irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR) # Color sensor self.color = Color(self.tamp) # Limit switches self.conveyorLimSwitch = DigitalInput(self.tamp, CONVEYOR_LIMIT_SWITCH) self.blockLimSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH) # Servo controlling the door of the collection chamber. self.backDoorServo = Servo(self.tamp, SERVO_PIN) # Make sure the door is closed self.backDoorServo.write(SERVO_CLOSE) # The switch that tells the program that the competition has started self.competitionModeSwitch = DigitalInput(self.tamp, COMPETITION_MODE) ################################# #### INTERNAL MODULE SETUP #### ################################# # Timer object to moderate checking for intake errors. self.intakeTimer = Timer() # Are the intake motors reversing? True if so, False if going forwards. self.intakeDirection = False # Start the intake motor. self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) # Wall Follow object self.wallFollow = WallFollow(self.leftMotor, self.rightMotor, Timer(), self.irFL, self.irFR, self.irBL, self.irBR) # A timer to make sure timesteps are only 10 times/second self.timestepTimer = Timer() # Timer object describing how long the current module has been running. self.moduleTimer = Timer() # Timer for the whole game self.gameTimer = Timer() # Runs the PICKUP process self.pickup = PickupModule(self.moduleTimer, self.conveyorLimSwitch, self.conveyorMotor, self.conveyorEncoder) # Runs the DROPOFF process self.dropoff = DropoffModule(self.moduleTimer, self.backDoorServo, self.rightMotor, self.leftMotor, self.rightEncoder) # Runs the FOLLOW process TODO: Fix forward to actually mean forward. self.follow = FollowModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.wallFollow, FORWARD_SPEED, self.blockLimSwitch) # Runs the CHECK process. TODO: pass in proper timers. self.check = CheckModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.color) # Waits for the game to start self.off = OffModule(self.gameTimer, self.competitionModeSwitch) # Describes which stage of the program is running. self.module = MODULE_OFF def loop(self): if self.timestepTimer.millis() > 100: self.timestepTimer.reset() #print "Module Number", self.module state = -1 if self.module == MODULE_OFF: state = self.off.run() elif self.module == MODULE_CHECK: state = self.check.run() elif self.module == MODULE_PICKUP: state = self.pickup.run() elif self.module == MODULE_DROPOFF: self.module = MODULE_FOLLOW state = self.follow.run() elif self.module == MODULE_FOLLOW: state = self.follow.run() else: print "Attempting to run nonexistent module" self.stop() self.updateState(state) if self.gameTimer.millis() % 10000 == 0: print "Game Time: ", self.gameTimer.millis() if self.gameTimer.millis() > GAME_LENGTH - 500: self.backDoorServo.write(SERVO_OPEN) self.leftMotor.write(0, 0) self.rightMotor.write(0, 0) if self.gameTimer.millis() > GAME_LENGTH: self.module = MODULE_END self.stop() return ## Switch module if necessary. def updateState(self, module): if self.module == module: return elif module == MODULE_OFF: self.off.start() self.module = MODULE_OFF return elif module == MODULE_CHECK: self.check.start() self.module = MODULE_CHECK return elif module == MODULE_PICKUP: self.pickup.start() self.module = MODULE_PICKUP return elif module == MODULE_DROPOFF: self.dropoff.start() self.module = MODULE_DROPOFF return elif module == MODULE_FOLLOW: self.follow.start() self.module = MODULE_FOLLOW return else: print "Attempting to switch to nonexistent module" self.stop() ## Make sure that the intake motor does not stall. # If so, reverse the intake motors. # # @param checkTime Time in ms between checking stalls. # @param reverseTime Time in ms that the intake motors will reverse if needed. def checkForIntakeErrors(self, checkTime=100, reverseTime=800): if self.intakeDirection: # We are moving forward. if self.intakeTimer.millis() > checkTime: self.intakeTimer.reset() if self.intakeEncoder.val < INTAKE_ENCODER_LIMIT: # if we're stalled self.intakeDirection = True self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) else: # if we're not stalled self.intakeEncoder.write(0) else: # We are reversing the motors. if self.intakeTimer.millis() > reverseTime: self.intakeTimer.reset() self.intakeDirection = False self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) self.intakeEncoder.write(0) self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
def setup(self): self.TicpIn = 4480/2.875 self.ir_array = Ir_array(self.tamp, 16, 15, 14, 40, 11, 10) self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.uIR = DigitalInput(self.tamp, 17) self.uIR.val = 1 self.servo = Servo(self.tamp, 9) self.servo.bottom = 0 self.servo.top = 200 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.sorter = Servo(self.tamp, 5) self.sorter.center = 90 self.sorter.right = 25 self.sorter.left = 165 self.sorter.speed = 15 self.sorter.write(self.sorter.center) self.sorterval = self.sorter.center self.dsorter = 0 self.encoderL = Encoder(self.tamp, 22, 23) self.encoderR = Encoder(self.tamp, 21, 20) self.init_time = time.time() # self.encoderL = Encoder(self.tamp, 23, 22) # self.encoderR = Encoder(self.tamp, 21, 20) #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorLdrive = 0 self.motorL.write(1,0) #self.deltaL = 1 #self.motorvalL = 0 #motor right self.motorR = Motor(self.tamp,1, 3) self.motorRdrive = 0 self.motorR.write(1,0) #self.deltaR = 1 #self.motorvalR = 0 #gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val self.newAngle = 0 self.blockAngle = 0 self.blockDistance = 0 # print "initial angle:"+str(self.initAngle) self.Follow = 'Right' self.IRs = {'Left': [0, 1, 2], 'Right': [5, 4, 3]} self.prevGyro = 0 self.drift = -.02875 self.totalDrift = 0 self.drifts = [] self.PID=PID(.5, 1, 0.15) self.IRPID = PID(10, 5, .15) self.fwdVel = 30 self.turnVel = 0 self.MoveArm, self.Top, self.Bottom = False, 0, 0 self.Sort = 0 self.SortPause = 0 self.State = 1 self.timer = Timer()
def setup(self): self.init_time = time.time() self.functions = {} self.servo = Servo(self.tamp,self.arm_pin) # self.servo.write(20) # self.servoval = 20 # self.delta = 0 self.functions['servoARM'] = lambda angle: self.servo.write(angle) self.sorter = Servo(self.tamp, self.sorter_pin) # self.sorter.center = 98 # self.sorter.right = 20 # self.sorter.left = 170 # self.sorter.speed = 30 self.functions['servoSORT'] = lambda angle: self.sorter.write(angle) self.encoderL = Encoder(self.tamp, *self.Lencoder_pins) self.encoderR = Encoder(self.tamp, *self.Rencoder_pins) #motor left self.motorL = Motor(self.tamp, *self.Lmotor_pins) self.motorLdrive = 0 self.motorL.write(1,0) #self.deltaL = 1 #self.motorvalL = 0 self.functions['left'] = lambda speed: self.motorL.write(speed<0, abs(speed)) #motor right self.motorR = Motor(self.tamp, *self.Rmoter_pins) self.motorRdrive = 0 self.motorR.write(1,0) #self.deltaR = 1 #self.motorvalR = 0 self.functions['right'] = lambda speed: self.motorR.write(speed<0, abs(speed)) #gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val print "initial angle:"+str(self.initAngle) self.prevGyro = 0 self.drift = -.02875 self.totalDrift = 0 self.drifts = [] self.PID=PID(2,1,0.15) self.fwdVel = 0 self.turnVel = 0 self.maxVel = 40 self.Distance = 0 self.MoveArm, self.Top, self.Bottom = False, 0, 0 self.Sort = 0 self.SortPause = 0 self.timer = Timer() #connect to pipe pipe_path = "./robot" try: os.mkfifo(pipe_path) except OSError: print "error" self.pipe_fd = os.open(pipe_path, os.O_RDONLY | os.O_NONBLOCK)
class SortBlocks(SyncedSketch): pins = 31, 32 def setup(self): self.LEFT_TOWER = 85 self.RIGHT_TOWER = 24 self.CENTER = 53 self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.servo = Servo(self.tamp, 23) #pin TBD self.red_detected = False self.green_detected = False self.servo.write(self.CENTER) self.counter = 0 self.servo_bottom = Servo(self.tamp,22) self.servo_bottom.write(34) self.motor = Motor(self.tamp, 24, 25) self.motor.write(True, 250) self.jammed = False self.conveyor_encoder = Encoder(self.tamp, *self.pins, continuous=True) self.prev_encoder_value = 0 self.conveyor_encoder.update() self.timer = Timer() self.sorter_delta = 50 def loop(self): if self.timer.millis() > 300: self.timer.reset() #self.conveyor_encoder.update() encoder_value = self.conveyor_encoder.val #print self.prev_encoder_value #print encoder_value if(encoder_value == self.prev_encoder_value): self.jammed = True else: self.jammed = False self.prev_encoder_value = encoder_value self.motor.write(not self.jammed,250) # print self.color.r, self.color.g, self.color.b, self.color.c # print self.color.colorTemp, self.color.lux #if not self.red_detected and not self.green_detected and self.color.r > 1.3 * self.color.g and self.color.r > 1.3 * self.color.b: detect_red = self.color.r > 1.3*self.color.g detect_green = self.color.g > 1.3*self.color.r sum_val = self.color.r+self.color.g+self.color.b if detect_red and sum_val > 300: self.servo.write(self.LEFT_TOWER) self.counter = 0 #elif not self.red_detected and not self.green_detected and self.color.g > 1.3 * self.color.r and self.color.g > 1.3 * self.color.b: elif detect_green and sum_val > 300: self.servo.write(self.RIGHT_TOWER) self.counter = 0 elif self.counter > 400: self.servo.write(self.CENTER) # else: # self.servo.write(self.CENTER + self.sorter_delta) # self.sorter_delta *= -1 self.counter += 100
class Robot(SyncedSketch): def setup(self): #################### #### EE SETUP #### #################### # Motor object representing the left motor. self.leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION, LEFT_DRIVE_CONTROLLER_PWM) # Encoder object for the left motor. self.leftEncoder = Encoder(self.tamp, LEFT_DRIVE_ENCODER_YELLOW, LEFT_DRIVE_ENCODER_WHITE) # Motor object representing the right motor. self.rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION, RIGHT_DRIVE_CONTROLLER_PWM) # Encoder object for the right motor. self.rightEncoder = Encoder(self.tamp, RIGHT_DRIVE_ENCODER_YELLOW, RIGHT_DRIVE_ENCODER_WHITE) # Motor object representing the intake mechanism motors. self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION, HUGS_MOTOR_CONTROLLER_PWM) # Encoder object for the intake motor. self.intakeEncoder = Encoder(self.tamp, HUGS_MOTOR_ENCODER_YELLOW, HUGS_MOTOR_ENCODER_WHITE) # Motor object representing the conveyor belt motor. self.conveyorMotor = Motor(self.tamp, BELT_MOTOR_CONTROLLER_DIRECTION, BELT_MOTOR_CONTROLLER_PWM) # Encoder object for the conveyor belt motor. self.conveyorEncoder = Encoder(self.tamp, BELT_MOTOR_ENCODER_YELLOW, BELT_MOTOR_ENCODER_WHITE) # Long range IR sensors self.irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL) self.irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR) self.irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL) self.irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR) # Color sensor self.color = Color(self.tamp) # Limit switches self.conveyorLimSwitch = DigitalInput(self.tamp, CONVEYOR_LIMIT_SWITCH) self.blockLimSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH) # Servo controlling the door of the collection chamber. self.backDoorServo = Servo(self.tamp, SERVO_PIN) # Make sure the door is closed self.backDoorServo.write(SERVO_CLOSE) # The switch that tells the program that the competition has started self.competitionModeSwitch = DigitalInput(self.tamp, COMPETITION_MODE) ################################# #### INTERNAL MODULE SETUP #### ################################# # Timer object to moderate checking for intake errors. self.intakeTimer = Timer() # Are the intake motors reversing? True if so, False if going forwards. self.intakeDirection = False # Start the intake motor. self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) # Wall Follow object self.wallFollow = WallFollow(self.leftMotor, self.rightMotor, Timer(), self.irFL, self.irFR, self.irBL, self.irBR) # A timer to make sure timesteps are only 10 times/second self.timestepTimer = Timer() # Timer object describing how long the current module has been running. self.moduleTimer = Timer() # Timer for the whole game self.gameTimer = Timer() # Runs the PICKUP process self.pickup = PickupModule(self.moduleTimer, self.conveyorLimSwitch, self.conveyorMotor, self.conveyorEncoder) # Runs the DROPOFF process self.dropoff = DropoffModule(self.moduleTimer, self.backDoorServo, self.rightMotor, self.leftMotor, self.rightEncoder) # Runs the FOLLOW process TODO: Fix forward to actually mean forward. self.follow = FollowModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.wallFollow, FORWARD_SPEED, self.blockLimSwitch) # Runs the CHECK process. TODO: pass in proper timers. self.check = CheckModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.color) # Waits for the game to start self.off = OffModule(self.gameTimer, self.competitionModeSwitch) # Describes which stage of the program is running. self.module = MODULE_OFF def loop(self): if self.timestepTimer.millis() > 100: self.timestepTimer.reset() #print "Module Number", self.module state = -1 if self.module == MODULE_OFF: state = self.off.run() elif self.module == MODULE_CHECK: state = self.check.run() elif self.module == MODULE_PICKUP: state = self.pickup.run() elif self.module == MODULE_DROPOFF: self.module = MODULE_FOLLOW state = self.follow.run() elif self.module == MODULE_FOLLOW: state = self.follow.run() else: print "Attempting to run nonexistent module" self.stop() self.updateState(state) if self.gameTimer.millis() % 10000 == 0: print "Game Time: ", self.gameTimer.millis() if self.gameTimer.millis() > GAME_LENGTH - 500: self.backDoorServo.write(SERVO_OPEN) self.leftMotor.write(0,0) self.rightMotor.write(0,0) if self.gameTimer.millis() > GAME_LENGTH: self.module = MODULE_END self.stop() return ## Switch module if necessary. def updateState(self, module): if self.module == module: return elif module == MODULE_OFF: self.off.start() self.module = MODULE_OFF return elif module == MODULE_CHECK: self.check.start() self.module = MODULE_CHECK return elif module == MODULE_PICKUP: self.pickup.start() self.module = MODULE_PICKUP return elif module == MODULE_DROPOFF: self.dropoff.start() self.module = MODULE_DROPOFF return elif module == MODULE_FOLLOW: self.follow.start() self.module = MODULE_FOLLOW return else: print "Attempting to switch to nonexistent module" self.stop() ## Make sure that the intake motor does not stall. # If so, reverse the intake motors. # # @param checkTime Time in ms between checking stalls. # @param reverseTime Time in ms that the intake motors will reverse if needed. def checkForIntakeErrors(self, checkTime = 100, reverseTime = 800): if self.intakeDirection: # We are moving forward. if self.intakeTimer.millis() > checkTime: self.intakeTimer.reset() if self.intakeEncoder.val < INTAKE_ENCODER_LIMIT: # if we're stalled self.intakeDirection = True self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) else: # if we're not stalled self.intakeEncoder.write(0) else: # We are reversing the motors. if self.intakeTimer.millis() > reverseTime: self.intakeTimer.reset() self.intakeDirection = False self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) self.intakeEncoder.write(0) self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
def setup(self): """One-time method that sets up the robot, like in Arduino""" ##--------------------------------------------## ##--------- DRIVING MOTORS (+encoder) --------## ##--------------------------------------------## # Create the motor objects self.lmotor = Motor(self.tamp, *LMOTOR_PINS) self.rmotor = Motor(self.tamp, *RMOTOR_PINS) # Create a subscriber to listen for drive motor commands rospy.Subscriber('drive_cmd', kitware.msg.DriveCMD, self.drive_callback) ## --------- ## if ENCODER: # encoder objects self.lmotor_encoder = Encoder(self.tamp, *LMOTOR_ENCODER_PINS, continuous=True) self.rmotor_encoder = Encoder(self.tamp, *RMOTOR_ENCODER_PINS, continuous=True) # Publishers for encoder values self.pub_lmotor_encoder_val = rospy.Publisher('lmotor_encoder_val', Int64, queue_size=10) self.pub_rmotor_encoder_val = rospy.Publisher('rmotor_encoder_val', Int64, queue_size=10) ##--------------------------------------------## ##--------------- INTAKE MOTOR ---------------## ##--------------------------------------------## # Create the motor object self.intake_motor = Motor(self.tamp, *INTAKE_MOTOR_PINS) # Create a subscriber to listen for intake motor commands (passively continuous) rospy.Subscriber('intake_cmd', kitware.msg.DriveCMD, self.intake_callback) ##--------------------------------------------## ##-------------- DISPENSER SERVO -------------## ##--------------------------------------------## # Setup servo self.servo = Servo(self.tamp, SERVO_PIN) self.servo.write(0) self.servo_val = 0 self.delta = DISPENSOR_SPEED self.end = False # Subscriber to listen for drive motor commands rospy.Subscriber('dispenser_servo_speed', Int64, self.dispenser_servo_callback) # Publisher for current servo value self.pub_dispenser_servo_val = rospy.Publisher('dispenser_servo_val', Int64, queue_size=10) ##--------------------------------------------## ##--------- DISPENSER MOTOR (+encoder) -------## ##--------------------------------------------## # Create the motor object self.dispenser_motor = Motor(self.tamp, *DISPENSER_MOTOR_PINS) # Create a subscriber to listen for dispenser motor commands rospy.Subscriber('dispenser_cmd', kitware.msg.DriveCMD, self.dispenser_motor_callback) ## --------- ## if ENCODER: # encoder objects self.dispenser_motor_encoder = Encoder( self.tamp, *DISPENSER_MOTOR_ENCODER_PINS, continuous=True) # Publishers for encoder values self.pub_dispenser_motor_encoder_val = rospy.Publisher( 'dispenser_motor_encoder_val', Int64, queue_size=10) ##--------------------------------------------## ##---------- General ---------## ##--------------------------------------------## # timer for loop() self.timer = Timer() # reset all motor encoders self.lmotor_encoder.val = 0 self.rmotor_encoder.val = 0 self.dispenser_motor_encoder.val = 0
def setup(self): self.encoder = Encoder(self.tamp, *self.pins, continuous=True) self.timer = Timer()
class MyRobot(SyncedSketch): runtime = 180000 #ms DOOR_OPEN_POS = 40 DOOR_CLOSE_POS = 144 GRIPPER_OPEN_POS = 0 GRIPPER_CLOSE_POS = 180 GRIPPER_DOWN = 1 GRIPPER_UP = 0 def setup(self): # initialize sensors, settings, start timers, etc. self.gameTimer = Timer() # 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))