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"
def setup(self):
self.state = self.STRAIGHT_STATE
self.leftMotor = Motor(self.tamp, LEFT_MOTOR_DIRECTION, LEFT_MOTOR_PWM)
self.rightMotor = Motor(self.tamp, RIGHT_MOTOR_DIRECTION, RIGHT_MOTOR_PWM)
self.bumper_RF = DigitalInput(self.tamp, BUMP_SENSOR_R_FRONT)
self.bumper_LF = DigitalInput(self.tamp, BUMP_SENSOR_L_FRONT)
self.bumper_RS = DigitalInput(self.tamp, BUMP_SENSOR_R_SIDE)
self.bumper_LS = DigitalInput(self.tamp, BUMP_SENSOR_L_SIDE)
self.comp_timer = Timer()
self.straight_timer = Timer()
self.reverse_timer = Timer()
self.turn_timer = Timer()
self.bump_timer = Timer()
self.turn_direction = LEFT
self.turn_pwm = 50
def setup(self):
#self.motorLeft = Motor(self.tamp, 21, 20)
#self.motorRight = Motor(self.tamp, 23, 22)
self.motorLeft = Motor(self.tamp, 20, 21)
self.motorRight = Motor(self.tamp, 22, 23)
self.motorLeft.write(1, 0)
self.motorRight.write(1, 0)
left_pins = 6, 5
right_pins = 3, 4
# Encoder doesn't work when after gyro
self.encoderLeft = Encoder(self.tamp, 6, 5, continuous=False)
self.encoderRight = Encoder(self.tamp, 3, 4, continuous=True)
# TODO: set encoder to 0
self.timer = Timer()
self.gyro = Gyro(self.tamp, 10)
self.P = 5
self.I = 0 # should be negative
self.D = 0
self.dT = .03
self.motorval = 25 #50
self.last_diff = 0
self.integral = 0
self.desired = self.gyro.val #+ 45 # to drive in a straight line
self.encoderLeft.start_continuous()
self.encoderRight.start_continuous()
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
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 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 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 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()
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 setup(self):
self.motor = Motor(self.tamp, 24, 25)
self.motor.write(1, 0)
self.delta = 1
self.motorval = 0
self.timer = Timer()
self.counter = 0
self.spin_forwards = True
def setup(self):
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 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):
self.motor1 = Motor(self.tamp, 2, 3)
self.motor1.write(1,0)
self.motor2 = Motor(self.tamp, 5, 4)
self.motor2.write(1,0)
self.delta = 1
self.motorval = 70
self.timer = Timer()
self.steps = 0
class 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()
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):
# 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 setup(self):
self.state = self.STRAIGHT_STATE
self.leftMotor = Motor(self.tamp, LEFT_MOTOR_DIRECTION, LEFT_MOTOR_PWM)
self.rightMotor = Motor(self.tamp, RIGHT_MOTOR_DIRECTION,
RIGHT_MOTOR_PWM)
self.bumper_RF = DigitalInput(self.tamp, BUMP_SENSOR_R_FRONT)
self.bumper_LF = DigitalInput(self.tamp, BUMP_SENSOR_L_FRONT)
self.bumper_RS = DigitalInput(self.tamp, BUMP_SENSOR_R_SIDE)
self.bumper_LS = DigitalInput(self.tamp, BUMP_SENSOR_L_SIDE)
self.comp_timer = Timer()
self.straight_timer = Timer()
self.reverse_timer = Timer()
self.turn_timer = Timer()
self.bump_timer = Timer()
self.turn_direction = LEFT
self.turn_pwm = 50
def setup(self):
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
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)
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):
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 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 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 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)
def setup(self):
self.left = Motor(self.tamp, 5, 4)
self.right = Motor(self.tamp, 2, 3)
hugs = Motor(self.tamp, 8, 9)
ir0 = LRIR(self.tamp,14)
ir1 = LRIR(self.tamp,15)
ir2 = LRIR(self.tamp,16)
ir3 = LRIR(self.tamp,17)
self.movement = WallFollow(self.left, self.right, Timer(), ir0, ir1, ir2, ir3)
self.timer = Timer()
self.wintimer = Timer()
class 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 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 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()
def setup(self):
# Motor object representing the intake mechanism motors.
self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION,
HUGS_MOTOR_CONTROLLER_PWM)
# Encoder object for the intake motor.
self.intakeEncoder = Encoder(self.tamp, HUGS_MOTOR_ENCODER_YELLOW,
HUGS_MOTOR_ENCODER_WHITE)
# Timer object to moderate checking for intake errors.
self.intakeTimer = Timer()
# Are the intake motors going forward? True if so, False if reversing.
self.intakeDirection = False
# Start the intake motor.
self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
class 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
def setup(self):
self.left = Motor(self.tamp, 5, 4)
self.right = Motor(self.tamp, 2, 3)
hugs = Motor(self.tamp, 8, 9)
ir0 = LRIR(self.tamp, 14)
ir1 = LRIR(self.tamp, 15)
ir2 = LRIR(self.tamp, 16)
ir3 = LRIR(self.tamp, 17)
self.movement = WallFollow(self.left, self.right, Timer(), ir0, ir1,
ir2, ir3)
self.timer = Timer()
self.wintimer = Timer()
def setup(self):
#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 setup(self):
timer = Timer()
timeoutTimer = Timer()
leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION,
LEFT_DRIVE_CONTROLLER_PWM)
rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION,
RIGHT_DRIVE_CONTROLLER_PWM)
intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION,
HUGS_MOTOR_CONTROLLER_PWM)
color = Color(self.tamp)
self.check = CheckModule(timer, timeoutTimer, leftMotor, rightMotor,
intakeMotor, color)
self.check.start()
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)
def setup(self):
self.timer = Timer()
# Motor object representing the left motor.
self.leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION,
LEFT_DRIVE_CONTROLLER_PWM)
# Encoder object for the left motor.
self.leftEncoder = Encoder(self.tamp, LEFT_DRIVE_ENCODER_YELLOW,
LEFT_DRIVE_ENCODER_WHITE)
# Motor object representing the right motor.
self.rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION,
RIGHT_DRIVE_CONTROLLER_PWM)
# Encoder object for the right motor.
self.rightEncoder = Encoder(self.tamp, RIGHT_DRIVE_ENCODER_YELLOW,
RIGHT_DRIVE_ENCODER_WHITE)
class 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 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 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 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 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()
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)
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()
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 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)
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()
class BeltMove(SyncedSketch):
def setup(self):
# Motor object representing the conveyor belt motor.
self.limSwitch = DigitalInput(self.tamp, 23)
self.conveyorMotor = Motor(self.tamp, 7, 6)
self.timer = Timer()
def loop(self):
if self.timer.millis() > 100:
self.timer.reset()
if self.limSwitch.val:
self.conveyorMotor.write(UP, 0)
self.stop()
else:
self.conveyorMotor.write(DOWN, 80)
def setup(self):
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 setup(self):
# initialize sensors, settings, start timers, etc.
self.gameTimer = Timer()
self.motorLeft = Motor(self.tamp, 7, 22)
self.motorRight = Motor(self.tamp, 0, 21)
self.motorval = 0
self.motorLeft.write(1,0)
self.motorRight.write(1,0)
print "Motors connected."
self.timer = Timer()
self.gyro = Gyro(self.tamp, 9)
print "Gyro connected."
self.theta = self.gyro.val
self.dT = .01
frontLeftIR_pin = 14
self.frontLeftIR = AnalogInput(self.tamp, frontLeftIR_pin)
frontRightIR_pin = 15
self.frontRightIR = AnalogInput(self.tamp, frontRightIR_pin)
leftIR_pin = 16
self.leftIR = AnalogInput(self.tamp, leftIR_pin)
rightIR_pin = 17
self.rightIR = AnalogInput(self.tamp, rightIR_pin)
# Initialize PID Values
self.P = 10
self.I = 0 #5
self.D = 0
self.last_diff = 0
self.integral = 0
self.desiredAngle = self.theta
self.finishedCollectingBlock = False
self.finishedDiscardingBlock = False
self.timer = Timer()
self.state = ExploreState()
self.blocksCollected = 0
self.leftIRVals = deque([])
self.rightIRVals = deque([])
self.frontRightIRVals = deque([])
self.frontLeftIRVals = deque([])
# Starts the robot
print "Robot setup complete."
def setup(self):
self.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)
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
