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 MyRobot(SyncedSketch):
runtime = 180000 #ms
DOOR_OPEN_POS = 40
DOOR_CLOSE_POS = 144
GRIPPER_OPEN_POS = 0
GRIPPER_CLOSE_POS = 180
GRIPPER_DOWN = 1
GRIPPER_UP = 0
def setup(self):
# initialize sensors, settings, start timers, etc.
self.gameTimer = Timer()
# Pins (7,22); (3,23); (0,21) work.
# Problem was with the Double Motor controller.
self.motorGripper = Motor(self.tamp, 23, 3)
self.motorLeft = Motor(self.tamp, 7, 22)
self.motorRight = Motor(self.tamp, 0, 21) # good
self.motorval = 0
self.motorLeft.write(1, 0)
self.motorRight.write(1, 0)
self.motorGripper.write(1, 0)
self.currentGripperLevel = 2
print "Motors connected."
self.servoDoor = Servo(self.tamp, 20)
self.servovalDoor = self.DOOR_CLOSE_POS
self.servoDoor.write(self.DOOR_CLOSE_POS)
self.timerDoor = Timer()
self.servoGripper = Servo(self.tamp, 10)
self.servovalGripper = self.GRIPPER_CLOSE_POS
self.servoGripper.write(self.servovalGripper)
self.timerGripper = Timer()
print "Servos connected."
left_pins = 6, 5
right_pins = 3, 4
# Encoder doesn't work when after gyro
self.encoderLeft = Encoder(self.tamp, 6, 5, continuous=False)
self.encoderRight = Encoder(self.tamp, 3, 4, continuous=True)
print "Encoders connected."
# TODO: set encoder to 0
self.timer = Timer()
self.gyro = Gyro(self.tamp, 9)
print "Gyro connected."
self.theta = self.gyro.val
self.dT = .01
self.cam = cv2.VideoCapture(0)
print "Camera connected."
# self.color = Color(self.tamp,
# integrationTime=Color.INTEGRATION_TIME_101MS,
# gain=Color.GAIN_1X)
self.encoderLeft.start_continuous()
self.encoderRight.start_continuous()
frontLeftIR_pin = 14
self.frontLeftIR = AnalogInput(self.tamp, frontLeftIR_pin)
frontRightIR_pin = 15
self.frontRightIR = AnalogInput(self.tamp, frontRightIR_pin)
leftIR_pin = 16
self.leftIR = AnalogInput(self.tamp, leftIR_pin)
rightIR_pin = 17
self.rightIR = AnalogInput(self.tamp, rightIR_pin)
# Initialize PID Values
self.P = 10
self.I = 5
self.D = 5
self.last_diff = 0
self.integral = 0
self.desiredAngle = self.theta
self.finishedCollectingBlock = False
self.finishedDiscardingBlock = False
self.timer = Timer()
self.state = ExploreState()
# self.state = CollectBlockState()
self.blocksCollected = 0
self.leftIRVals = deque([])
self.rightIRVals = deque([])
self.frontRightIRVals = deque([])
self.frontLeftIRVals = deque([])
self.goIntoTimeoutState = False
self.timeoutCounter = 0
# Starts the robot
print "Robot setup complete."
def loop(self):
if self.timer.millis() > self.dT * 1000:
# print("GameTimer:", self.gameTimer.millis())
if (self.gameTimer.millis() >
self.runtime - 5000): # 5 seconds left in the game
self.openDoorAndBuildTower()
inputs = self.readSensors()
process = self.state.process(inputs)
print "Process: " + process.__class__.__name__
if self.goIntoTimeoutState:
self.state = TimeoutState()
elif self.timeoutCounter == 3000:
self.state = ExploreState()
self.timeoutCounter = 0
self.goIntoTimeoutState = False
else:
self.state = process.get_next_state()
self.processOutputs(process.get_outputs())
self.timer.reset()
def readSensors(self):
# Calculate the distance traveled, change in theta, and then reset sensors
distance_traveled = (self.encoderLeft.val +
self.encoderRight.val) / 2.0
#encoder_omega = self.encoderLeft.val - self.encoderRight.val
print('frontRightIR: ', self.frontRightIR.val)
print("frontLeftIR: ", self.frontLeftIR.val)
print("leftIR: ", self.leftIR.val)
print("rightIR: ", self.rightIR.val)
blocks = [] #CalculateBlocks(); #what should CalculateBlocks return?
leftIR = self.leftIR.val
rightIR = self.rightIR.val
frontLeftIR = self.frontLeftIR.val
frontRightIR = self.frontRightIR.val
# if len(self.leftIRVals) == 100:
# print("Reading from averaged values")
# self.leftIRVals.append(leftIR)
# self.leftIRVals.popleft()
# leftIR = sum(leftIRVals)/100
# if len(self.rightIRVals) == 100:
# self.rightIRVals.append(rightIR)
# self.rightIRVals.popleft()
# rightIR = sum(self.rightIRVals)/100
# if len(self.frontLeftIRVals) == 100:
# self.frontLeftIRVals.append(frontLeftIR)
# self.frontLeftIRVals.popleft()
# frontLeftIR = sum(self.frontLeftIRVals)/100
# if len(self.frontRightIRVals) == 100:
# self.frontRightIRVals.append(frontRightIR)
# self.frontRightIRVals.popleft()
# frontRightIR = sum(self.frontRightIRVals)/100
ret, frame = self.cam.read()
img = cv2.resize(frame,
None,
fx=0.25,
fy=0.25,
interpolation=cv2.INTER_AREA)
print("VideoFrame captured: ", ret)
return Inputs(distance_traveled, self.gyro.val, frontRightIR,
frontLeftIR, leftIR, rightIR,
self.finishedCollectingBlock, img)
# self.leftIR.val, self.rightIR.val, self.color.r, self.color.g, self.color.b)
# distance_traveled, theta, frontRightIR, frontLeftIR, leftIR, rightIR
def processOutputs(self, Outputs):
# TODO Missing servo outputs
if (Outputs.driving == True):
self.motorval = 50 #25?
else:
self.motorval = 0
if (Outputs.turning == True):
# if we turn, then update self.desiredAngle
self.desiredAngle = self.gyro.val
if (Outputs.turn_clockwise == True):
self.PID(self.desiredAngle + 3) # originally = 5
else:
self.PID(self.desiredAngle - 3) # originally = 5
else:
# self.PID(self.gyro.val)
self.PID(self.desiredAngle)
if Outputs.isCollectingBlock and not self.finishedCollectingBlock:
# Gripper is at level 2 and closed
if self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_CLOSE_POS:
self.closeOrOpenGripper()
# Gripper is at level 2 and open
elif self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_OPEN_POS:
self.moveGripper()
# Gripper is at level 1 and open
elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_OPEN_POS:
self.closeOrOpenGripper()
# Gripper is at level 1 and closed
elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_CLOSE_POS:
self.moveGripper()
# self.finishedCollectingBlock = True
time.sleep(2)
self.blocksCollected += 1
if (self.blocksCollected == 4):
self.finishedCollectingBlock = True
if Outputs.isDiscardingBlock and not self.finishedDiscardingBlock:
# Gripper level 2, closed
if self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_CLOSE_POS:
self.moveGripper()
# Gripper level 1, closed
elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_CLOSE_POS:
self.closeOrOpenGripper()
# Gripper level 1, open
elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_OPEN_POS:
self.moveGripper()
# Gripper level 2, open
elif self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_OPEN_POS:
self.closeOrOpenGripper()
self.finishedDiscardingBlock = True
def PID(self, desired_theta):
# Set encoder to 0 after turning.
# To turn in place, set bias (i.e. motorval to 0)
estimated = self.gyro.val # TODO: calculate estimated with encoder
# print(self.gyro.val)
diff = desired_theta - estimated
# print diff
self.integral += diff * self.dT
derivative = (diff - self.last_diff) / self.dT
print(derivative) # check this for timeout?
# if derivative > x:
# self.goIntoTimeoutMode = True
power = self.P * diff + self.I * self.integral + self.D * derivative # NOTE: Cap self.D*derivative, use as timeout
# print("motorLeft: ", min(255, abs(self.motorval + power)))
# print("motorRight: ", min(255, abs(self.motorval - power)))
self.motorLeft.write((self.motorval + power) > 0,
min(255, abs(self.motorval + power)))
self.motorRight.write((self.motorval - power) > 0,
min(255, abs(self.motorval - power)))
# print "EncoderLeft: " + str(self.encoderLeft.val)
# print "EncoderRight: " + str(self.encoderRight.val)
def moveGripper(self):
if self.currentGripperLevel == 1:
self.motorGripper.write(self.GRIPPER_UP, 100)
timeToSleep = 1.3
if (self.blocksCollected == 1):
timeToSleep = 1.4
elif (self.blocksCollected == 2):
timeToSleep = 1.55
elif (self.blocksCollected == 3):
timeToSleep = 1.7
elif self.currentGripperLevel == 2:
self.motorGripper.write(self.GRIPPER_DOWN, 100)
timeToSleep = 0.9
time.sleep(timeToSleep)
self.motorGripper.write(1, 0)
if self.currentGripperLevel == 1:
self.currentGripperLevel = 2
else:
self.currentGripperLevel = 1
def closeOrOpenGripper(self):
if (self.servovalGripper > self.GRIPPER_OPEN_POS):
while (self.servovalGripper > self.GRIPPER_OPEN_POS):
if (self.timerGripper.millis() > 1):
self.timerGripper.reset()
self.servovalGripper -= 1
self.servoGripper.write(abs(self.servovalGripper))
elif (self.servovalGripper < self.GRIPPER_CLOSE_POS):
while (self.servovalGripper < self.GRIPPER_CLOSE_POS):
if (self.timerGripper.millis() > 1):
self.timerGripper.reset()
self.servovalGripper += 1
self.servoGripper.write(abs(self.servovalGripper))
time.sleep(.1)
def openDoorAndBuildTower(self):
self.moveGripper() # should be blocking
self.closeOrOpenGripper()
while (self.servovalDoor > self.DOOR_OPEN_POS):
if (self.timerDoor.millis() > 10):
self.timerDoor.reset()
self.servovalDoor -= 1
# print self.servovalDoor
self.servoDoor.write(abs(self.servovalDoor))
class MyRobot(SyncedSketch):
runtime = 180000 #ms
DOOR_OPEN_POS = 40
DOOR_CLOSE_POS = 144
GRIPPER_OPEN_POS = 0
GRIPPER_CLOSE_POS = 180
GRIPPER_DOWN = 1
GRIPPER_UP = 0
def setup(self):
# initialize sensors, settings, start timers, etc.
self.gameTimer = Timer()
# Pins (7,22); (3,23); (0,21) work.
# Problem was with the Double Motor controller.
self.motorGripper = Motor(self.tamp, 23, 3)
self.motorLeft = Motor(self.tamp, 7, 22)
self.motorRight = Motor(self.tamp, 0, 21) # good
self.motorval = 0
self.motorLeft.write(1,0)
self.motorRight.write(1,0)
self.motorGripper.write(1,0)
self.currentGripperLevel = 2
print "Motors connected."
self.servoDoor = Servo(self.tamp, 20)
self.servovalDoor = self.DOOR_CLOSE_POS
self.servoDoor.write(self.DOOR_CLOSE_POS)
self.timerDoor = Timer()
self.servoGripper = Servo(self.tamp, 10)
self.servovalGripper = self.GRIPPER_CLOSE_POS
self.servoGripper.write(self.servovalGripper)
self.timerGripper = Timer()
print "Servos connected."
left_pins = 6,5
right_pins = 3,4
# Encoder doesn't work when after gyro
self.encoderLeft = Encoder(self.tamp, 6,5, continuous=False)
self.encoderRight = Encoder(self.tamp, 3,4, continuous=True)
print "Encoders connected."
# TODO: set encoder to 0
self.timer = Timer()
self.gyro = Gyro(self.tamp, 9)
print "Gyro connected."
self.theta = self.gyro.val
self.dT = .01
self.cam = cv2.VideoCapture(0)
print "Camera connected."
# self.color = Color(self.tamp,
# integrationTime=Color.INTEGRATION_TIME_101MS,
# gain=Color.GAIN_1X)
self.encoderLeft.start_continuous()
self.encoderRight.start_continuous()
frontLeftIR_pin = 14
self.frontLeftIR = AnalogInput(self.tamp, frontLeftIR_pin)
frontRightIR_pin = 15
self.frontRightIR = AnalogInput(self.tamp, frontRightIR_pin)
leftIR_pin = 16
self.leftIR = AnalogInput(self.tamp, leftIR_pin)
rightIR_pin = 17
self.rightIR = AnalogInput(self.tamp, rightIR_pin)
# Initialize PID Values
self.P = 10
self.I = 5
self.D = 5
self.last_diff = 0
self.integral = 0
self.desiredAngle = self.theta
self.finishedCollectingBlock = False
self.finishedDiscardingBlock = False
self.timer = Timer()
self.state = ExploreState()
# self.state = CollectBlockState()
self.blocksCollected = 0
self.leftIRVals = deque([])
self.rightIRVals = deque([])
self.frontRightIRVals = deque([])
self.frontLeftIRVals = deque([])
self.goIntoTimeoutState = False
self.timeoutCounter = 0
# Starts the robot
print "Robot setup complete."
def loop(self):
if self.timer.millis() > self.dT*1000:
# print("GameTimer:", self.gameTimer.millis())
if (self.gameTimer.millis() > self.runtime - 5000): # 5 seconds left in the game
self.openDoorAndBuildTower()
inputs = self.readSensors()
process = self.state.process(inputs)
print "Process: " + process.__class__.__name__
if self.goIntoTimeoutState:
self.state = TimeoutState()
elif self.timeoutCounter == 3000:
self.state = ExploreState()
self.timeoutCounter = 0
self.goIntoTimeoutState = False
else:
self.state = process.get_next_state()
self.processOutputs(process.get_outputs())
self.timer.reset()
def readSensors(self):
# Calculate the distance traveled, change in theta, and then reset sensors
distance_traveled = (self.encoderLeft.val + self.encoderRight.val) / 2.0
#encoder_omega = self.encoderLeft.val - self.encoderRight.val
print('frontRightIR: ', self.frontRightIR.val)
print("frontLeftIR: ", self.frontLeftIR.val)
print("leftIR: ", self.leftIR.val)
print("rightIR: ", self.rightIR.val)
blocks = [] #CalculateBlocks(); #what should CalculateBlocks return?
leftIR = self.leftIR.val
rightIR = self.rightIR.val
frontLeftIR = self.frontLeftIR.val
frontRightIR = self.frontRightIR.val
# if len(self.leftIRVals) == 100:
# print("Reading from averaged values")
# self.leftIRVals.append(leftIR)
# self.leftIRVals.popleft()
# leftIR = sum(leftIRVals)/100
# if len(self.rightIRVals) == 100:
# self.rightIRVals.append(rightIR)
# self.rightIRVals.popleft()
# rightIR = sum(self.rightIRVals)/100
# if len(self.frontLeftIRVals) == 100:
# self.frontLeftIRVals.append(frontLeftIR)
# self.frontLeftIRVals.popleft()
# frontLeftIR = sum(self.frontLeftIRVals)/100
# if len(self.frontRightIRVals) == 100:
# self.frontRightIRVals.append(frontRightIR)
# self.frontRightIRVals.popleft()
# frontRightIR = sum(self.frontRightIRVals)/100
ret, frame = self.cam.read()
img = cv2.resize(frame,None,fx=0.25, fy=0.25, interpolation = cv2.INTER_AREA)
print("VideoFrame captured: ", ret)
return Inputs(distance_traveled, self.gyro.val, frontRightIR, frontLeftIR, leftIR, rightIR, self.finishedCollectingBlock, img)
# self.leftIR.val, self.rightIR.val, self.color.r, self.color.g, self.color.b)
# distance_traveled, theta, frontRightIR, frontLeftIR, leftIR, rightIR
def processOutputs(self, Outputs):
# TODO Missing servo outputs
if (Outputs.driving == True):
self.motorval = 50 #25?
else:
self.motorval = 0
if (Outputs.turning == True):
# if we turn, then update self.desiredAngle
self.desiredAngle = self.gyro.val
if (Outputs.turn_clockwise == True):
self.PID(self.desiredAngle + 3) # originally = 5
else:
self.PID(self.desiredAngle - 3 ) # originally = 5
else:
# self.PID(self.gyro.val)
self.PID(self.desiredAngle)
if Outputs.isCollectingBlock and not self.finishedCollectingBlock:
# Gripper is at level 2 and closed
if self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_CLOSE_POS:
self.closeOrOpenGripper()
# Gripper is at level 2 and open
elif self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_OPEN_POS:
self.moveGripper()
# Gripper is at level 1 and open
elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_OPEN_POS:
self.closeOrOpenGripper()
# Gripper is at level 1 and closed
elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_CLOSE_POS:
self.moveGripper()
# self.finishedCollectingBlock = True
time.sleep(2)
self.blocksCollected += 1
if (self.blocksCollected == 4):
self.finishedCollectingBlock = True
if Outputs.isDiscardingBlock and not self.finishedDiscardingBlock:
# Gripper level 2, closed
if self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_CLOSE_POS:
self.moveGripper()
# Gripper level 1, closed
elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_CLOSE_POS:
self.closeOrOpenGripper()
# Gripper level 1, open
elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_OPEN_POS:
self.moveGripper()
# Gripper level 2, open
elif self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_OPEN_POS:
self.closeOrOpenGripper()
self.finishedDiscardingBlock = True
def PID(self, desired_theta):
# Set encoder to 0 after turning.
# To turn in place, set bias (i.e. motorval to 0)
estimated = self.gyro.val # TODO: calculate estimated with encoder
# print(self.gyro.val)
diff = desired_theta - estimated
# print diff
self.integral += diff * self.dT
derivative = (diff - self.last_diff)/self.dT
print(derivative) # check this for timeout?
# if derivative > x:
# self.goIntoTimeoutMode = True
power = self.P*diff + self.I*self.integral + self.D*derivative # NOTE: Cap self.D*derivative, use as timeout
# print("motorLeft: ", min(255, abs(self.motorval + power)))
# print("motorRight: ", min(255, abs(self.motorval - power)))
self.motorLeft.write((self.motorval + power)>0, min(255, abs(self.motorval + power)))
self.motorRight.write((self.motorval - power)>0, min(255, abs(self.motorval - power)))
# print "EncoderLeft: " + str(self.encoderLeft.val)
# print "EncoderRight: " + str(self.encoderRight.val)
def moveGripper(self):
if self.currentGripperLevel == 1:
self.motorGripper.write(self.GRIPPER_UP, 100)
timeToSleep = 1.3
if (self.blocksCollected == 1):
timeToSleep = 1.4
elif (self.blocksCollected == 2):
timeToSleep = 1.55
elif (self.blocksCollected == 3):
timeToSleep = 1.7
elif self.currentGripperLevel == 2:
self.motorGripper.write(self.GRIPPER_DOWN, 100)
timeToSleep = 0.9
time.sleep(timeToSleep)
self.motorGripper.write(1,0)
if self.currentGripperLevel == 1:
self.currentGripperLevel = 2
else:
self.currentGripperLevel = 1
def closeOrOpenGripper(self):
if (self.servovalGripper > self.GRIPPER_OPEN_POS):
while(self.servovalGripper > self.GRIPPER_OPEN_POS):
if (self.timerGripper.millis() > 1):
self.timerGripper.reset()
self.servovalGripper -= 1
self.servoGripper.write(abs(self.servovalGripper))
elif (self.servovalGripper < self.GRIPPER_CLOSE_POS):
while(self.servovalGripper < self.GRIPPER_CLOSE_POS):
if (self.timerGripper.millis() > 1):
self.timerGripper.reset()
self.servovalGripper += 1
self.servoGripper.write(abs(self.servovalGripper))
time.sleep(.1)
def openDoorAndBuildTower(self):
self.moveGripper() # should be blocking
self.closeOrOpenGripper()
while(self.servovalDoor > self.DOOR_OPEN_POS):
if (self.timerDoor.millis() > 10):
self.timerDoor.reset()
self.servovalDoor -= 1
# print self.servovalDoor
self.servoDoor.write(abs(self.servovalDoor))
