class Robot(object):
def __init__(self, file):
super(Robot, self).__init__()
(self.actuators, self.sensors) = utils.file2bot(file, mbl_bots.BOTH)
self.bus = smbus.SMBus(1)
self.pwm = PCA9685(self.bus, 0x40, debug=False)
self.pwm.setPWMFreq(50)
#print("My name is: ", self.actuators[0].name)
self.names_acc = [
self.actuators[i].name for i in range(len(self.actuators))
]
self.names_sen = [
self.sensors[i].name for i in range(len(self.sensors))
]
self.idx_acc = [i for i in range(len(self.actuators))]
self.idx_sen = [i for i in range(len(self.sensors))]
self.acc_dic = utils.genDictionary(self.names_acc, self.idx_acc)
self.sen_dic = utils.genDictionary(self.names_sen, self.idx_sen)
self.state = mbl_bots.INIT
self.exploreState = mbl_bots.GETDATA
self.movesCode = mbl_bots.NONE
self.camera = Cam()
self.imu = IMU(self.bus)
self.vision = Vision()
def moveAcc(self, name, pos):
poss = pos * mbl_bots.SCALE_ACC + mbl_bots.CNT_ACC
if (poss > self.actuators[int(self.acc_dic[name])].max):
poss = self.actuators[int(self.acc_dic[name])].max
if (poss < self.actuators[int(self.acc_dic[name])].min):
poss = self.actuators[int(self.acc_dic[name])].min
self.pwm.setServoPulse(
int(self.actuators[int(self.acc_dic[name])].adress), poss)
#print('Moving ', name ,'to', poss )
def executeMove(self, file, speed):
moves = utils.file2move(file)
for i in range(len(moves)):
self.moveAcc(moves[i].actuator, moves[i].pos)
if (moves[i].delay > 0.0):
time.sleep(moves[i].delay * speed)
def executeMoveOBO(self, file, speed, count, correction):
moves = utils.file2move(file)
if (count * 2 <= len(moves)):
self.moveAcc(moves[count * 2].actuator, moves[count * 2].pos)
self.moveAcc(moves[count * 2 + 1].actuator,
moves[count * 2 + 1].pos)
#if(moves[i].delay > 0.0):
# time.sleep(moves[i].delay*speed)
def flat(self):
print("LoCoQuad is Flat")
self.executeMove("/home/pi/LoCoQuad/Code/LoCoQuad_flat.movefile.txt",
1)
def stand(self):
print("LoCoQuad is Standing")
self.executeMove("/home/pi/LoCoQuad/Code/LoCoQuad_stand.movefile.txt",
1)
def walkFront(self, speed=1):
print("LoCoQuad is Walking Forward")
self.executeMove(
"/home/pi/LoCoQuad/Code/LoCoQuad_walkFront.movefile.txt", speed)
def walkRight(self, speed=1):
print("LoCoQuad is Walking Right")
self.executeMove(
"/home/pi/LoCoQuad/Code/LoCoQuad_walkFront.movefile.txt", speed)
def walkLeft(self, speed=1):
print("LoCoQuad is Walking Left")
self.executeMove(
"/home/pi/LoCoQuad/Code/LoCoQuad_walkFront.movefile.txt", speed)
def walkBack(self, speed=1):
print("LoCoQuad is Walking Backwards")
self.executeMove(
"/home/pi/LoCoQuad/Code/LoCoQuad_walkBack.movefile.txt", speed)
def turnRight(self, speed=1):
print("LoCoQuad is Turning Right")
self.executeMove(
"/home/pi/LoCoQuad/Code/LoCoQuad_turnRight.movefile.txt", speed)
self.executeMove(
"/home/pi/LoCoQuad/Code/LoCoQuad_turnRight.movefile.txt", speed)
self.executeMove(
"/home/pi/LoCoQuad/Code/LoCoQuad_turnRight.movefile.txt", speed)
def turnLeft(self, speed=1):
print("LoCoQuad is Turning Left")
self.executeMove(
"/home/pi/LoCoQuad/Code/LoCoQuad_turnLeft.movefile.txt", speed)
def sayHello(self):
print("LoCoQuad is Saying Hi!")
self.executeMove(
"/home/pi/LoCoQuad/Code/LoCoQuad_sayHello.movefile.txt", 1)
def cameraPose(self):
print("LoCoQuad ready to take Picture")
self.executeMove(
"/home/pi/LoCoQuad/Code/LoCoQuad_cameraPose.movefile.txt", 1)
def swing(self):
print("LoCoQuad is Swinging")
self.executeMove("/home/pi/LoCoQuad/Code/LoCoQuad_swing.movefile.txt",
1)
def shake(self):
print("LoCoQuad is Shaking")
self.executeMove("/home/pi/LoCoQuad/Code/LoCoQuad_shake.movefile.txt",
1)
def balancePos(self, count, correction=0):
print("LoCoQuad is balancing")
self.executeMoveOBO(
"/home/pi/LoCoQuad/Code/LoCoQuad_balance.movefile.txt", 1, count,
correction)
def move(self, code):
moves = {
1: self.walkFront,
2: self.walkBack,
3: self.walkRight,
4: self.walkLeft,
5: self.turnRight,
6: self.turnLeft,
7: self.flat,
8: self.stand,
}
func = moves.get(code, lambda: None)
return func()
def detectCatch(self, imu):
data = imu.getImuRawData()
print(data)
if (data[3] > 3 or data[4] > 3 or data[5] > 3):
print("ROBOT CATCHED...do something!!")
return True
else:
return False
def isBalanced(self):
data = self.imu.getImuRawData()
if (data[0] < 0.2 or data[1] < 0.2):
return True
else:
return False
class SPR4(Robot):
def __init__(self):
super(SPR4, self).__init__("/home/pi/SPR4/SPR4_code/SPR4.botfile.txt")
#Brain method --- conscience!!!!
if (len(sys.argv) == 2):
print("EXECUTING TEST OF MOVEMENT", str(sys.argv[1]))
while True:
super(SPR4, self).executeMove(str(sys.argv[1]), 1)
else:
while True:
self.generalFSM(self.state)
#=============================================================================
# GENERAL FSM
#=============================================================================
def generalFSM(self, state):
states_list = {
0: self.INIT,
1: self.REST,
2: self.EXPLORE,
3: self.SHOWOFF,
4: self.PHOTO,
}
func = states_list.get(state, lambda: None)
return func()
#=============================================================================
# INIT STATE
#=============================================================================
def INIT(self):
print("CURRENT STATE: INIT")
# use Raspberry Pi board pin numbers
GPIO.setmode(GPIO.BCM)
GPIO.setup(mbl_bots.TRIG, GPIO.OUT)
GPIO.setup(mbl_bots.ECHO, GPIO.IN)
self.lastIMU = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.currentIMU = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.camera = Cam()
self.imu = IMU(self.bus)
self.distance = -1
signal.signal(signal.SIGINT, self.close)
self.state = mbl_bots.REST
time.sleep(1)
#=============================================================================
# REST STATE
#=============================================================================
def REST(self):
print("CURRENT STATE: REST")
start_time = time.time()
while ((time.time() - start_time) < 20):
if (super(SPR4, self).detectCatch(self.imu)):
for i in range(5):
super(SPR4, self).shake()
break
else:
time.sleep(0.5)
time.sleep(1)
self.state = mbl_bots.EXPLORE
#=============================================================================
# EXPLORE STATE & FSM
#=============================================================================
def exploreFSM(self, state):
substates_explorelist = {
0: self.exploreGetData,
1: self.exploreProcessData,
2: self.exploreMove,
3: self.exploreReconTurn,
}
func = substates_explorelist.get(state, lambda: None)
return func()
def EXPLORE(self):
print("CURRENT STATE: EXPLORE")
super(SPR4, self).stand()
#EXPLORING FiniteStateMachine
while (self.state == mbl_bots.EXPLORE):
self.exploreFSM(self.exploreState)
def exploreGetData(self):
print("CURRENT STATE: EXPLORE")
print("CURRENT SUBSTATE: DATA ACQUISITION")
self.distance = utils.getDistance()
self.lastIMU = self.currentIMU
self.currentIMU = self.imu.getImuRawData()
self.exploreState = mbl_bots.PROCESSDATA
def exploreProcessData(self):
print("CURRENT STATE: EXPLORE")
print("CURRENT SUBSTATE: DATA PROCESSING")
if (self.distance < 5):
self.movesCode = mbl_bots.WB
elif (self.distance > 15):
self.movesCode = mbl_bots.WF
else:
if (randint(0, 1) == 1):
self.movesCode = mbl_bots.TR
else:
self.movesCode = mbl_bots.TL
self.exploreState = mbl_bots.MOVE
# if(randint(0,5)>1):
# self.exploreState = mbl_bots.MOVE
# else:
# self.exploreState = mbl_bots.GETDATA
# self.state = mbl_bots.SHOWOFF
def exploreMove(self):
print("CURRENT STATE: EXPLORE")
print("CURRENT SUBSTATE: MOVING")
super(SPR4, self).move(self.movesCode)
self.exploreState = mbl_bots.GETDATA
def exploreReconTurn(self):
print("Not implemented...")
#=============================================================================
# SHOW OFF STATE
#=============================================================================
def SHOWOFF(self):
print("CURRENT STATE: SHOWOFF")
#SHOWOFF METHODS
super(SPR4, self).swing()
super(SPR4, self).sayHello()
self.state = mbl_bots.PHOTO
#=============================================================================
# PHOTO STATE
#=============================================================================
def PHOTO(self):
print("CURRENT STATE: PHOTO")
super(SPR4, self).cameraPose()
self.camera.takePic()
super(SPR4, self).stand()
self.state = mbl_bots.EXPLORE
def close(self, signal, frame):
print("\nTurning off SPR4 Activity...\n")
GPIO.cleanup()
sys.exit(0)
