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))