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 KitBot(ROSSketch): """ROS Node that controls the KitBod via the Teensy and tamproxy""" # PIN MAPPINGS LMOTOR_PINS = (2, 3) # DIR, PWM RMOTOR_PINS = (4, 5) # DIR, PWM def setup(self): """One-time method that sets up the robot, like in Arduino""" # Create the motor objects self.lmotor = Motor(self.tamp, *self.LMOTOR_PINS) self.rmotor = Motor(self.tamp, *self.RMOTOR_PINS) # Create a subscriber to listen for drive motor commands self.drive_sub = rospy.Subscriber( 'drive_cmd', kitware.msg.DriveCMD, self.drive_callback) def loop(self): """Method that loops at a fast rate, like in Arduino""" pass def speed_to_dir_pwm(self, speed): """Converts floating point speed (-1.0 to 1.0) to dir and pwm values""" speed = max(min(speed, 1), -1) return speed > 0, int(abs(speed * 255)) def drive_callback(self, msg): """Processes a new drive command and controls motors appropriately""" self.lmotor.write(*self.speed_to_dir_pwm(msg.l_speed)) self.rmotor.write(*self.speed_to_dir_pwm(msg.r_speed))
class MotorWrite(SyncedSketch): def setup(self): # Motor object representing the conveyor belt motor. self.conveyorMotor = Motor(self.tamp, 7, 6) #self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION, HUGS_MOTOR_CONTROLLER_PWM) self.start() def loop(self): self.conveyorMotor.write(1, CONVEYOR_POWER) #0 for up, 1 for down
class Simple(Sketch): def setup(self): self.motor = Motor(self.tamp, 11, 9) self.tof1 = TimeOfFlight(self.tamp, 20, 1) self.tof2 = TimeOfFlight(self.tamp, 22, 2) self.tof1.enable() self.tof2.enable() def loop(self): sleep(0.01) print self.tof1.dist, self.tof2.dist self.motor.write(1, 50)
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 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() 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 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 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 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 KitBot(ROSSketch): """ROS Node that controls the KitBod via the Teensy and tamproxy""" # PIN MAPPINGS LMOTOR_PINS = (2, 3) # DIR, PWM RMOTOR_PINS = (4, 5) # DIR, PWM IR_PINS = [14, 15, 16] def setup(self): """One-time method that sets up the robot, like in Arduino""" # Create the motor and short IR objects self.lmotor = Motor(self.tamp, *self.LMOTOR_PINS) self.rmotor = Motor(self.tamp, *self.RMOTOR_PINS) self.irs = [AnalogInput(self.tamp, p) for p in self.IR_PINS] # Create a publisher to publish short IR sensor readings self.ir_pub = rospy.Publisher('ir_read', kitware.msg.IRRead, queue_size=10) # Create a subscriber to listen for drive motor commands self.drive_sub = rospy.Subscriber('drive_cmd', kitware.msg.DriveCMD, self.drive_callback) def loop(self): """Method that loops at a fast rate, like in Arduino""" # Create an IR read message containing the short IR analog values ir_msg = kitware.msg.IRRead() ir_msg.data = [ir.val for ir in self.irs] # Publish the IR readings self.ir_pub.publish(ir_msg) def speed_to_dir_pwm(self, speed): """Converts floating point speed (-1.0 to 1.0) to dir and pwm values""" speed = max(min(speed, 1), -1) return speed > 0, int(abs(speed * 255)) def drive_callback(self, msg): """Processes a new drive command and controls motors appropriately""" self.lmotor.write(*self.speed_to_dir_pwm(msg.l_speed)) self.rmotor.write(*self.speed_to_dir_pwm(msg.r_speed))
class Actuators: def __init__(self, tamp): self.tamp = tamp #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorL.write(1, 0) #motor right self.motorR = Motor(self.tamp, 1, 3) self.motorR.write(1, 0) #arm servo self.arm = Arm(self.tamp) #sorting servo self.sorter = Sorter(self.tamp) def update(self): self.arm.update() self.sorter.update()
class Actuators: def __init__(self,tamp): self.tamp=tamp #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorL.write(1,0) #motor right self.motorR = Motor(self.tamp,1, 3) self.motorR.write(1,0) #arm servo self.arm = Arm(self.tamp) #sorting servo self.sorter = Sorter(self.tamp) def update(self): self.arm.update() self.sorter.update()
class KitBot(ROSSketch): """ROS Node that controls the KitBot via the Teensy and tamproxy""" # current mappings # left motor: M1, right motor: M2 # green: GND # blue: 2, pwm2 # purple: 3, dir2 # grey: 4, pwm1 # white: 5, dir1 # PIN MAPPINGS LMOTOR_PINS = (5, 4) # DIR1 (white), PWM1 (grey) RMOTOR_PINS = (3, 2) # DIR2 (purple), PWM2 (blue) def setup(self): """One-time method that sets up the robot, like in Arduino""" # Create the motor objects self.lmotor = Motor(self.tamp, *self.LMOTOR_PINS) self.rmotor = Motor(self.tamp, *self.RMOTOR_PINS) # Create a subscriber to listen for drive motor commands self.drive_sub = rospy.Subscriber('drive_cmd', kitware.msg.DriveCMD, self.drive_callback) def loop(self): """Method that loops at a fast rate, like in Arduino""" pass def speed_to_dir_pwm(self, speed): """Converts floating point speed (-1.0 to 1.0) to dir and pwm values""" speed = max(min(speed, 1), -1) return speed > 0, int(abs(speed * 255)) def drive_callback(self, msg): """Processes a new drive command and controls motors appropriately""" self.lmotor.write(*self.speed_to_dir_pwm(msg.l_speed)) self.rmotor.write(*self.speed_to_dir_pwm(msg.r_speed))
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 MotorWrite(Sketch): 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 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)) if(self.counter >= 2000 and self.spin_forwards): self.counter = 0 self.spin_forwards = False elif(self.counter >= 500 and not self.spin_forwards): self.counter = 0 self.spin_forwards = True self.motor.write(self.spin_forwards, 250) self.counter += 10
class MotorWrite(Sketch): 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 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)) if (self.counter >= 2000 and self.spin_forwards): self.counter = 0 self.spin_forwards = False elif (self.counter >= 500 and not self.spin_forwards): self.counter = 0 self.spin_forwards = True self.motor.write(self.spin_forwards, 250) self.counter += 10
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 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 MyRobot(SyncedSketch): runtime = 300000 #ms 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 loop(self): if self.timer.millis() > self.dT*1000: 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__ # print(self.gyro.val) 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 = 0 #(self.encoderLeft.val + self.encoderRight.val) / 2.0 leftIR = self.leftIR.val rightIR = self.rightIR.val frontLeftIR = self.frontLeftIR.val frontRightIR = self.frontRightIR.val return Inputs(distance_traveled, self.gyro.val, frontRightIR, frontLeftIR, leftIR, rightIR, self.finishedCollectingBlock) def processOutputs(self, Outputs): # TODO Missing servo outputs if (Outputs.driving == True): self.motorval = 50 #25? else: self.motorval = 0 if (Outputs.isDiscardingBlock == True): self.motorval = -50 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 + 5) else: self.PID(self.desiredAngle - 5) else: self.PID(self.desiredAngle) 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 power = self.P*diff + self.I*self.integral + self.D*derivative # NOTE: Cap self.D*derivative, use as timeout # print("motorLeft: ", min(75, abs(self.motorval + power))) # print("motorRight: ", min(75, abs(self.motorval - power))) self.motorLeft.write((self.motorval + power)>0, min(75, abs(self.motorval + power))) self.motorRight.write((self.motorval - power)>0, min(75, abs(self.motorval - power)))
class Robot(SyncedSketch): def setup(self): #################### #### EE SETUP #### #################### # 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) # 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) # Motor object representing the conveyor belt motor. self.conveyorMotor = Motor(self.tamp, BELT_MOTOR_CONTROLLER_DIRECTION, BELT_MOTOR_CONTROLLER_PWM) # Encoder object for the conveyor belt motor. self.conveyorEncoder = Encoder(self.tamp, BELT_MOTOR_ENCODER_YELLOW, BELT_MOTOR_ENCODER_WHITE) # Long range IR sensors self.irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL) self.irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR) self.irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL) self.irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR) # Color sensor self.color = Color(self.tamp) # Limit switches self.conveyorLimSwitch = DigitalInput(self.tamp, CONVEYOR_LIMIT_SWITCH) self.blockLimSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH) # Servo controlling the door of the collection chamber. self.backDoorServo = Servo(self.tamp, SERVO_PIN) # Make sure the door is closed self.backDoorServo.write(SERVO_CLOSE) # The switch that tells the program that the competition has started self.competitionModeSwitch = DigitalInput(self.tamp, COMPETITION_MODE) ################################# #### INTERNAL MODULE SETUP #### ################################# # Timer object to moderate checking for intake errors. self.intakeTimer = Timer() # Are the intake motors reversing? True if so, False if going forwards. self.intakeDirection = False # Start the intake motor. self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) # Wall Follow object self.wallFollow = WallFollow(self.leftMotor, self.rightMotor, Timer(), self.irFL, self.irFR, self.irBL, self.irBR) # A timer to make sure timesteps are only 10 times/second self.timestepTimer = Timer() # Timer object describing how long the current module has been running. self.moduleTimer = Timer() # Timer for the whole game self.gameTimer = Timer() # Runs the PICKUP process self.pickup = PickupModule(self.moduleTimer, self.conveyorLimSwitch, self.conveyorMotor, self.conveyorEncoder) # Runs the DROPOFF process self.dropoff = DropoffModule(self.moduleTimer, self.backDoorServo, self.rightMotor, self.leftMotor, self.rightEncoder) # Runs the FOLLOW process TODO: Fix forward to actually mean forward. self.follow = FollowModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.wallFollow, FORWARD_SPEED, self.blockLimSwitch) # Runs the CHECK process. TODO: pass in proper timers. self.check = CheckModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.color) # Waits for the game to start self.off = OffModule(self.gameTimer, self.competitionModeSwitch) # Describes which stage of the program is running. self.module = MODULE_OFF def loop(self): if self.timestepTimer.millis() > 100: self.timestepTimer.reset() #print "Module Number", self.module state = -1 if self.module == MODULE_OFF: state = self.off.run() elif self.module == MODULE_CHECK: state = self.check.run() elif self.module == MODULE_PICKUP: state = self.pickup.run() elif self.module == MODULE_DROPOFF: self.module = MODULE_FOLLOW state = self.follow.run() elif self.module == MODULE_FOLLOW: state = self.follow.run() else: print "Attempting to run nonexistent module" self.stop() self.updateState(state) if self.gameTimer.millis() % 10000 == 0: print "Game Time: ", self.gameTimer.millis() if self.gameTimer.millis() > GAME_LENGTH - 500: self.backDoorServo.write(SERVO_OPEN) self.leftMotor.write(0, 0) self.rightMotor.write(0, 0) if self.gameTimer.millis() > GAME_LENGTH: self.module = MODULE_END self.stop() return ## Switch module if necessary. def updateState(self, module): if self.module == module: return elif module == MODULE_OFF: self.off.start() self.module = MODULE_OFF return elif module == MODULE_CHECK: self.check.start() self.module = MODULE_CHECK return elif module == MODULE_PICKUP: self.pickup.start() self.module = MODULE_PICKUP return elif module == MODULE_DROPOFF: self.dropoff.start() self.module = MODULE_DROPOFF return elif module == MODULE_FOLLOW: self.follow.start() self.module = MODULE_FOLLOW return else: print "Attempting to switch to nonexistent module" self.stop() ## Make sure that the intake motor does not stall. # If so, reverse the intake motors. # # @param checkTime Time in ms between checking stalls. # @param reverseTime Time in ms that the intake motors will reverse if needed. def checkForIntakeErrors(self, checkTime=100, reverseTime=800): 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 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 SortBlocks(SyncedSketch): pins = 31, 32 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 def loop(self): if self.timer.millis() > 300: self.timer.reset() #self.conveyor_encoder.update() encoder_value = self.conveyor_encoder.val #print self.prev_encoder_value #print encoder_value if(encoder_value == self.prev_encoder_value): self.jammed = True else: self.jammed = False self.prev_encoder_value = encoder_value self.motor.write(not self.jammed,250) # 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) # else: # self.servo.write(self.CENTER + self.sorter_delta) # self.sorter_delta *= -1 self.counter += 100
class PIDDrive(SyncedSketch): ss_pin = 10 #gyro select pin image_pipe = './image' if not os.path.exists(image_pipe): os.mkfifo(image_pipe) image_fd = os.open(image_pipe, os.O_RDONLY) 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 loop(self): #state 0 - wait for first no block, do nothing #transition: no block -> state 1 #state 1 - look for block #transition: found block -> state 2 #sate 2 - drive over block #transition: ir triggered -> state 3 #sate 3 - pick up block #transition: color sensor done -> sate 1 message = os.read(self.image_fd, 20) if message: # print("Recieved: '%s'" % message) if message[:2] == 'no': self.blockAngle = 0 if self.State == 0: self.State = 1 if self.State == 2: self.State = 3 else: if self.State != 0: self.State = 2 try: self.blockDistance, self.blockAngle = [number[:6] for number in message.split(',')] except: self.blockAngle = 0 self.blockAngle = float(self.blockAngle) if self.timer.millis() > 100: for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.display.quit() if event.type == pygame.KEYDOWN: if event.key == pygame.K_LEFT: self.turnVel = -10 if event.key == pygame.K_RIGHT: self.turnVel = 10 if event.key == pygame.K_UP: self.fwdVel = 100 if event.key == pygame.K_DOWN: self.fwdVel = -100 if event.key == pygame.K_1: self.Sort = 1 if event.key == pygame.K_2: self.Sort = 2 if event.key == pygame.K_SPACE: self.MoveArm = True if event.type == pygame.KEYUP: if event.key == pygame.K_LEFT or event.key == pygame.K_RIGHT: self.turnVel = 0 self.turn = 0 if event.key == pygame.K_UP or event.key == pygame.K_DOWN: self.fwdVel = 0 print 'Color' print self.color.c print self.color.r, self.color.g print if self.color.c > 800 and self.Sort == 0: if self.color.r > self.color.g: self.Sort = 1 else: self.Sort = 2 # print self.uIR.val if self.uIR.val == 0: self.MoveArm = True self.State = 0 if self.MoveArm: if self.Bottom == 2 and self.Top == 1: self.delta = 0 self.servoval = self.servo.bottom self.servo.write(self.servo.bottom) self.Bottom, self.Top = 0, 0 self.MoveArm = False elif self.servoval >= self.servo.top: time.sleep(1) self.delta = -self.servo.speed self.Top = 1 elif self.servoval <= self.servo.bottom: self.delta = self.servo.speed self.Bottom = 1 if self.Top == 1: self.Bottom = 2 if self.Sort == 1: if self.sorterval < self.sorter.left: self.dsorter = self.sorter.speed elif self.sorterval >= self.sorter.left: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 2: if self.sorterval > self.sorter.right: self.dsorter = -self.sorter.speed elif self.sorterval <= self.sorter.right: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 3: if time.time() - self.SortPause > 1: self.sorterval = self.sorter.center self.Sort = 0 self.sorterval += self.dsorter self.sorter.write(abs(self.sorterval)) self.servoval += self.delta self.servo.write(abs(self.servoval)) self.turn += self.turnVel self.timer.reset() #response = self.rp.read() #print "Got response %s" % response # Valid gyro status is [0,1], see datasheet on ST1:ST0 bits cAngle=self.gyro.val - self.totalDrift #corrected angle # print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift self.prevGyro=self.gyro.val self.totalDrift+=self.drift # print 'State: ' + str(self.Searching) self.newAngle = cAngle + self.blockAngle print self.blockAngle pidResult=self.PID.valuePID(cAngle, cAngle+self.blockAngle+self.turn) # print 'Angle Dif: ' + str(cAngle-self.initAngle) + '\tPID RESULT: '+ str(pidResult) # print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str(self.encoderL.val) # print 'AVG: ' + str((self.encoderR.val + self.encoderL.val)/2.) self.motorLdrive = self.fwdVel - pidResult self.motorRdrive = self.fwdVel + pidResult self.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive)) self.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive)) '''
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() 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 loop(self): if self.timer.millis() > self.dT * 1000: # print("self.gameTimer", self.gameTimer) # print(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__ # print(self.gyro.val) 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 = 0 #(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) # Camera ret, frame = self.cam.read() img = cv2.resize(frame, None, fx=0.25, fy=0.25, interpolation=cv2.INTER_AREA) blocks = CalculateBlocks(img) #what should CalculateBlocks return? leftIR = self.leftIR.val rightIR = self.rightIR.val frontLeftIR = self.frontLeftIR.val frontRightIR = self.frontRightIR.val return Inputs(distance_traveled, self.gyro.val, frontRightIR, frontLeftIR, leftIR, rightIR, self.finishedCollectingBlock, blocks, self.color.r, self.color.g, self.color.b) 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) else: self.PID(self.desiredAngle - 3) 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 power = self.P * diff + self.I * self.integral + self.D * derivative # NOTE: Cap self.D*derivative, use as timeout # print("motorLeft: ", min(75, abs(self.motorval + power))) # print("motorRight: ", min(75, abs(self.motorval - power))) self.motorLeft.write((self.motorval + power) > 0, min(75, abs(self.motorval + power))) self.motorRight.write((self.motorval - power) > 0, min(75, 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 SemiBot(ROSSketch): """ROS Node that controls the SemiBot via the Teensy and tamproxy""" def setup(self): """One-time method that sets up the robot, like in Arduino""" ##--------------------------------------------## ##--------- DRIVING MOTORS (+encoder) --------## ##--------------------------------------------## # Create the motor objects self.lmotor = Motor(self.tamp, *LMOTOR_PINS) self.rmotor = Motor(self.tamp, *RMOTOR_PINS) # Create a subscriber to listen for drive motor commands rospy.Subscriber('drive_cmd', kitware.msg.DriveCMD, self.drive_callback) ## --------- ## if ENCODER: # encoder objects self.lmotor_encoder = Encoder(self.tamp, *LMOTOR_ENCODER_PINS, continuous=True) self.rmotor_encoder = Encoder(self.tamp, *RMOTOR_ENCODER_PINS, continuous=True) # Publishers for encoder values self.pub_lmotor_encoder_val = rospy.Publisher('lmotor_encoder_val', Int64, queue_size=10) self.pub_rmotor_encoder_val = rospy.Publisher('rmotor_encoder_val', Int64, queue_size=10) ##--------------------------------------------## ##--------------- INTAKE MOTOR ---------------## ##--------------------------------------------## # Create the motor object self.intake_motor = Motor(self.tamp, *INTAKE_MOTOR_PINS) # Create a subscriber to listen for intake motor commands (passively continuous) rospy.Subscriber('intake_cmd', kitware.msg.DriveCMD, self.intake_callback) ##--------------------------------------------## ##-------------- DISPENSER SERVO -------------## ##--------------------------------------------## # Setup servo self.servo = Servo(self.tamp, SERVO_PIN) self.servo.write(0) self.servo_val = 0 self.delta = DISPENSOR_SPEED self.end = False # Subscriber to listen for drive motor commands rospy.Subscriber('dispenser_servo_speed', Int64, self.dispenser_servo_callback) # Publisher for current servo value self.pub_dispenser_servo_val = rospy.Publisher('dispenser_servo_val', Int64, queue_size=10) ##--------------------------------------------## ##--------- DISPENSER MOTOR (+encoder) -------## ##--------------------------------------------## # Create the motor object self.dispenser_motor = Motor(self.tamp, *DISPENSER_MOTOR_PINS) # Create a subscriber to listen for dispenser motor commands rospy.Subscriber('dispenser_cmd', kitware.msg.DriveCMD, self.dispenser_motor_callback) ## --------- ## if ENCODER: # encoder objects self.dispenser_motor_encoder = Encoder( self.tamp, *DISPENSER_MOTOR_ENCODER_PINS, continuous=True) # Publishers for encoder values self.pub_dispenser_motor_encoder_val = rospy.Publisher( 'dispenser_motor_encoder_val', Int64, queue_size=10) ##--------------------------------------------## ##---------- General ---------## ##--------------------------------------------## # timer for loop() self.timer = Timer() # reset all motor encoders self.lmotor_encoder.val = 0 self.rmotor_encoder.val = 0 self.dispenser_motor_encoder.val = 0 ##--------- GENERAL ---------## def loop(self): """Method that loops at a fast rate, like in Arduino (isn't looping???)""" #rospy.loginfo("looping!") if self.timer.millis() > 100: self.timer.reset() self.publish_servo_val() self.publish_encoder_val() pass def speed_to_dir_pwm(self, speed): """Converts floating point speed (-1.0 to 1.0) to dir and pwm values""" speed = max(min(speed, 1), -1) return speed > 0, int(abs(speed * 255)) ##--------- ENCODERS ---------## def publish_encoder_val(self): if ENCODER: self.pub_lmotor_encoder_val.publish(self.lmotor_encoder.val) self.pub_rmotor_encoder_val.publish(self.rmotor_encoder.val) self.pub_dispenser_motor_encoder_val.publish( self.dispenser_motor_encoder.val) ##--------- DRIVING MOTORS ---------## def drive_callback(self, msg): """Processes a new drive command and controls motors appropriately""" self.lmotor.write(*self.speed_to_dir_pwm(msg.l_speed)) self.rmotor.write(*self.speed_to_dir_pwm(msg.r_speed)) ##--------- INTAKE MOTOR ---------## def intake_callback(self, msg): """Processes a new drive command and controls motors appropriately""" self.intake_motor.write(*self.speed_to_dir_pwm( msg.l_speed)) # use lspeed ##--------- DISPENSER SERVO ---------## def publish_servo_val(self): self.pub_dispenser_servo_val.publish(self.servo_val) def dispenser_servo_callback(self, msg): # write to servo self.delta = msg.data self.servo_val += self.delta self.servo.write(abs(self.servo_val)) ##--------- DISPENSER MOTOR ---------## def dispenser_motor_callback(self, msg): """Processes a new drive command and controls motors appropriately""" self.dispenser_motor.write(*self.speed_to_dir_pwm( msg.l_speed)) # use LEFT motor: l_speed
class Robot(SyncedSketch): STRAIGHT_STATE = 0 REVERSE_STATE = 1 TURN_STATE = 2 OTHER_REVERSE_STATE = 3 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 goStraight(self, direction, pwm): self.leftMotor.write(direction, pwm) self.rightMotor.write(direction, pwm) def turn(self, direction, pwm): self.leftMotor.write(direction, pwm) self.rightMotor.write(1 - direction, pwm) def loop(self): if self.comp_timer.millis() < 2000: self.goStraight(0, 100) if self.comp_timer.millis() > 2000: if self.state == self.STRAIGHT_STATE: self.goStraight(0, 100) if self.bumper_RF.val == 0: self.turn_direction = LEFT self.turn_pwm = 60 if self.comp_timer.millis() > 90000: self.turn_pwm = 75 if self.comp_timer.millis() > 105000: self.turn_pwm = 90 self.state = self.REVERSE_STATE self.reverse_timer.reset() if self.bumper_LF.val == 0: self.turn_direction = RIGHT self.turn_pwm = 90 if self.comp_timer.millis() > 90000: self.turn_pwm = 50 if self.comp_timer.millis() > 105000: self.turn_pwm = 60 self.state = self.REVERSE_STATE self.reverse_timer.reset() if self.bumper_RS.val == 0: self.turn_direction = LEFT self.turn_pwm = 40 self.state = self.OTHER_REVERSE_STATE self.reverse_timer.reset() if self.bumper_LS.val == 0: self.turn_direction = RIGHT self.turn_pwm = 40 if self.comp_timer.millis() > 90000: self.turn_direction = 0 self.turn_pwm = 50 self.state = self.OTHER_REVERSE_STATE self.reverse_timer.reset() if self.straight_timer.millis() > 7000: if self.comp_timer.millis() % 2 == 0: self.turn_direction = RIGHT self.turn_pwn = 40 else: self.turn_direction = LEFT self.turn_pwm = 40 self.state = self.REVERSE_STATE self.reverse_timer.reset() elif self.state == self.REVERSE_STATE: self.goStraight(1, 50) if self.reverse_timer.millis() > 800: self.state = self.TURN_STATE self.turn_timer.reset() elif self.state == self.OTHER_REVERSE_STATE: self.goStraight(1, 50) if self.reverse_timer.millis() > 500: self.state = self.TURN_STATE self.turn_timer.reset() elif self.state == self.TURN_STATE: self.turn(self.turn_direction, self.turn_pwm) if self.turn_timer.millis() > 600: self.state = self.STRAIGHT_STATE self.straight_timer.reset()
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 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() 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 loop(self): if self.timer.millis() > self.dT*1000: # print("self.gameTimer", self.gameTimer) # print(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__ # print(self.gyro.val) 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 = 0 #(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) # Camera ret, frame = self.cam.read() img = cv2.resize(frame,None,fx=0.25, fy=0.25, interpolation = cv2.INTER_AREA) blocks = CalculateBlocks(img); #what should CalculateBlocks return? leftIR = self.leftIR.val rightIR = self.rightIR.val frontLeftIR = self.frontLeftIR.val frontRightIR = self.frontRightIR.val return Inputs(distance_traveled, self.gyro.val, frontRightIR, frontLeftIR, leftIR, rightIR, self.finishedCollectingBlock, blocks, self.color.r, self.color.g, self.color.b) 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) else: self.PID(self.desiredAngle - 3) 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 power = self.P*diff + self.I*self.integral + self.D*derivative # NOTE: Cap self.D*derivative, use as timeout # print("motorLeft: ", min(75, abs(self.motorval + power))) # print("motorRight: ", min(75, abs(self.motorval - power))) self.motorLeft.write((self.motorval + power)>0, min(75, abs(self.motorval + power))) self.motorRight.write((self.motorval - power)>0, min(75, 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 PIDDrive(SyncedSketch): ss_pin = 10 #gyro select pin image_pipe = './image' if not os.path.exists(image_pipe): os.mkfifo(image_pipe) image_fd = os.open(image_pipe, os.O_RDONLY) 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 loop(self): #state 0 - wait for first no block, do nothing #transition: no block -> state 1 #state 1 - look for block #transition: found block -> state 2 #sate 2 - drive over block #transition: ir triggered -> state 3 #sate 3 - pick up block #transition: color sensor done -> sate 1 message = os.read(self.image_fd, 20) if message: # print("Recieved: '%s'" % message) if message[:2] == 'no': self.blockAngle = 0 if self.State == 0: self.State = 1 if self.State == 2: self.State = 3 else: if self.State != 0: self.State = 2 try: self.blockDistance, self.blockAngle = [ number[:6] for number in message.split(',') ] except: self.blockAngle = 0 self.blockAngle = float(self.blockAngle) if self.timer.millis() > 100: for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.display.quit() if event.type == pygame.KEYDOWN: if event.key == pygame.K_LEFT: self.turnVel = -10 if event.key == pygame.K_RIGHT: self.turnVel = 10 if event.key == pygame.K_UP: self.fwdVel = 100 if event.key == pygame.K_DOWN: self.fwdVel = -100 if event.key == pygame.K_1: self.Sort = 1 if event.key == pygame.K_2: self.Sort = 2 if event.key == pygame.K_SPACE: self.MoveArm = True if event.type == pygame.KEYUP: if event.key == pygame.K_LEFT or event.key == pygame.K_RIGHT: self.turnVel = 0 self.turn = 0 if event.key == pygame.K_UP or event.key == pygame.K_DOWN: self.fwdVel = 0 print 'Color' print self.color.c print self.color.r, self.color.g print if self.color.c > 800 and self.Sort == 0: if self.color.r > self.color.g: self.Sort = 1 else: self.Sort = 2 # print self.uIR.val if self.uIR.val == 0: self.MoveArm = True self.State = 0 if self.MoveArm: if self.Bottom == 2 and self.Top == 1: self.delta = 0 self.servoval = self.servo.bottom self.servo.write(self.servo.bottom) self.Bottom, self.Top = 0, 0 self.MoveArm = False elif self.servoval >= self.servo.top: time.sleep(1) self.delta = -self.servo.speed self.Top = 1 elif self.servoval <= self.servo.bottom: self.delta = self.servo.speed self.Bottom = 1 if self.Top == 1: self.Bottom = 2 if self.Sort == 1: if self.sorterval < self.sorter.left: self.dsorter = self.sorter.speed elif self.sorterval >= self.sorter.left: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 2: if self.sorterval > self.sorter.right: self.dsorter = -self.sorter.speed elif self.sorterval <= self.sorter.right: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 3: if time.time() - self.SortPause > 1: self.sorterval = self.sorter.center self.Sort = 0 self.sorterval += self.dsorter self.sorter.write(abs(self.sorterval)) self.servoval += self.delta self.servo.write(abs(self.servoval)) self.turn += self.turnVel self.timer.reset() #response = self.rp.read() #print "Got response %s" % response # Valid gyro status is [0,1], see datasheet on ST1:ST0 bits cAngle = self.gyro.val - self.totalDrift #corrected angle # print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift self.prevGyro = self.gyro.val self.totalDrift += self.drift # print 'State: ' + str(self.Searching) self.newAngle = cAngle + self.blockAngle print self.blockAngle pidResult = self.PID.valuePID(cAngle, cAngle + self.blockAngle + self.turn) # print 'Angle Dif: ' + str(cAngle-self.initAngle) + '\tPID RESULT: '+ str(pidResult) # print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str(self.encoderL.val) # print 'AVG: ' + str((self.encoderR.val + self.encoderL.val)/2.) self.motorLdrive = self.fwdVel - pidResult self.motorRdrive = self.fwdVel + pidResult self.motorL.write(self.motorLdrive < 0, abs(self.motorLdrive)) self.motorR.write(self.motorRdrive < 0, abs(self.motorRdrive)) '''
class Robot(SyncedSketch): def setup(self): #################### #### EE SETUP #### #################### # 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) # 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) # Motor object representing the conveyor belt motor. self.conveyorMotor = Motor(self.tamp, BELT_MOTOR_CONTROLLER_DIRECTION, BELT_MOTOR_CONTROLLER_PWM) # Encoder object for the conveyor belt motor. self.conveyorEncoder = Encoder(self.tamp, BELT_MOTOR_ENCODER_YELLOW, BELT_MOTOR_ENCODER_WHITE) # Long range IR sensors self.irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL) self.irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR) self.irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL) self.irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR) # Color sensor self.color = Color(self.tamp) # Limit switches self.conveyorLimSwitch = DigitalInput(self.tamp, CONVEYOR_LIMIT_SWITCH) self.blockLimSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH) # Servo controlling the door of the collection chamber. self.backDoorServo = Servo(self.tamp, SERVO_PIN) # Make sure the door is closed self.backDoorServo.write(SERVO_CLOSE) # The switch that tells the program that the competition has started self.competitionModeSwitch = DigitalInput(self.tamp, COMPETITION_MODE) ################################# #### INTERNAL MODULE SETUP #### ################################# # Timer object to moderate checking for intake errors. self.intakeTimer = Timer() # Are the intake motors reversing? True if so, False if going forwards. self.intakeDirection = False # Start the intake motor. self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) # Wall Follow object self.wallFollow = WallFollow(self.leftMotor, self.rightMotor, Timer(), self.irFL, self.irFR, self.irBL, self.irBR) # A timer to make sure timesteps are only 10 times/second self.timestepTimer = Timer() # Timer object describing how long the current module has been running. self.moduleTimer = Timer() # Timer for the whole game self.gameTimer = Timer() # Runs the PICKUP process self.pickup = PickupModule(self.moduleTimer, self.conveyorLimSwitch, self.conveyorMotor, self.conveyorEncoder) # Runs the DROPOFF process self.dropoff = DropoffModule(self.moduleTimer, self.backDoorServo, self.rightMotor, self.leftMotor, self.rightEncoder) # Runs the FOLLOW process TODO: Fix forward to actually mean forward. self.follow = FollowModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.wallFollow, FORWARD_SPEED, self.blockLimSwitch) # Runs the CHECK process. TODO: pass in proper timers. self.check = CheckModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.color) # Waits for the game to start self.off = OffModule(self.gameTimer, self.competitionModeSwitch) # Describes which stage of the program is running. self.module = MODULE_OFF def loop(self): if self.timestepTimer.millis() > 100: self.timestepTimer.reset() #print "Module Number", self.module state = -1 if self.module == MODULE_OFF: state = self.off.run() elif self.module == MODULE_CHECK: state = self.check.run() elif self.module == MODULE_PICKUP: state = self.pickup.run() elif self.module == MODULE_DROPOFF: self.module = MODULE_FOLLOW state = self.follow.run() elif self.module == MODULE_FOLLOW: state = self.follow.run() else: print "Attempting to run nonexistent module" self.stop() self.updateState(state) if self.gameTimer.millis() % 10000 == 0: print "Game Time: ", self.gameTimer.millis() if self.gameTimer.millis() > GAME_LENGTH - 500: self.backDoorServo.write(SERVO_OPEN) self.leftMotor.write(0,0) self.rightMotor.write(0,0) if self.gameTimer.millis() > GAME_LENGTH: self.module = MODULE_END self.stop() return ## Switch module if necessary. def updateState(self, module): if self.module == module: return elif module == MODULE_OFF: self.off.start() self.module = MODULE_OFF return elif module == MODULE_CHECK: self.check.start() self.module = MODULE_CHECK return elif module == MODULE_PICKUP: self.pickup.start() self.module = MODULE_PICKUP return elif module == MODULE_DROPOFF: self.dropoff.start() self.module = MODULE_DROPOFF return elif module == MODULE_FOLLOW: self.follow.start() self.module = MODULE_FOLLOW return else: print "Attempting to switch to nonexistent module" self.stop() ## Make sure that the intake motor does not stall. # If so, reverse the intake motors. # # @param checkTime Time in ms between checking stalls. # @param reverseTime Time in ms that the intake motors will reverse if needed. def checkForIntakeErrors(self, checkTime = 100, reverseTime = 800): 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 MyRobot(SyncedSketch): runtime = 300000 #ms 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 loop(self): if self.timer.millis() > self.dT * 1000: 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__ # print(self.gyro.val) 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 = 0 #(self.encoderLeft.val + self.encoderRight.val) / 2.0 leftIR = self.leftIR.val rightIR = self.rightIR.val frontLeftIR = self.frontLeftIR.val frontRightIR = self.frontRightIR.val return Inputs(distance_traveled, self.gyro.val, frontRightIR, frontLeftIR, leftIR, rightIR, self.finishedCollectingBlock) def processOutputs(self, Outputs): # TODO Missing servo outputs if (Outputs.driving == True): self.motorval = 50 #25? else: self.motorval = 0 if (Outputs.isDiscardingBlock == True): self.motorval = -50 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 + 5) else: self.PID(self.desiredAngle - 5) else: self.PID(self.desiredAngle) 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 power = self.P * diff + self.I * self.integral + self.D * derivative # NOTE: Cap self.D*derivative, use as timeout # print("motorLeft: ", min(75, abs(self.motorval + power))) # print("motorRight: ", min(75, abs(self.motorval - power))) self.motorLeft.write((self.motorval + power) > 0, min(75, abs(self.motorval + power))) self.motorRight.write((self.motorval - power) > 0, min(75, abs(self.motorval - power)))
class pythonDriver(SyncedSketch): ss_pin = 10 #gyro select pin # also 11, 12, and 13 color_pins = 18, 19 uIR_pin = 17 Lencoder_pins = 22, 23 Rencoder_pins = 21, 20 arm_pin = 9 sorter_pin = 5 Rmoter_pins = 1, 3 Lmotor_pins = 2, 4 pipes = './gyro', './IR' for path in pipes: if not os.path.exists(path): os.mkfifo(path) 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 loop(self): if self.timer.millis() > 100: #if get message #read message '''Alternative Command Names: left right servoARM servoSORT ''' response = os.read(self.pipe_fd,100) print "Got response %s" % response #rp.close() print "no message" value, function = response.split(' ') functions[function](int(value)) ''' if response == "LEFT": self.turnVel = -5 print 'Got Key' if response == "RIGHT": self.turnVel = 5 if response == "UP": self.fwdVel = 40 if response == "DOWN": self.fwdVel = -40 if response == "SPACE": self.MoveArm = True #stop message if response =="STOP": self.turVel=0 self.fwdVel=0 if event.key == 'SORT': if self.Sort == 0: self.Sort = 1 if event.key == pygame.K_2: if self.Sort == 0: self.Sort = 2 if response == "LEFT" or response == "RIGHT": self.turnVel = 0 if response == "UP" or response == "DOWN": self.fwdVel = 0 if self.MoveArm: if self.Bottom == 2 and self.Top == 1: self.delta = 0 self.servoval = 20 self.servo.write(20) self.Bottom, self.Top = 0, 0 self.MoveArm = False elif self.servoval >= 170: self.delta = -30 self.Top = 1 elif self.servoval <= 30: self.delta = 30 self.Bottom = 1 if self.Top == 1: self.Bottom = 2 self.servoval += self.delta self.servo.write(abs(self.servoval)) print 'delta: ' + str(self.delta) print 'servoval: ' + str(self.servoval) print 'MoveArm: ' + str(self.MoveArm) self.initAngle += self.turnVel self.timer.reset() #response = self.rp.read() #print "Got response %s" % response # Valid gyro status is [0,1], see datasheet on ST1:ST0 bit''' cAngle=self.gyro.val - self.totalDrift #corrected angle #print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift '''
class TestBelt(SyncedSketch): 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() def loop(self): if self.ttt.millis() > 100: print self.conveyorEncoder.val self.ttt.reset() self.runThing() ## Set up the beginning of the pickup process. def startThing(self): self.conveyorEncoder.write(0) self.conveyorMotor.write(UP, CONVEYOR_POWER) self.timer.reset() ## Pick up a block from the block capture mechanism. # # Move the conveyor belt upwards until the encoders indicate that # the block has moved far enough. Then move the conveyor belt back. # # @return The value of the next module to return to. def runThing(self): # Allow timeout. if self.timer.millis() > self.timeout: print "Timed out from PICKUP to FIND" self.stop() encval = self.conveyorEncoder.val # Move up the conveyor belt until it hits the encoder limit. if encval > CONVEYOR_ENCODER_LIMIT: self.conveyorMotor.write(DOWN, CONVEYOR_POWER) else: self.conveyorMotor.write(UP, CONVEYOR_POWER) # Stop the motor when it gets to the bottom. if encval < 0 and self.timer.millis() > 1000: self.conveyorMotor.write(False, 0) self.blocksPickedUp += 1 # Switch modules if self.blocksPickedUp >= 4: print "Going from PICKUP to DROPOFF" self.stop() else: print "Going from PICKUP to FIND" self.stop() return
class Robot(SyncedSketch): STRAIGHT_STATE = 0 REVERSE_STATE = 1 TURN_STATE = 2 OTHER_REVERSE_STATE = 3 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 goStraight(self, direction, pwm): self.leftMotor.write(direction, pwm) self.rightMotor.write(direction, pwm) def turn(self, direction, pwm): self.leftMotor.write(direction, pwm) self.rightMotor.write(1-direction, pwm) def loop(self): if self.comp_timer.millis() < 2000: self.goStraight(0, 100) if self.comp_timer.millis() > 2000: if self.state == self.STRAIGHT_STATE: self.goStraight(0, 100) if self.bumper_RF.val == 0: self.turn_direction = LEFT self.turn_pwm = 60 if self.comp_timer.millis() > 90000: self.turn_pwm = 75 if self.comp_timer.millis() > 105000: self.turn_pwm = 90 self.state = self.REVERSE_STATE self.reverse_timer.reset() if self.bumper_LF.val == 0: self.turn_direction = RIGHT self.turn_pwm = 90 if self.comp_timer.millis() > 90000: self.turn_pwm = 50 if self.comp_timer.millis() > 105000: self.turn_pwm = 60 self.state = self.REVERSE_STATE self.reverse_timer.reset() if self.bumper_RS.val == 0: self.turn_direction = LEFT self.turn_pwm = 40 self.state = self.OTHER_REVERSE_STATE self.reverse_timer.reset() if self.bumper_LS.val == 0: self.turn_direction = RIGHT self.turn_pwm = 40 if self.comp_timer.millis() > 90000: self.turn_direction = 0 self.turn_pwm = 50 self.state = self.OTHER_REVERSE_STATE self.reverse_timer.reset() if self.straight_timer.millis() > 7000: if self.comp_timer.millis() % 2 == 0: self.turn_direction = RIGHT self.turn_pwn = 40 else: self.turn_direction = LEFT self.turn_pwm = 40 self.state = self.REVERSE_STATE self.reverse_timer.reset() elif self.state == self.REVERSE_STATE: self.goStraight(1, 50) if self.reverse_timer.millis() > 800: self.state = self.TURN_STATE self.turn_timer.reset() elif self.state == self.OTHER_REVERSE_STATE: self.goStraight(1, 50) if self.reverse_timer.millis() > 500: self.state = self.TURN_STATE self.turn_timer.reset() elif self.state == self.TURN_STATE: self.turn(self.turn_direction, self.turn_pwm) if self.turn_timer.millis() > 600: self.state = self.STRAIGHT_STATE self.straight_timer.reset()
class PIDDrive(SyncedSketch): ss_pin = 10 #gyro select pin 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() ''' self.pipePath = "./vision" print "ok1" time.sleep(1) try: os.mkfifo(self.pipePath) except OSError: print "error" print "ok" self.rp = open(self.pipePath, 'r') ''' def loop(self): if self.timer.millis() > 100: self.timer.reset() #response = self.rp.read() #print "Got response %s" % response # Valid gyro status is [0,1], see datasheet on ST1:ST0 bits cAngle = self.gyro.val - self.totalDrift #corrected angle #print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift self.prevGyro = self.gyro.val self.totalDrift += self.drift pidResult = self.PID.valuePID(cAngle, self.initAngle) print 'Angle Dif: ' + str( cAngle - self.initAngle) + '\tPID RESULT: ' + str(pidResult) print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str( self.encoderL.val) print 'AVG: ' + str((self.encoderR.val + self.encoderL.val) / 2.) self.motorLdrive = self.fwdVel - pidResult self.motorRdrive = self.fwdVel + pidResult if self.motorLdrive >= 0: dirL = 0 else: dirL = 1 if self.motorRdrive >= 0: dirR = 0 else: dirR = 1 self.motorL.write(dirL, abs(self.motorLdrive)) self.motorR.write(dirR, abs(self.motorRdrive)) '''
class PIDDrive(SyncedSketch): ss_pin = 10 # gyro select pin 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 = -0.02875 self.totalDrift = 0 self.drifts = [] self.PID = PID(5, 4, 0.2) self.fwdVel = 0 self.timer = Timer() """ self.pipePath = "./vision" print "ok1" time.sleep(1) try: os.mkfifo(self.pipePath) except OSError: print "error" print "ok" self.rp = open(self.pipePath, 'r') """ def loop(self): if self.timer.millis() > 100: self.timer.reset() # response = self.rp.read() # print "Got response %s" % response # Valid gyro status is [0,1], see datasheet on ST1:ST0 bits cAngle = self.gyro.val - self.totalDrift # corrected angle # print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift self.prevGyro = self.gyro.val self.totalDrift += self.drift pidResult = self.PID.valuePID(cAngle, self.initAngle) print "Angle Dif: " + str(cAngle - self.initAngle) + "\tPID RESULT: " + str(pidResult) print "Encoders:\tR: " + str(self.encoderR.val) + "\tL: " + str(self.encoderL.val) print "AVG: " + str((self.encoderR.val + self.encoderL.val) / 2.0) self.motorLdrive = self.fwdVel - pidResult self.motorRdrive = self.fwdVel + pidResult if self.motorLdrive >= 0: dirL = 0 else: dirL = 1 if self.motorRdrive >= 0: dirR = 0 else: dirR = 1 self.motorL.write(dirL, abs(self.motorLdrive)) self.motorR.write(dirR, abs(self.motorRdrive)) """
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 PIDDrive(SyncedSketch): ss_pin = 10 #gyro select pin # image_pipe = './image' # if not os.path.exists(image_pipe): # os.mkfifo(image_pipe) # image_fd = os.open(image_pipe, os.O_RDONLY) 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 loop(self): #state 0 - wait for first no block, do nothing #transition: no block -> state 1 #state 1 - look for block #transition: found block -> state 2 #sate 2 - drive over block #transition: ir triggered -> state 3 #sate 3 - pick up block #transition: color sensor done -> sate 1 if self.timer.millis() > 100: # print 'Color' # print self.color.c # print self.color.r, self.color.g if self.color.c > 800 and self.Sort == 0: if self.color.r > self.color.g: self.Sort = 1 else: self.Sort = 2 # print self.uIR.val if self.uIR.val == 0: # self.MoveArm = True self.State = 0 if self.MoveArm: if self.Bottom == 2 and self.Top == 1: self.delta = 0 self.servoval = self.servo.bottom self.servo.write(self.servo.bottom) self.Bottom, self.Top = 0, 0 self.MoveArm = False elif self.servoval >= self.servo.top: time.sleep(1) self.delta = -self.servo.speed self.Top = 1 elif self.servoval <= self.servo.bottom: self.delta = self.servo.speed self.Bottom = 1 if self.Top == 1: self.Bottom = 2 if self.Sort == 1: if self.sorterval < self.sorter.left: self.dsorter = self.sorter.speed elif self.sorterval >= self.sorter.left: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 2: if self.sorterval > self.sorter.right: self.dsorter = -self.sorter.speed elif self.sorterval <= self.sorter.right: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 3: if time.time() - self.SortPause > 1: self.sorterval = self.sorter.center self.Sort = 0 self.sorterval += self.dsorter self.sorter.write(abs(self.sorterval)) self.servoval += self.delta self.servo.write(abs(self.servoval)) self.initAngle += self.turnVel self.timer.reset() #response = self.rp.read() #print "Got response %s" % response # Valid gyro status is [0,1], see datasheet on ST1:ST0 bits cAngle=self.gyro.val - self.totalDrift #corrected angle # print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift self.prevGyro=self.gyro.val self.totalDrift+=self.drift # message = os.read(self.image_fd, 20) # if ParseMessage(message): # self.blockDistance, self.blockAngle = ParseMessage(message) # print 'Angle Dif: ' + str(cAngle-self.initAngle) + '\tPID RESULT: '+ str(pidResult) # print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str(self.encoderL.val) # print 'AVG: ' + str((self.encoderR.val + self.encoderL.val)/2.) self.ir_array.update() ir = self.ir_array.ir_value ir90 = self.IRs[self.Follow][0] ir30 = self.IRs[self.Follow][1] avg = (ir[self.IRs[self.Follow][0]]+ir[self.IRs[self.Follow][1]]/2)/2 min_val = min(ir[self.IRs[self.Follow][0]], ir[self.IRs[self.Follow][1]]) if avg != float('inf'): pidResult= -self.IRPID.valuePID(4, avg) elif min_val != float('inf'): pidResult= -self.IRPID.valuePID(4, min_val) else: pidResult = -20 if ir[self.IRs[self.Follow][2]] < 4: pidResult = 20 # self.fwdVel = 0 else: self.fwdVel = 30 pidResult = self.PID.valuePID(self.initAngle, cAngle) self.motorLdrive = self.fwdVel - pidResult self.motorRdrive = self.fwdVel + pidResult print 'Distance Traveled : ' + str(self.getDistanceTraveled()) self.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive)) self.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive)) ''' print "ok" #response = self.rp.read() #print "Got response %s" % response ''' def ParseMessage(message): if message: if message[:2] == 'no': blockDistance, blockAngle = 0, 0 return None else: try: blockDistance, blockAngle = [number[:6] for number in message.split(',')] except: blockDistance, blockAngle = 0, 0 return blockDistance, blockAngle else: return None def getDistanceTraveled(self): """returns Distance traveled in inches. Call resetEncoders, drive forward until you've reached your destination""" print 'Left: ' + str(self.encoderL.val) print 'Right: ' + str(self.encoderR.val) avg = (self.encoderL.val + self.encoderR.val)/2 return avg/360.