def moveRobot(keyp,speed):
if keyp == 'w' or ord(keyp) == 16:
forward(speed)
print('Forward', speed)
elif keyp == 'z' or ord(keyp) == 17:
reverse(speed)
print('Reverse', speed)
elif keyp == 's' or ord(keyp) == 18:
spinRight(speed)
print('Spin Right', speed,imu.getBearing())
elif keyp == 'a' or ord(keyp) == 19:
spinLeft(speed)
print('Spin Left', speed)
elif keyp == '.' or keyp == '>':
speed = min(100, speed+10)
print('Speed+', speed)
elif keyp == ',' or keyp == '<':
speed = max (0, speed-10)
print('Speed-', speed)
elif keyp == ' ':
stop()
print('Stop')
elif keyp == "x":
setHeading(90,speed)
elif keyp == "b":
print(int(imu.getBearing()))
def setHeading(requiredHeading,speed):
while True:
bearing = int(imu.getBearing())
print(bearing)
if bearing != requiredHeading:
spinRight(50)
sleep(0.01)
stop()
#sleep(0.1)
else:
break
#!/bin/python
#
#terrierbot control
#
#v0.01
#
import os, sys, smbus, time, math, datetime, math, datetime, imu, piconzero as pz
from time import sleep
bus = smbus.SMBus(1)
speed = 60
try:
pz.init()
imu = imu.imuSystem(bus)
imu.getBearing()
except:
print "IMU sensor not found"
sys.exit()
try:
while True:
# keyp = pz.readkey()
# if ord(keyp) == 3:
# break
# else:
# pz.moveRobot(keyp,speed)
# print(imu.getBearing())
# sleep(0.03)
# pz.spinRight(speed)
# sleep(1)
def stop():
pz.stop()
print('Connected')
d = threading.Thread(name='daemon', target=blueConnection)
d.setDaemon(True)
screenMessage = displayMsg()
kb = keyBoardControl()
conn = robotMove()
try:
pz.init()
imu = imu.imuSystem(bus)
imu.getBearing()
except:
print "IMU sensor not found"
sys.exit()
try:
d.start()
screenMessage.start()
kb.start()
kb.readKey(screenMessage, conn)
while True:
# ("Still running")
# d = threading.Thread(name='daemon', target=daemon)
import bluetooth
import smbus,imu
bus=smbus.SMBus(1)
imu=imu.imuSystem(bus)
bd_addr = "00:14:03:06:1E:DF"
port = 1
sock = bluetooth.BluetoothSocket (bluetooth.RFCOMM)
sock.connect((bd_addr,port))
while 1:
tosend = str(imu.getBearing())
print(str(imu.getBearing()))
if tosend != 'q':
sock.send(tosend)
else:
break