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
# ２回以降は代入
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)
