async def control(cmds):
'''
Main control method for Raspberry Pi robot. Function
accepts commands via local keybaord input or remote
client as written to cmds list parameter. Local
commands take precedence. Function executes commands
via its robotMotor object. Most recent command is
always executed and previously accumulated commands
are erased.
'''
motor = robotMotor()
kb = KBHit()
local = True
localDict = {True: 'local', False: 'remote'}
# main contorl loop
while True:
await asyncio.sleep(0)
cmd = ''
# check for local commands
if kb.kbhit():
cmd = kb.getch()
local = True
# check for remote commands
elif cmds:
cmd = cmds[-1]
cmds.clear()
local = False
# check for exit command
if cmd == 'e':
break
# check for contorl commands
elif cmd is '5':
await motor.fwd()
print(localDict[local], '- foward')
elif cmd is '2':
await motor.rev()
print(localDict[local], '- reverse')
elif cmd is '1':
await motor.softLeft()
print(localDict[local], '- soft left')
elif cmd is '4':
await motor.hardLeft()
print(localDict[local], '- hard left')
elif cmd is '3':
await motor.softRight()
print(localDict[local], '- soft right')
elif cmd is '6':
await motor.hardRight()
print(localDict[local], '- hard right')
elif cmd is '+':
motor.servoDown()
elif cmd is '-':
motor.servoUp()
def __init__(self):
"""Construct a new CPU."""
self.ram = [0] * 256
self.register = [0] * 8
self.register[7] = 0xF4
self.pc = 0
self.fl = 0
self.setupBranchtable()
self.lastFire = time.time()
self.interruptsEnabled = True
self.keyboardMonitor = KBHit()
def run(self):
self.out("start kbSrv")
storageCtrl.addThreadToStop(self)
my_kb = KBHit()
while storageCtrl.getStopRequested() == False:
c = ""
try:
if my_kb.get_kbhit():
c = my_kb.getch()
if ord(c) == 27: # ESC
break
self.out(str(ord(c)) + "=" + c)
allKWandCommands = storageCtrl.getkeywordsAndCommands()
for [kw, kb, command, gpio] in allKWandCommands:
if c == kb:
if command != "":
storageCtrl.pushWebRequest(command)
elif gpio != "":
storageCtrl.pushGpioRequest(gpio)
except:
self.out("Error on keypressed")
# print(".2")
sleep(self.refreshRate)
my_kb.set_normal_term()
storageCtrl.stopAcheived = storageCtrl.getStopAcheived() - 1
def game_mode():
global socket_game
mes = protocol_read_message(socket_game)
end_time = time.time() + 10
print_game_mode(mes)
kb = KBHit()
while time.time() < end_time:
time.sleep(0.5)
if kb.kbhit():
kb.set_normal_term()
kb = KBHit()
send_press()
async def cmdClient():
'''
Opens connection with command server using asyncio's
stream APIs. Connects to Raspberry Pi at 192.168.0.27
on port 8888. Reads keyboard input and transmits to
server.
'''
reader, writer = await asyncio.open_connection('192.168.0.27', 8888)
kb = KBHit()
while True:
if kb.kbhit():
message = kb.getch()
print(f'Send: {message!r}')
writer.write(message.encode())
data = await reader.read(100)
if not data:
break
await asyncio.sleep(0)
writer.close()
print('Command connection closed')
class CPU:
"""Main CPU class."""
def __init__(self):
"""Construct a new CPU."""
self.ram = [0] * 256
self.register = [0] * 8
self.register[7] = 0xF4
self.pc = 0
self.fl = 0
self.setupBranchtable()
self.lastFire = time.time()
self.interruptsEnabled = True
self.keyboardMonitor = KBHit()
def setupBranchtable(self):
self.branchtable = {}
self.branchtable[LDI] = self.handleLDI
self.branchtable[PRN] = self.handlePRN
self.branchtable[HLT] = self.handleHLT
self.branchtable[MUL] = self.handleMUL
self.branchtable[PUSH] = self.handlePUSH
self.branchtable[POP] = self.handlePOP
self.branchtable[CALL] = self.handleCALL
self.branchtable[RET] = self.handleRET
self.branchtable[ADD] = self.handleADD
self.branchtable[ST] = self.handleST
self.branchtable[JMP] = self.handleJMP
self.branchtable[PRA] = self.handlePRA
self.branchtable[IRET] = self.handleIRET
self.branchtable[LD] = self.handleLD
def load(self, program):
"""Load a program into memory."""
address = 0
for instruction in program:
self.ram[address] = instruction
address += 1
def getStackIndex(self):
return self.register[7]
def setStackIndex(self, index):
self.register[7] = index
def ramRead(self, mar):
return self.ram[mar]
def ramWrite(self, mdr, mar):
"""
mar is the address
mdr is the data to store at that address
"""
self.ram[mar] = mdr
def alu(self, op, operandA, operandB):
"""ALU operations."""
if op == MUL:
self.register[operandA] *= self.register[operandB]
self.register[operandA] &= 0xFF
elif op == ADD:
self.register[operandA] += self.register[operandB]
self.register[operandA] &= 0xFF
else:
raise Exception("Unsupported ALU operation")
def trace(self):
"""
Handy function to print out the CPU state. You might want to call this
from run() if you need help debugging.
"""
print(
f"TRACE (pc): %02X | (values at pc, pc+1, pc+2) %02X %02X %02X | REGISTERS: "
% (
self.pc,
#self.fl,
#self.ie,
self.ramRead(self.pc),
self.ramRead(self.pc + 1),
self.ramRead(self.pc + 2)),
end='')
for i in range(8):
print(" %02X" % self.register[i], end='')
print()
def handleLDI(self):
operandIndex = self.ramRead(self.pc + 1)
operand = self.ramRead(self.pc + 2)
self.register[operandIndex] = operand
def handlePRN(self):
operandIndex = self.ramRead(self.pc + 1)
operand = self.register[operandIndex]
print(operand)
def handleHLT(self):
sys.exit(0)
def handleMUL(self):
operandAIndex = self.ram[self.pc + 1]
operandBIndex = self.ram[self.pc + 2]
self.alu(MUL, operandAIndex, operandBIndex)
def handleADD(self):
operandAIndex = self.ram[self.pc + 1]
operandBIndex = self.ram[self.pc + 2]
self.alu(ADD, operandAIndex, operandBIndex)
def handlePUSH(self):
operandIndex = self.ramRead(self.pc + 1)
operand = self.register[operandIndex]
self.pushValueOnStack(operand)
def pushValueOnStack(self, operand):
stackPointer = self.getStackIndex() - 1
self.setStackIndex(stackPointer)
self.ramWrite(operand, stackPointer)
def handlePOP(self):
operand = self.popValueFromStack()
operandIndex = self.ramRead(self.pc + 1)
self.register[operandIndex] = operand
def popValueFromStack(self):
stackPointer = self.getStackIndex()
self.setStackIndex(stackPointer + 1)
return self.ramRead(stackPointer)
def handleCALL(self):
operandIndex = self.ramRead(self.pc + 1)
operand = self.register[operandIndex]
# save address of next *INSTRUCTION* in stack
self.pushValueOnStack(self.pc + 2)
self.pc = operand
def handleRET(self):
operand = self.popValueFromStack()
self.pc = operand
def handleST(self):
operandA = self.ramRead(self.pc + 1)
operandB = self.ramRead(self.pc + 2)
self.ramWrite(self.register[operandB], self.register[operandA])
def handleJMP(self):
operand = self.ramRead(self.pc + 1)
self.pc = self.register[operand]
def handlePRA(self):
operand = self.ramRead(self.pc + 1)
value = self.register[operand]
print(chr(value))
def handleIRET(self):
self.register[6] = self.popValueFromStack()
self.register[5] = self.popValueFromStack()
self.register[4] = self.popValueFromStack()
self.register[3] = self.popValueFromStack()
self.register[2] = self.popValueFromStack()
self.register[1] = self.popValueFromStack()
self.register[0] = self.popValueFromStack()
self.fl = self.popValueFromStack()
self.pc = self.popValueFromStack()
self.interruptsEnabled = True
def handleLD(self):
operandA = self.ramRead(self.pc + 1)
operandB = self.ramRead(self.pc + 2)
self.register[operandA] = self.ramRead(self.register[operandB])
def __interuptTimer(self):
currentTime = time.time()
if currentTime > self.lastFire + 1:
self.lastFire = currentTime
self.register[IS] |= 0b1
def __checkKeyboardInterrupts(self):
if self.keyboardMonitor.kbhit():
keyPress = self.keyboardMonitor.getch()
value = ord(keyPress)
self.ramWrite(value, KEY_PRESSED)
self.register[IS] |= 0b10
def run(self):
"""Run the CPU."""
while True:
# poll interrupts
self.__interuptTimer()
self.__checkKeyboardInterrupts()
# check for interrupts
if self.interruptsEnabled:
maskedInterrupts = self.register[IM] & self.register[IS]
for i in range(8):
interrupt = ((maskedInterrupts >> i) & 1) == 1
if interrupt:
self.interruptsEnabled = False
# clear only the bit of the current interrupt
self.register[IS] &= 0xFF ^ (1 << i)
self.pushValueOnStack(self.pc)
self.pushValueOnStack(self.fl)
self.pushValueOnStack(self.register[0])
self.pushValueOnStack(self.register[1])
self.pushValueOnStack(self.register[2])
self.pushValueOnStack(self.register[3])
self.pushValueOnStack(self.register[4])
self.pushValueOnStack(self.register[5])
self.pushValueOnStack(self.register[6])
newAddress = self.ramRead(0xF8 + i)
self.pc = newAddress
break
instructionRegister = self.ram[self.pc]
instructionMethod = self.branchtable.get(instructionRegister, None)
# self.trace()
if instructionMethod is not None:
instructionMethod()
else:
print(f"Instruction not recognized: {instructionRegister}")
exit(1)
# check to see if the PC is explicitly set by the instruction. if not, increment the PC by the number of arguments + 1
if ((instructionRegister >> 4) & 0b1) != 1:
self.pc += ((instructionRegister & 0xC0) >> 6) + 1
return (None, None)
# fall-through from axis movement commands
return ('move', [axis, delta])
# def measure(scenario, m):
# m.move(scenario.get_first_pos())
# for pos in scenario.positions[1:]:
# pos.timing1 = m.measure1(pos.pos)
# m.move(scenario.get_first_pos())
# for pos in scenario.positions[1:]:
# pos.timing2 = m.measure2(pos.pos)
# m.move(scenario.get_first_pos())
kb = KBHit()
m = Manipulator(homing=False, dry_run=False)
scenarios = [
Scenario('scenario-1.csv'),
Scenario('scenario-2.csv'),
Scenario('scenario-3.csv')
]
curr_scenario = 0
scenario = scenarios[curr_scenario]
pos = scenario.get_first_pos()
m.move(pos.pos)
while True:
print("# %s line %d %s" % (scenario.file, pos.nline, pos.pos))
wave_ampl = np.zeros(NJ * 2)
wave_phase = np.zeros(NJ * 2)
wave_b = np.zeros(NJ * 2)
if not gui:
target_phi = DESIRED_CAM_PHI
target_theta = DESIRED_CAM_THETA
if auto_retarget:
err = None
wave_axis = None
from KBHit import KBHit
kb = KBHit()
ASTEPS = 30
BSTEPS = 30
FSTEPS = 30
while not gui or (p.readUserDebugParameter(s_quit) == 0):
phis = wave_ampl * np.sin(t * 24 + wave_phase) + wave_b
# apply phis
r.step(phis)
t += timeStep
# update current views in gui, gui2, and vout
if gui is not None or gui2 is not None or vout is not None:
img = r.getCameraImage()