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 TestWallFollow(SyncedSketch):
def setup(self):
self.left = Motor(self.tamp, 5, 4)
self.right = Motor(self.tamp, 2, 3)
hugs = Motor(self.tamp, 8, 9)
ir0 = LRIR(self.tamp,14)
ir1 = LRIR(self.tamp,15)
ir2 = LRIR(self.tamp,16)
ir3 = LRIR(self.tamp,17)
self.movement = WallFollow(self.left, self.right, Timer(), ir0, ir1, ir2, ir3)
self.timer = Timer()
self.wintimer = Timer()
def loop(self):
if self.wintimer.millis() < 19600:
if self.timer.millis() > 100:
self.timer.reset()
print self.movement.distance()
# Intended behavior: bot will follow wall
# IR return distance, 50 speed
self.movement.followWall(self.movement.distance(),-FORWARD_SPEED)
elif self.wintimer.millis() < 19900:
self.left.write( 0, 145)
self.right.write( 1, 145)
else:
self.wintimer.reset()
self.left.write(0,0)
self.right.write(0,0)
class MotorWrite(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 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 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 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 TestWallFollow(SyncedSketch):
def setup(self):
self.left = Motor(self.tamp, 5, 4)
self.right = Motor(self.tamp, 2, 3)
hugs = Motor(self.tamp, 8, 9)
ir0 = LRIR(self.tamp, 14)
ir1 = LRIR(self.tamp, 15)
ir2 = LRIR(self.tamp, 16)
ir3 = LRIR(self.tamp, 17)
self.movement = WallFollow(self.left, self.right, Timer(), ir0, ir1,
ir2, ir3)
self.timer = Timer()
self.wintimer = Timer()
def loop(self):
if self.wintimer.millis() < 19600:
if self.timer.millis() > 100:
self.timer.reset()
print self.movement.distance()
# Intended behavior: bot will follow wall
# IR return distance, 50 speed
self.movement.followWall(self.movement.distance(),
-FORWARD_SPEED)
elif self.wintimer.millis() < 19900:
self.left.write(0, 145)
self.right.write(1, 145)
else:
self.wintimer.reset()
self.left.write(0, 0)
self.right.write(0, 0)
class MotorWrite(Sketch):
def setup(self):
#self.motor = Motor(self.tamp, 2, 3)
#self.motor.write(1,0)
self.delta = 1
self.motorval = 0
self.timer = Timer()
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 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 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 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 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 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 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 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):
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 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 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 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 Bot(SyncedSketch):
def setup(self):
self.timer = Timer()
self.utils = Utils(time.time(),180) #start time, total game time
self.sensors = Sensors(self.tamp)
self.actuators = Actuators(self.tamp)
self.motorController= MotorController(self.sensors,self.actuators)
self.myState = startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils)
def loop(self):
self.myState=self.myState.run() #run current state and return next state
self.timer.reset()
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 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 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 Bot(SyncedSketch):
def setup(self):
self.timer = Timer()
self.utils = Utils(time.time(), 180) #start time, total game time
self.sensors = Sensors(self.tamp)
self.actuators = Actuators(self.tamp)
self.motorController = MotorController(self.sensors, self.actuators)
self.myState = startState(self.sensors, self.actuators,
self.motorController, self.timer, self.utils)
def loop(self):
self.myState = self.myState.run(
) #run current state and return next state
self.timer.reset()
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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 Bot(SyncedSketch):
def setup(self):
self.timer = Timer()
self.utils = Utils(time.time(),180) #start time, total game time
self.sensors = Sensors(self.tamp)
self.actuators = Actuators(self.tamp)
self.motorController= MotorController(self.sensors,self.actuators)
self.myState = startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils)
def loop(self):
try:
self.myState=self.myState.run() #run current state and return next state
except KeyboardInterrupt:
raise
except:
print "cought an error, back to start state"
self.myState=startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils)
self.timer.reset()
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 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 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 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
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 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):
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):
if (self.timer.millis() > 10):
self.timer.reset()
if self.servoval >= 180: self.delta = -1
elif self.servoval <= 0: self.delta = 1
self.servoval += self.delta
print self.servoval
self.servo.write(abs(self.servoval))
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 Bot(SyncedSketch):
def setup(self):
self.timer = Timer()
self.utils = Utils(time.time(), 180) #start time, total game time
self.sensors = Sensors(self.tamp)
self.actuators = Actuators(self.tamp)
self.motorController = MotorController(self.sensors, self.actuators)
self.myState = startState(self.sensors, self.actuators,
self.motorController, self.timer, self.utils)
def loop(self):
try:
self.myState = self.myState.run(
) #run current state and return next state
except KeyboardInterrupt:
raise
except:
print "cought an error, back to start state"
self.myState = startState(self.sensors, self.actuators,
self.motorController, self.timer,
self.utils)
self.timer.reset()
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 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()
