def __init__(self): self.operation = [] # 受信作成 self.sub = rospy.Subscriber('joy', Joy, self.joyCallback, queue_size=1) # 送信作成 self.pub_arm = rospy.Publisher('arm', arm, queue_size=100) self.pub_foot = rospy.Publisher('foot',foot, queue_size=100) # messageのインスタンスを作る self.msg_arm = arm() self.msg_foot = foot() # index self.frame_id = 0 # rotaryencoder self.rotary = RotaryEncoder() self.rotary.setPin(PIN_ROTARY_A,PIN_ROTARY_B) # updownの衝突判定用 #self.updown = IN_INITIAL self.updown = IN_OPERATE # for debug self.pi = pigpio.pi() self.pi.set_mode(PIN_MICRO_SW,pigpio.INPUT) # self.mode = Mode.HERVEST self.strike = False self.grub = False
from rotaryEncoder import RotaryEncoder def callback(direction): print("Hello Rotary Encoder - " + direction) rotaryEncoder = RotaryEncoder(62, "pi10", 64, "pb13", 61, "pi13", 66, "pb10", callback) rotaryEncoder.enable()
class Brain(object): def __init__(self): self.operation = [] # 受信作成 self.sub = rospy.Subscriber('joy', Joy, self.joyCallback, queue_size=1) # 送信作成 self.pub_arm = rospy.Publisher('arm', arm, queue_size=100) self.pub_foot = rospy.Publisher('foot',foot, queue_size=100) # messageのインスタンスを作る self.msg_arm = arm() self.msg_foot = foot() # index self.frame_id = 0 # rotaryencoder self.rotary = RotaryEncoder() self.rotary.setPin(PIN_ROTARY_A,PIN_ROTARY_B) # updownの衝突判定用 #self.updown = IN_INITIAL self.updown = IN_OPERATE # for debug self.pi = pigpio.pi() self.pi.set_mode(PIN_MICRO_SW,pigpio.INPUT) # self.mode = Mode.HERVEST self.strike = False self.grub = False def clearMsg(self): #arm self.msg_arm.strike = False self.msg_arm.home = False self.msg_arm.release= False self.msg_arm.grub = False self.msg_arm.store = False self.msg_arm.tilt = Arm.NONE self.msg_arm.updown = Arm.NONE self.msg_arm.mode = Mode.HERVEST # foot self.msg_foot.direction_l = Direction.AHEAD self.msg_foot.direction_r = Direction.AHEAD self.msg_foot.speed_l = 0 self.msg_foot.speed_r = 0 def convertUpDown(self): # initialize if self.updown == IN_INITIAL: micro_sw = self.pi.read(PIN_MICRO_SW) if micro_sw == 1 : self.updown = 0 self.rotary.setRotate(0) else: self.msg_arm.updown = Arm.PLUS # oparating if self.updown == IN_OPERATE: rotate = self.rotary.getRotate() rotate = 1 # for debug if self.operation[INDEX_UP] == 1 and rotate > MIN_ROTATE: self.msg_arm.updown = Arm.PLUS elif self.operation[INDEX_DOWN] != 1 and rotate < MAX_ROTATE: self.msg_arm.updown = Arm.MINUS #print "rotate " + str(self.rotary.getRotate()) def convertFoot(self): # foot ## direction if self.operation[INDEX_SPEED_L] >= 0: self.msg_foot.direction_l = Direction.AHEAD else: self.msg_foot.direction_l = Direction.BACK if self.operation[INDEX_SPEED_R] >= 0: self.msg_foot.direction_r = Direction.AHEAD else: self.msg_foot.direction_r = Direction.BACK ## speed speed = self.operation[INDEX_SPEED_L] self.msg_foot.speed_l = (speed*SPEED_STEP*100)/SPEED_STEP speed = self.operation[INDEX_SPEED_R] self.msg_foot.speed_r = (speed*SPEED_STEP*100)/SPEED_STEP def printMsg(self): print "UpDown=" + str(self.msg_arm.updown) print "Dir_L =" + str(self.msg_foot.direction_l) print "Dir_R =" + str(self.msg_foot.direction_r) print "Spd_L =" + str(self.msg_foot.speed_l) print "Spd_R =" + str(self.msg_foot.speed_r) def convert(self): #print(self.operation) # arm if self.operation[INDEX_STRIKE_ON] == 1: self.strike = True if self.operation[INDEX_STRIKE_OFF] == 1: self.strike = False self.msg_arm.strike = self.strike self.msg_arm.home = bool(self.operation[INDEX_HOME]) self.msg_arm.release= bool(self.operation[INDEX_RELEASE]) ## grub & store if self.operation[INDEX_GRUB_ON] == 1: self.grub = True if self.operation[INDEX_GRUB_OFF] == 1: self.grub = False self.msg_arm.grub = self.grub self.msg_arm.store = bool(self.operation[INDEX_STORE]) ## tilt if self.operation[INDEX_TILT] == 1: self.msg_arm.tilt = Arm.PLUS if self.operation[INDEX_TILT] == -1: self.msg_arm.tilt= Arm.MINUS ## updown self.convertUpDown() # foot self.convertFoot() # mode if self.operation[INDEX_MODE] == 1: if self.mode == Mode.HERVEST: self.mode = Mode.BULB else: self.mode = Mode.HERVEST self.msg_arm.mode = self.mode def transmit(self): # clear self.clearMsg() self.msg_arm.frame_id = self.frame_id; self.msg_foot.frame_id = self.frame_id; if len(self.operation) > INDEX_MAX: self.convert() # publishする関数 self.pub_arm.publish(self.msg_arm) self.pub_foot.publish(self.msg_foot) self.frame_id+=1 print "frame_id" + str(self.msg_foot.frame_id) print "rotate " + str(self.rotary.getRotate()) def joyCallback(self, joy_msg): j = 0 # 2回以降は代入 if len(self.operation) > INDEX_MAX: for i,item in enumerate(joy_msg.axes): self.operation[i] = item j=i for i,item in enumerate(joy_msg.buttons): self.operation[j+i] = item # 初回は追加 else: for i,item in enumerate(joy_msg.axes): self.operation.append(item) j=i for i,item in enumerate(joy_msg.buttons): self.operation.append(item) # for debug if(DEBUG): for i ,item in enumerate(self.operation): print str(i) + "=" + str(self.operation[i])
else: radio.increaseVolume() def powerOffButtonCallback(): radio.stop() display.disable() os.system("shutdown -h now") config = configparser.ConfigParser() config.read("tube.cfg") display = LCD(config["display"]["address"]) display.enable() radio = Radio(config["radio"]["stations"].splitlines()) trackDataThread = _thread.start_new_thread(updateTrackData, ()) powerOffButton = Button(int(config["button"]["pinNumber"]),config["button"]["pinName"],int(config["button"]["pullupNumber"]),config["button"]["pullupName"], powerOffButtonCallback) powerOffButton.enable() stationRotary = RotaryEncoder(int(config["rightRotary"]["aPinNumber"]),config["rightRotary"]["aPinName"],int(config["rightRotary"]["aPullupNumber"]),config["rightRotary"]["aPullupName"],int(config["rightRotary"]["bPinNumber"]),config["rightRotary"]["bPinName"],int(config["rightRotary"]["bPullupNumber"]),config["rightRotary"]["bPullupName"],stationCallback) stationRotary.enable() volumeRotary = RotaryEncoder(int(config["leftRotary"]["aPinNumber"]),config["leftRotary"]["aPinName"],int(config["leftRotary"]["aPullupNumber"]),config["leftRotary"]["aPullupName"],int(config["leftRotary"]["bPinNumber"]),config["leftRotary"]["bPinName"],int(config["leftRotary"]["bPullupNumber"]),config["leftRotary"]["bPullupName"],volumeCallback) volumeRotary.enable() radio.play() while True: sleep(1000000)