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