class GyroRead(SyncedSketch): # Set me! ss_pin = 10 def setup(self): self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.timer = Timer() self.cali_timer = Timer() self.calibration = 0.0 self.calibrated = False def loop(self): if self.timer.millis() > 100: self.timer.reset() # Valid gyro status is [0,1], see datasheet on ST1:ST0 bits print "{:6f}, raw: 0x{:08x} = 0b{:032b}".format(self.gyro.val, self.gyro.raw, self.gyro.raw) # Janky autocalibration scheme if not self.calibrated and self.cali_timer.millis() > 3000: drift = self.gyro.val / (self.cali_timer.millis() / 1000.0) # Arbitrary calibration tolerance of 0.1 deg/sec if abs(drift) > 0.1: self.calibration += drift print "Calibration:", self.calibration self.gyro.calibrate_bias(self.calibration) else: print "Calibration complete:", self.calibration self.calibrated = True self.gyro.reset_integration() self.cali_timer.reset()
class MotorWrite(SyncedSketch): def setup(self): self.motor1 = Motor(self.tamp, 2, 3) self.motor1.write(1,0) self.motor2 = Motor(self.tamp, 5, 4) self.motor2.write(1,0) self.delta = 1 self.motorval = 70 self.timer = Timer() self.steps = 0 def loop(self): if self.steps < 2: if (self.timer.millis() > 5000): self.timer.reset() self.motorval = -self.motorval self.motor1.write(self.motorval>0, abs(self.motorval)) self.motor2.write(self.motorval>0, abs(self.motorval)) self.steps += 1 elif self.steps < 5: if (self.timer.millis() > 5000): self.timer.reset() self.motorval = -self.motorval self.motor1.write(self.motorval>0, abs(self.motorval)) self.motor2.write(self.motorval<0, abs(self.motorval)) self.steps += 1 else: self.motor1.write(0, 0) self.motor2.write(0, 0) self.stop()
class GyroRead(SyncedSketch): # Set me! ss_pin = 10 def setup(self): self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.timer = Timer() self.cali_timer = Timer() self.calibration = 0.0 self.calibrated = False def loop(self): if self.timer.millis() > 100: self.timer.reset() # Valid gyro status is [0,1], see datasheet on ST1:ST0 bits print "{:6f}, raw: 0x{:08x} = 0b{:032b}".format( self.gyro.val, self.gyro.raw, self.gyro.raw) # Janky autocalibration scheme if not self.calibrated and self.cali_timer.millis() > 3000: drift = self.gyro.val / (self.cali_timer.millis() / 1000.0) # Arbitrary calibration tolerance of 0.1 deg/sec if abs(drift) > 0.1: self.calibration += drift print "Calibration:", self.calibration self.gyro.calibrate_bias(self.calibration) else: print "Calibration complete:", self.calibration self.calibrated = True self.gyro.reset_integration() self.cali_timer.reset()
class TestWallFollow(SyncedSketch): def setup(self): self.left = Motor(self.tamp, 5, 4) self.right = Motor(self.tamp, 2, 3) hugs = Motor(self.tamp, 8, 9) ir0 = LRIR(self.tamp, 14) ir1 = LRIR(self.tamp, 15) ir2 = LRIR(self.tamp, 16) ir3 = LRIR(self.tamp, 17) self.movement = WallFollow(self.left, self.right, Timer(), ir0, ir1, ir2, ir3) self.timer = Timer() self.wintimer = Timer() def loop(self): if self.wintimer.millis() < 19600: if self.timer.millis() > 100: self.timer.reset() print self.movement.distance() # Intended behavior: bot will follow wall # IR return distance, 50 speed self.movement.followWall(self.movement.distance(), -FORWARD_SPEED) elif self.wintimer.millis() < 19900: self.left.write(0, 145) self.right.write(1, 145) else: self.wintimer.reset() self.left.write(0, 0) self.right.write(0, 0)
class TestWallFollow(SyncedSketch): def setup(self): self.left = Motor(self.tamp, 5, 4) self.right = Motor(self.tamp, 2, 3) hugs = Motor(self.tamp, 8, 9) ir0 = LRIR(self.tamp,14) ir1 = LRIR(self.tamp,15) ir2 = LRIR(self.tamp,16) ir3 = LRIR(self.tamp,17) self.movement = WallFollow(self.left, self.right, Timer(), ir0, ir1, ir2, ir3) self.timer = Timer() self.wintimer = Timer() def loop(self): if self.wintimer.millis() < 19600: if self.timer.millis() > 100: self.timer.reset() print self.movement.distance() # Intended behavior: bot will follow wall # IR return distance, 50 speed self.movement.followWall(self.movement.distance(),-FORWARD_SPEED) elif self.wintimer.millis() < 19900: self.left.write( 0, 145) self.right.write( 1, 145) else: self.wintimer.reset() self.left.write(0,0) self.right.write(0,0)
class SwitchRead(SyncedSketch): def setup(self): self.switch = A(DigitalInput(self.tamp, 23)) self.timer = Timer() self.ttt = Timer() def loop(self): if self.timer.millis() > 100: self.timer.reset() print self.switch.read() if self.ttt.millis() > 8000: self.stop()
class AnalogRead(SyncedSketch): screen = pygame.display.set_mode((300, 300)) v_max = 1 points = [] distances = [[x, []] for x in xrange(2, 16)] distances = dict(distances) data = [] set = 0 def setup(self): self.sensor_array = Ir_array(self.tamp, 16, 15, 14, 40, 11, 10) # self.testpin = AnalogInput(self.tamp, self.adc_pin) #these self.timer = Timer() #this, too. def loop(self): if self.timer.millis() > 100: self.screen.fill((0, 0, 0)) events = pygame.event.get() self.sensor_array.update() for x in xrange(6): value = self.sensor_array.ir_value[x] print value if value != float('inf'): pygame.draw.rect(self.screen, (255, 255, 255), (10 * x, 300 - value * 10, 5, 5)) pygame.display.flip()
class AnalogRead(SyncedSketch): screen = pygame.display.set_mode((300, 300)) v_max = 1 points = [] distances = [[x, []] for x in xrange(2, 16)] distances = dict(distances) data = [] set = 0 def setup(self): self.sensor_array = Ir_array(self.tamp, 16, 15, 14, 40, 11, 10) # self.testpin = AnalogInput(self.tamp, self.adc_pin) #these self.timer = Timer() #this, too. def loop(self): if self.timer.millis() > 100: self.screen.fill((0, 0, 0)) events = pygame.event.get() self.sensor_array.update() for x in xrange(6): value = self.sensor_array.ir_value[x] print value if value != float('inf'): pygame.draw.rect(self.screen, (255, 255, 255), (10*x, 300-value*10, 5, 5)) pygame.display.flip()
class ColorRead(SyncedSketch): def setup(self): self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.timer = Timer() def loop(self): if self.timer.millis() > 100: self.timer.reset() print "~~~~~~~~~" print self.color.r, self.color.g, self.color.b, self.color.c #print self.calibrate_colors(self.color.r, self.color.g, self.color.b) #print self.color.colorTemp, self.color.lux print self.whatColorIsThis(self.color.r, self.color.g, self.color.b) def calibrate_colors(self, r, g, b): return (int(r / 0.90), int(g / 0.45), int(b / 0.40)) def whatColorIsThis(self, r, g, b): if r > 1.6*g and r > 1.6*b: return "Red" elif g > r and g > 1.2*b: return "Green" else: return "No Block"
class MotorWrite(Sketch): def setup(self): #self.motor = Motor(self.tamp, 2, 3) #self.motor.write(1,0) self.delta = 1 self.motorval = 0 self.timer = Timer() self.pipe_path = "./motorL" if not os.path.exists(pipe_path): os.mkfifo(pipe_path) # Open the fifo. We need to open in non-blocking mode or it will stalls until # someone opens it for writting pipe_fd = os.open(pipe_path, os.O_RDONLY) def loop(self): if (self.timer.millis() > 10): message = os.read(pipe_fd,100) if message: print("Received: '%s'" % message) self.timer.reset() if abs(self.motorval) == 255: self.delta = -self.delta self.motorval += self.delta
class TestFollow(SyncedSketch): def setup(self): self.ttt = Timer() timer = Timer() stepTimer = Timer() wallFollowTimer = Timer() leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION, LEFT_DRIVE_CONTROLLER_PWM) rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION, RIGHT_DRIVE_CONTROLLER_PWM) intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION, HUGS_MOTOR_CONTROLLER_PWM) irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL) irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR) irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL) irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR) self.wallFollow = WallFollow(leftMotor, rightMotor, wallFollowTimer, irFL, irFR, irBL, irBR) blockSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH) self.blockSwitch = DigitalInput(self.tamp, 21) self.follow = FollowModule(timer, leftMotor, rightMotor, intakeMotor, self.wallFollow, FORWARD_SPEED, self.blockSwitch) self.follow.start() def loop(self): response = self.follow.run() if self.ttt.millis() > 100: self.ttt.reset() print self.wallFollow.distance() if response != MODULE_FOLLOW: self.stop()
class ColorRead(SyncedSketch): def setup(self): self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.timer = Timer() def loop(self): if self.timer.millis() > 100: self.timer.reset() print "~~~~~~~~~" print self.color.r, self.color.g, self.color.b, self.color.c #print self.calibrate_colors(self.color.r, self.color.g, self.color.b) #print self.color.colorTemp, self.color.lux print self.whatColorIsThis(self.color.r, self.color.g, self.color.b) def calibrate_colors(self, r, g, b): return (int(r / 0.90), int(g / 0.45), int(b / 0.40)) def whatColorIsThis(self, r, g, b): if r > 1.6 * g and r > 1.6 * b: return "Red" elif g > r and g > 1.2 * b: return "Green" else: return "No Block"
class EncoderRead(SyncedSketch): encoderPins = 28, 27 motorPwm = 6 #green motorDir = 7 #yellow def setup(self): self.encoder = Encoder(self.tamp, *self.encoderPins, continuous=True) self.motor = Motor(self.tamp, self.motorDir, self.motorPwm) self.miscTimer = Timer() self.timer = Timer() self.rotations = 0 self.lastRot = 0 self.motor.write(0, 100) def loop(self): if self.timer.millis() > 100: print "enc", self.encoder.val self.timer.reset() if self.encoder.val > 5.0 * 3200: print "Stopped at", self.encoder.val self.motor.write(1,0) self.stop() '''
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()
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();
class Blink(Sketch): def setup(self): self.led = DigitalOutput(self.tamp, 13) self.led_timer = Timer() self.led_state = False def loop(self): if self.led_timer.millis() > 1000: self.led_timer.reset() self.led_state = not self.led_state self.led.write(self.led_state)
class AnalogRead(SyncedSketch): adc_pin = 0 def setup(self): self.testpin = AnalogInput(self.tamp, self.adc_pin) self.timer = Timer() def loop(self): if self.timer.millis() > 100: self.timer.reset() print self.testpin.val
class EncoderRead(SyncedSketch): pins = 5, 6 def setup(self): self.encoder = Encoder(self.tamp, *self.pins, continuous=True) self.timer = Timer() def loop(self): if self.timer.millis() > 100: self.timer.reset() print self.encoder.val
class ColorRead(SyncedSketch): def setup(self): self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.timer = Timer() def loop(self): if self.timer.millis() > 100: self.timer.reset() print self.color.r, self.color.g, self.color.b, self.color.c print self.color.colorTemp, self.color.lux
class AnalogWrite(Sketch): PWM_PIN = 3 def setup(self): self.pwm = AnalogOutput(self.tamp, self.PWM_PIN) self.pwm_value = 0 self.pwm_timer = Timer() def loop(self): if self.pwm_timer.millis() > 10: self.pwm_timer.reset() self.pwm_value = (self.pwm_value + 1) % 256 self.pwm.write(self.pwm_value)
class ImuRead(SyncedSketch): def setup(self): self.imu = Imu(self.tamp) self.timer = Timer() def loop(self): if self.timer.millis() > 100: self.timer.reset() print "Accel x: {} g, y: {} g, z: {} g".format( self.imu.ax, self.imu.ay, self.imu.az) print "Gyro x: {} deg/sec, y: {} deg/sec, z: {} deg/sec".format( self.imu.gx, self.imu.gy, self.imu.gz) print "Mag x: {} mG, y: {} mG, z: {} mG".format( self.imu.mx, self.imu.my, self.imu.mz)
class GyroRead(SyncedSketch): # Set me! ss_pin = 10 def setup(self): self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.timer = Timer() def loop(self): if self.timer.millis() > 100: self.timer.reset() # Valid gyro status is [0,1], see datasheet on ST1:ST0 bits print self.gyro.val, self.gyro.status
class AnalogWrite(Sketch): PWM_PIN = 3 def setup(self): self.pwm = AnalogOutput(self.tamp, self.PWM_PIN) self.pwm_value = 0 self.pwm_timer = Timer() def loop(self): if (self.pwm_timer.millis() > 10): self.pwm_timer.reset() self.pwm_value = (self.pwm_value + 1) % 256 self.pwm.write(self.pwm_value)
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 TestGoStraight(SyncedSketch): def setup(self): left = Motor(self.tamp, 5, 4) right = Motor(self.tamp, 2, 3) gyro = Gyro(self.tamp, 10, integrate=True) self.movement = GoStraight(left, right, Timer()) self.timer = Timer() def loop(self): if self.timer.millis() > 100: self.timer.reset() # Intended behavior: Have the robot turn in a circle. # move_to_target() was not defined self.movement.move_to_target(45, 50)
class BeltMove(SyncedSketch): def setup(self): # Motor object representing the conveyor belt motor. self.limSwitch = DigitalInput(self.tamp, 23) self.conveyorMotor = Motor(self.tamp, 7, 6) self.timer = Timer() def loop(self): if self.timer.millis() > 100: self.timer.reset() if self.limSwitch.val: self.conveyorMotor.write(UP, 0) self.stop() else: self.conveyorMotor.write(DOWN, 80)
class TestPickup(SyncedSketch): def setup(self): self.timer = Timer() 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 loop(self): if self.timer.millis() > 100: response = self.pickup.run() if response != MODULE_PICKUP: self.stop()
class IRRead(SyncedSketch): adc_pin = 4 def setup(self): self.testpin = LongIR(self.tamp, self.adc_pin) self.timer = Timer() def loop(self): if self.timer.millis() > 100: self.timer.reset() # if self.testpin.distInches != "far" : print str(self.testpin.distTicks) print str(self.testpin.distInches) print "---"
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 TestIR(SyncedSketch): def setup(self): self.ir = IR(self.tamp, 23) self.sum = 0 self.timer = Timer() self.timestep = 0 def loop(self): if (self.timer.millis() > 100): self.timer.reset() #self.sum += self.ir.read_ir() self.timestep += 1 print self.timestep print "raw val:", self.ir.val print "distance:", self.ir.read_ir()
class MotorWrite(Sketch): def setup(self): self.motor = Motor(self.tamp, 2, 3) self.motor.write(1, 0) self.delta = 1 self.motorval = 0 self.timer = Timer() def loop(self): if self.timer.millis() > 10: self.timer.reset() if abs(self.motorval) == 255: self.delta = -self.delta self.motorval += self.delta self.motor.write(self.motorval > 0, abs(self.motorval))
class MotorWrite(Sketch): def setup(self): self.motor = Motor(self.tamp, 2, 3) self.motor.write(1,0) self.delta = 1 self.motorval = 0 self.timer = Timer() def loop(self): if (self.timer.millis() > 10): self.timer.reset() if abs(self.motorval) == 255: self.delta = -self.delta self.motorval += self.delta self.motor.write(self.motorval>0, abs(self.motorval))
class TestIR(SyncedSketch): def setup(self): self.ir = IR(self.tamp, 23) self.sum = 0 self.timer = Timer() self.timestep = 0 def loop(self): if (self.timer.millis() > 100): self.timer.reset() #self.sum += self.ir.read_ir() self.timestep += 1 print self.timestep print "raw val:",self.ir.val print "distance:",self.ir.read_ir()
class ServoWrite(Sketch): """Cycles a servo back and forth between 1050us and 1950us pulse widths (most servos are 1000-2000)""" def setup(self): self.servo = Servo(self.tamp, 9) self.servo.write(1050) self.timer = Timer() self.end = False def loop(self): if (self.timer.millis() > 2000): self.timer.reset() if self.end: self.servo.write(1050) else: self.servo.write(1950) self.end = not self.end
class SerialConnect(Sketch): 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 loop(self): if (self.timer.millis() > 5000): self.stop()
class EncoderRead(SyncedSketch): def setup(self): self.steps = 0 # Motor object representing the conveyor belt motor. self.motor = Motor(self.tamp, 7, 6) # Encoder object for the conveyor belt motor. self.encoder = Encoder(self.tamp, 28, 27) self.timer = Timer() self.motor.write(DOWN, 75) def loop(self): if self.timer.millis() > 100: self.timer.reset() self.steps += 1 print self.steps, self.encoder.val if self.steps > 25: self.motor.write(DOWN, 75) self.stop()
class ServoWrite(Sketch): """Cycles a servo back and forth between 0 and 180 degrees. However, these degrees are not guaranteed accurate, and each servo's range of valid microsecond pulses is different""" SERVO_PIN = 9 def setup(self): pygame.init() self.screen = pygame.display.set_mode((300, 300)) self.servo = Servo(self.tamp, self.SERVO_PIN) self.servo.write(0) self.servoval = 0 self.delta = 1 self.timer = Timer() self.end = False self.wait = False def loop(self): for event in pygame.event.get(): if event.type == pygame.KEYDOWN: if event.key == pygame.K_UP: self.servoval = 180 if event.key == pygame.K_DOWN: self.servoval = 0 if event.key == pygame.K_SPACE: self.servoval = 90 if (self.timer.millis() > 10): self.timer.reset() if self.servoval >= 180: self.delta = -1 # down elif self.servoval <= 0: self.delta = 1 # up elif self.wait == True: self.delta = 0 # self.servoval += self.delta print self.servoval self.servo.write(abs(self.servoval))
class TestDropoff(SyncedSketch): 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 loop(self): if self.ttt.millis() > 100: self.ttt.reset() response = self.dropoff.run() if response != MODULE_DROPOFF: self.stop()
class ServoWrite(Sketch): """Cycles a servo back and forth between 0 and 180 degrees. However, these degrees are not guaranteed accurate, and each servo's range of valid microsecond pulses is different""" SERVO_PIN = 25 def setup(self): self.servo = Servo(self.tamp, self.SERVO_PIN) self.servo.write(0) self.servoval = 0 self.delta = 1 self.timer = Timer() self.end = False def loop(self): self.servo.write(180) if self.timer.millis() > 1000: self.stop() '''
class TestIRBR(SyncedSketch): def setup(self): self.ir = LRIR(self.tamp, 15) self.sum = 0 self.timer = Timer() self.timestep = 0 def loop(self): if (self.timer.millis() > 1000): self.timer.reset() #self.sum += self.ir.read_ir() self.timestep += 1 print self.timestep raw = self.ir.val print "raw val:", raw print "distance:", self.convertToDistance(raw) #print "average:",self.sum / self.timestep ## Takes raw IR data and outputs centimeters def convertToDistance(self, raw): return (1.0 / raw) * 799400 - 9.119
class AnalogRead(SyncedSketch): adc_pin = 23 def setup(self): self.testpin = AnalogInput(self.tamp, self.adc_pin) self.timer = Timer() def loop(self): if self.timer.millis() > 100: self.timer.reset() print self.testpin.val volts= ((self.testpin.val)*0.0001)-1 #print(volts) #at 5 volts this seems to be a good distance from wall? distance= float(13.0/(volts)) print distance if distance>0 and distance< 2.6: print("STOP!! then turn") elif distance >2.6: print("MOVE AHEAD") elif distance <0: print("MOVE AHEAD")
class TestIRBR(SyncedSketch): def setup(self): self.ir = LRIR(self.tamp, 15) self.sum = 0 self.timer = Timer() self.timestep = 0 def loop(self): if (self.timer.millis() > 1000): self.timer.reset() #self.sum += self.ir.read_ir() self.timestep += 1 print self.timestep raw = self.ir.val print "raw val:",raw print "distance:",self.convertToDistance(raw) #print "average:",self.sum / self.timestep ## Takes raw IR data and outputs centimeters def convertToDistance(self, raw): return (1.0/raw)*799400-9.119
class TimeOfFlightRead(SyncedSketch): # Set these to the digital inputs connected to each xshut pin tof_pin = 20 tof2_pin = 21 def setup(self): # Add all ToFs self.tof = TimeOfFlight(self.tamp, self.tof_pin, 1) self.tof2 = TimeOfFlight(self.tamp, self.tof2_pin, 2) # Now enable them all self.tof.enable() self.tof2.enable() self.timer = Timer() def loop(self): if self.timer.millis() > 100: self.timer.reset() print self.tof.dist, "mm", "/", self.tof2.dist, "mm"
class SortBlocks(SyncedSketch): def setup(self): self.LEFT_TOWER = 83.4 self.RIGHT_TOWER = 27 self.CENTER = 54 self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.servo = Servo(self.tamp, 23) #pin TBD self.red_detected = False self.green_detected = False self.servo.write(self.CENTER) self.counter = 0 self.timer = Timer() def loop(self): if self.timer.millis() > 200: self.timer.reset() # print self.color.r, self.color.g, self.color.b, self.color.c # print self.color.colorTemp, self.color.lux #if not self.red_detected and not self.green_detected and self.color.r > 1.3 * self.color.g and self.color.r > 1.3 * self.color.b: detect_red = self.color.r > 1.3*self.color.g detect_green = self.color.g > 1.3*self.color.r sum_val = self.color.r+self.color.g+self.color.b if detect_red and sum_val > 300: self.servo.write(self.LEFT_TOWER) self.counter = 0 #elif not self.red_detected and not self.green_detected and self.color.g > 1.3 * self.color.r and self.color.g > 1.3 * self.color.b: elif detect_green and sum_val > 300: self.servo.write(self.RIGHT_TOWER) self.counter = 0 elif self.counter > 400: self.servo.write(self.CENTER) self.counter += 100