# devastator3.py # # robot control for devastator # # created 2019 # by hennisoft <http://www.hennisoft.ie> # modified 07 Nov 2019 by # Henning Buchholz # # This example code is in the public domain. # Based on: # https://github.com/miguelgrinberg/Flask-SocketIO import subprocess import time import smbus import RPi.GPIO as GPIO from threading import Thread from flask import Flask, render_template, session, request from flask_socketio import SocketIO, emit import UltraBorg import ThunderBorg bus = smbus.SMBus(1) device = 0x60 CAMERA_LED = 40 LED = 7 IR_LED = 11 # Power settings voltageIn = 13.6 # Total battery voltage to the ThunderBorg voltageOut = 6.0 # 6v Motors maxPower = voltageOut / float(voltageIn)# 0.5 for 6v Motor and 12V input percentMotor = maxPower / 100 # percent factor motorVolts = voltageOut / maxPower # motor reading to Volts centerx = 0.0 # servo1 startposition centery = 0.3 # servo2 startposition ip = str(subprocess.check_output(["hostname", "-I"]).split()[0]) # get IP address GPIO.setwarnings(False) GPIO.setmode(GPIO.BCM) GPIO.setup(CAMERA_LED,GPIO.OUT) GPIO.setup(LED,GPIO.OUT) GPIO.setup(IR_LED,GPIO.OUT) GPIO.output(CAMERA_LED,True) # active low GPIO.output(LED,False) GPIO.output(IR_LED,False) TB = ThunderBorg.ThunderBorg() TB.Init() UB = UltraBorg.UltraBorg() # Create a new UltraBorg object UB.Init() # Set the board up (checks the board is connected) time.sleep(0.5) #okay = UB.SetWithRetry(UB.SetServoStartup2, UB.GetServoStartup2, 3290, 5) UB.SetServoPosition1(centerx) UB.SetServoPosition2(centery) app = Flask(__name__) app.debug = True app.config['SECRET_KEY'] = 'secret!' socketio = SocketIO(app) thread = None def background_stuff(): """ Let's do it a bit cleaner """ while True: time.sleep(0.3) b1 = bus.read_byte_data(device, 0x02) b2 = bus.read_byte_data(device, 0x03) pit = bus.read_byte_data(device, 0x04) rol = bus.read_byte_data(device, 0x05) pit = pit-256 if pit > 127 else pit rol = rol-256 if rol > 127 else rol dir = float(int((b1 << 8) + b2))/10 # not sure which one is more exact (in theory: active one, but commented one is faster) #usm1 = float(int(UB.GetRawDistance1()))/10 #usm2 = float(int(UB.GetRawDistance2()))/10 #usm3 = float(int(UB.GetRawDistance3()))/10 #usm4 = float(int(UB.GetRawDistance4()))/10 usm1 = float(int(UB.GetDistance1()))/10 usm2 = float(int(UB.GetDistance2()))/10 usm3 = float(int(UB.GetDistance3()))/10 usm4 = float(int(UB.GetDistance4()))/10 battCurrent = round(TB.GetBatteryReading(),2) leftmotor = round(TB.GetMotor1() * motorVolts,2) rightmotor = round(TB.GetMotor2() * motorVolts,2) socketio.emit('message', {'direction': dir, 'pitch': pit, 'roll': rol, 'distfront': usm1, 'distleft': usm2, 'distright': usm3, 'distrear': usm4, 'batt': battCurrent, 'lmotor': leftmotor, 'rmotor': rightmotor}, namespace='/test') @app.route('/') def index(): global thread if thread is None: thread = Thread(target=background_stuff) thread.start() return render_template('devastator2.html') @socketio.on('my event', namespace='/test') def my_event(msg): print 'Message: ', msg['data'] @socketio.on('motor', namespace='/test') def my_motor(msg): print 'motor-left: ', msg['left'] print 'motor-right: ', msg['right'] TB.SetMotor1(int(msg['left']) * percentMotor) TB.SetMotor2(int(msg['right']) * percentMotor) @socketio.on('servo', namespace='/test') def my_servo(msg): print 'servo-lr: ', msg['servoh'] print 'servo-ud: ', msg['servov'] UB.SetServoPosition1(float(int(msg['servoh']) - 50) / -50.0) UB.SetServoPosition2(float((int(msg['servov']) - 50) / -50.0)+centery) @socketio.on('centermode', namespace='/test') def my_center(msg): if (msg['on']=='True'): UB.SetServoPosition1(centerx) UB.SetServoPosition2(centery) @socketio.on('irmode', namespace='/test') def my_ir(msg): if (msg['on']=='True'): GPIO.output(CAMERA_LED,False) GPIO.output(IR_LED,True) else: GPIO.output(CAMERA_LED,True) GPIO.output(IR_LED,False) @socketio.on('ledmode', namespace='/test') def my_led(msg): if (msg['on']=='True'): GPIO.output(LED,True) else: GPIO.output(LED,False) @socketio.on('guidedmode', namespace='/test') def my_guided(msg): if (msg['on']=='True'): TB.MotorsOff() else: TB.MotorsOff() @socketio.on('disconnect', namespace='/test') def test_disconnect(): print('Client disconnected - Motors: off') TB.MotorsOff() if __name__ == '__main__': try: print '\n\nReady ! ...\n\n' print "please use: http://{}:8000".format(ip) print('in your browser to navigate Devastator !\n\n') print('to quit: close browser session and <CTRL>-c>\n\n') socketio.run(app, host='0.0.0.0', port=8000) except KeyboardInterrupt: print '\n... stopped by Ctrl+C' finally: #clean up print '... shutting down all services' socketio.stop() TB.MotorsOff() UB.SetServoPosition1(0.0) UB.SetServoPosition2(0.0) GPIO.cleanup() print '... dome ! See you next time ...'