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 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()
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.ss_pin = 10 #gyro select pin
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.gyroCAngle = 0
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.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()
'''
def setup(self):
self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
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)
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()
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 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.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
self.timer = Timer()
self.cali_timer = Timer()
self.calibration = 0.0
self.calibrated = False
def setup(self):
# initialize sensors, settings, start timers, etc.
self.gameTimer = Timer()
self.motorGripper = Motor(self.tamp, 23, 3)
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)
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."
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)
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.state = CollectBlockState()
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.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)
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 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."
