JoystickReceive.py and JoystickSend.py

#On the server side (ROV):------------------------------------------------------------------------------------------
#!/usr/bin/env python
# version joystickreceive10.py
# trial to incorporate heartbeat with bash script that pings
# included sql
from __future__ import division
from subprocess import PIPE, Popen
import socket
import time
import datetime
import os
import subprocess
import mysql.connector

import psutil
#!/usr/bin/env python
# install with $sudo apt-get install python-dev
# install with $sudo pip install psutil

def get_cpu_temperature():
    process = Popen(['vcgencmd', 'measure_temp'], stdout=PIPE)
    output, _error = process.communicate()
    return float(output[output.index('=') + 1:output.rindex("'")])

cnx = mysql.connector.connect(user='ROV', password='passROV' , database='sensors')
cursor = cnx.cursor()
##add_data = ("INSERT INTO sensordata "
##                "(Time,Heading,Roll,Pitch,Tin,Tout,Pin,Pout,Vlipo1)"
##                "VALUES (%s, %s, %s,%s, %s, %s,%s, %s, %s)")

add_data = ("INSERT INTO diagnostics "
                "(Mleft,Mright,Mvert,Tilt,Free,Light,Time,RamPct,DiskFree,Tcpu,CPUpct)"
                "VALUES (%s, %s, %s,%s, %s, %s, %s, %s, %s, %s, %s)")


# https://gist.github.com/SkyRocknRoll/8718086 get socket IP of client
ServoChnl = [4,17,18,27,22,23,24,25]
DELAY = 0.05
sett=0      #set date time first loop than set=1
SQLwrite=0  #every 4 steps write sql data
LightChange=0
RecMode='RecStart'
cmd="gpio mode 10 out"      ##lights out
os.system(cmd)
cmd="gpio write 10 0"
os.system(cmd)

def pwm(pin, axis):
    cmd = "echo \"" + str(ServoChnl[pin]) + "=" + str(axis) + "\" > /dev/pi-blaster"
##    print cmd
    os.system(cmd)
##    time.sleep(DELAY)

port = 5000
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.bind(("", port))
ok=1
FirstMessage=1	#this is a trigger to set the system time to the first timestamp from socket
print "waiting on port:", port
try:                            # try is used to trap errors
    while ok==1:

##        ips = s.accept()
##        print ips + " connected"
        data, addr = s.recvfrom(1024)
        # ['0.15', '0.15', '0.15', '0.158475', '', '0', '2015-12-08 21:12:01.994000']
        if data=="Bye":
            print "Client pressed 8, server terminated."
            quit()
        Controls=data.split(",")
#       print Controls



        if FirstMessage==1:				#step to synchronize time on RPI with laptop


            cmd = "sudo date -s \"" + Controls[6] + "\""
            os.system(cmd)
            FirstMessage=0
            print "Connected to client"
##            cmd ="sudo /data/testping.sh &"
##            os.system(cmd)
##            p=subprocess.Popen(cmd)
##            serverip=s.getsockname()
##            print serverip

        now= datetime.datetime.now() #for heartbeat
        if sett==0:
#           2015-10-21 00:10:08.693000
            now3=Controls[6]
            if len(now3)<26:    #  '2015-10-23 21:50:39' gives error (int, no float)
                now3=now3+".000000"
##            print now3
            TimeStamp1=datetime.datetime.strptime(now3, "%Y-%m-%d %H:%M:%S.%f")


        delay=now-TimeStamp1
        lag=delay.total_seconds() # 2.7+
##        print lag
        if lag>0.5:
            #do nothing and skip command to read more recent
            drop=1
            print "Line dropped"
        else:
##            print Controls
            for i in range(0,4):
                Controls[i]=float(Controls[i])
                pwm(i,Controls[i])
            cmd=""
            if Controls[5]<>LightChange:
                if Controls[5]=='1':
                    cmd = "gpio write 10 1"
                else:
                    cmd = "gpio write 10 0"
                os.system(cmd)      ##time consuming command
                LightChange=Controls[5]
            SQLwrite +=1
            if SQLwrite ==5:
                cpu_temperature = get_cpu_temperature()
                ram = psutil.virtual_memory()
                cpu_pct=psutil.cpu_percent()
                ram_percent_used = ram.percent
                disk = psutil.disk_usage('/')
                disk_free = disk.free / 2**30
                disk_percent_used = disk.percent

                sensorstring=Controls
                sensorstring=(sensorstring)
                sensorstring.append(ram_percent_used)
                sensorstring.append(disk_free)
                sensorstring.append(cpu_temperature)
                sensorstring.append(cpu_pct)
                cursor.execute(add_data, sensorstring)
                cnx.commit()
                SQLwrite=0
            time.sleep(DELAY)





##        if Controls[[4]<>RecMode]:
##            if Controls[4]=='RecStop':
##                cmd = "raspivid -t 9999999 -w 1900 -h 1080 -fps 10 -b 500000 -rot 270 -hf -o - | tee /data/stream.h264 | ffmpeg -i - -vcodec copy -an -r 25 -f flv -metadata streamName=myStream tcp://0.0.0.0:6666 &"
##            else:
##                cmd = "raspivid -t 9999999 -w 1900 -h 1080 -fps 10 -b 500000 -vf -hf -o - | ffmpeg -i - -vcodec copy -an -r 25 -f flv -metadata streamName=myStream tcp://0.0.0.0:6666 &"
##            os.system(cmd)      ##time consuming command
##            RecMode=Controls[4]



except:  #if try failed (in case of error) set servo's to 0 and lights out
    for i in range(0,4):
        pwm(i,0.15)
    cmd = "gpio write 10 0"
    os.system(cmd)
    print "No connection or error encountered."
    cmd= "echo \"release 4\" > /dev/pi-blaster"
    os.system(cmd)
    cmd= "echo \"release 17\" > /dev/pi-blaster"
    os.system(cmd)
    cmd= "echo \"release 18\" > /dev/pi-blaster"
    os.system(cmd)


## 1.25-1.75
##  GPIO number   Pin in P1 header
##      4              P1-7
##      17             P1-11
##      18             P1-12
##      21             P1-13
##      22             P1-15
##      23             P1-16
##      24             P1-18
##      25             P1-22
cursor.close()
cnx.close()
s.close()

#on the client side (laptop):
#-------------------------------------------------------------------------------------------------------------
# # version joystickSend10.py
# Purpose:
#
# Author:      Niels
#
# Created:     01-02-2015
# Copyright:   (c) Niels 2015
# Licence:     <your licence>
# TODO: heartbeat for fail safe. Feedback via website (lights on/off etc)
# pull umbilical and stop motor
#-------------------------------------------------------------------------------

import socket
import pygame
import time
import datetime
# Setup pygame and key states
global DriveCommands
global Command
##global fps
global moveLeft
global moveRighte
global moveQuit
hadEvent = True

fps             = 5                   # Quantity read Joystick  per second (def 20 times per second)
# Settings for the RemoteJoyBorg client 61 for ethernet 64 for wifi
broadcastIP = '192.168.1.45'           # IP address to send to, 255 in one or more positions is a broadcast / wild-card
#broadcastIP = '192.168.178.24'
broadcastPort = 5000                    # What message number to send with

# values will be send in list. Item in list explained below:
MotorPort1 = 0                          # Drive number for left motor
MotorStar2 = 1                          # Drive number for right motor
MotorVert3 = 2                          # Drive number for vertical motor
tilt       = 0.0                         # initialize tilt of webcam
Lights     = 0                          # toggle lights on or off.
## Joystick INPUT settings
regularUpdate   = True                  # If True we send a command at a regular interval, if False we only send commands when keys are pressed or released
AxesForward     = 0
AxesSide        = 1
AxesThrottle    = 2                     # regulate up down speed.
DurationmSec    = 0
##ButtonDescend   = 0
##ButtonAscend    = 1
##ButtonTiltUp    = 2
##ButtonTiltDown  = 3
##ButtonLights    = 4                     # after 0.5s turn on/off lights
##ButtonRecordStart= 5                    # after 0.5s Start recording
##ButtonRecordStop= 6                     # after 0.5s Stop recording
##Button_Quit     = 7                    # quit python script
##ButtonCalESC    = 8                     # Keep pressed for 5 seconds to start calibration routine ESC Turn off ESC, full throttle, startup ESC etc)
ServoMax    = [0.245,0.245,0.245,0.185,0.245,0.245] #limit sweep on some servos
ServoMin    = [0.055,0.055,0.055,0.13195,0.055,0.055]


# Tilt                                  # Tilt parameter down0-10up for webcam
DriveCommands   = [1,1,1,1,'x','x',1]
AxisInvert      = [1,-1,-1]              #invert axis 3
AxisAdjust      = [0.008,0.008,0]
Command=""
# Setup the connection for sending on
sender = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)       # Create the socket
##sender.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)                        # Enable broadcasting (sending to many IPs based on wild-cards)
##sender.bind(('0.0.0.0', 0))                                                         # Set the IP and port number to use locally, IP 0.0.0.0 means all connections and port 0 means assign a number for us (do not care)
#sender.sendto("JoystickSendPC1 activated.", (broadcastIP, broadcastPort))                # hello world




pygame.init()


#Loop until the user clicks the close button.
done = False

# Used to manage how fast the screen updates
clock = pygame.time.Clock()

# Initialize the joysticks
pygame.joystick.init()

# Get ready to #print
##text#print = Text#print()
buttonPressed=0
##when_pressed=0.0
# -------- Main Program Loop -----------
while done==False:
    # EVENT PROCESSING STEP
    for event in pygame.event.get(): # User did something
        if event.type == pygame.QUIT: # If user clicked close
            done=True # Flag that we are done so we exit this loop

        # Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION
        if event.type == pygame.JOYBUTTONDOWN:
            #print "Joystick button pressed."
            if buttonPressed <> 1:
                when_pressed = datetime.datetime.now()
            buttonPressed=1
        if event.type == pygame.JOYBUTTONUP:
            buttonPressed=0


    # Get count of joysticks
    joystick_count = pygame.joystick.get_count()

    # For each joystick:
    for i in range(joystick_count):
        joystick = pygame.joystick.Joystick(i)
        joystick.init()

        # Get the name from the OS for the controller/joystick
        name = joystick.get_name()
        #print "Joystick name: %s" % name

        # Usually axis run in pairs, up/down for one, and left/right for
        # the other.
        axes = joystick.get_numaxes()       #3 axis
        #print axes

        for i in range( axes ):
            axis = joystick.get_axis( i )*AxisInvert[i]+AxisAdjust[i]
            if abs(axis)<0.03: axis=0.00
            #print "Axes number is %i" % axis
            DriveCommands[i]=round(axis,2)            # fill list with values
        VertThrustAbs=round((DriveCommands[2]+ 1)/2,2)   #Vertical absolute value between 1 and 0
        DriveCommands[2]=0
        buttons = joystick.get_numbuttons()
        #print "number of buttons %i)" % buttons

        for i in range( buttons ):
            if buttonPressed ==1:
                Duration=(datetime.datetime.now()-when_pressed)
                DurationmSec=int(Duration.total_seconds()*1000)
            else:
                DurationmSec=0
##            print DurationmSec
            button = joystick.get_button( i )
            if i == 0 and button == 1:
                DriveCommands[2] = VertThrustAbs  #while pressed UP power according to lever
            if i == 1 and button == 1:
                DriveCommands[2] = -VertThrustAbs #while pressed Down power according to lever
            if i == 2 and button == 1:
                if tilt < 1.0: tilt = tilt +0.25/fps
##                print "Button nr %i value %f with value %f " %(i,button,tilt)
                hadEvent = True
            if i == 3 and button == 1:
                if tilt > -1.0: tilt = tilt -0.25/fps
                hadEvent = True
##                print "Button nr %i value %f with value %f " %(i,button,tilt)
            if i == 4 and button == 1:  #lights
                if DurationmSec > 100:
##                print when_pressed
                    if Lights == 1 : Lights=0  # 1=ON
                    else: Lights = 1
##                    print 'Lights are turned : %i' % Lights
                    buttonPressed=0
            if i == 5 and button == 1:      #start recording picam
                if DurationmSec > 200:
                    Command='RecStart'
##                    print 'Stream recording turned ON ( %s )' %Command
                    buttonPressed=0
            if i == 6 and button == 1:      #stop recording picam
                if DurationmSec > 200:
                    Command='RecStop'
##                    print 'Stream recording turned OFF ( %s )' %Command
                    buttonPressed=0
            if i == 7 and button == 1:
                if DurationmSec > 500:
                    sender.sendto("Bye", (broadcastIP, broadcastPort))
                    print "Button 8 is pressed, server terminated"
                    quit()               #stop script
        now= (datetime.datetime.now())
#        timecode=now.hour*3600+now.minute*60+now.second+int(now.microsecond/100000)/10.0


        DriveCommands[3]=round(tilt,2)
        DriveCommands[4]=Command                #commands button only one simultaneously
        DriveCommands[5]=Lights
        DriveCommands[6]=now

        for i in range(0,4): #keep values within allowable
            if DriveCommands[i] <-1.0 : DriveCommands[i]=-1.0
            elif DriveCommands[i] >1.0: DriveCommands[i]=1.0


        M1=DriveCommands[1]+DriveCommands[0]          # Convert forward-side to lef and right
        M2=DriveCommands[1]-DriveCommands[0]         # motor.
        if M1 >1:M1=1.0
        elif M1 < -1.0: M1=-1.0
        if M2 >1 :M2=1
        elif M2 < -1.0: M2=-1.0
        DriveCommands[0]=M1                      # overwrite original array (ugly way)
        DriveCommands[1]=M2

        for ID in range(0,4):
            PulseNeu=(ServoMax[ID]+ServoMin[ID])/2  #0.150 0.95 range=PulsePos
            PulseNeg=PulseNeu-ServoMin[ID]      # width above 0 0.095
            PulsePos=ServoMax[ID]-PulseNeu      # width below 0
##            print PulsePos,PulseNeu,PulseNeg
            if DriveCommands[ID]>0.0 : pulse=DriveCommands[ID]*PulsePos+PulseNeu
            else: pulse=DriveCommands[ID]*PulseNeg+PulseNeu         # axis already <= 1 max.
##            print "pulse of servo %i is %f " % (ID,pulse)
            DriveCommands[ID]=pulse

        for i in range(0,7):
            DriveCommands[i]=str(DriveCommands[i])
        print DriveCommands


        sender.sendto(','.join(DriveCommands), (broadcastIP, broadcastPort))
##        recv_data, addr = sender.recvfrom(2048)
##        recv_data, addr = sender.recvfrom(2048)
##        print recv_data
        # Hat switch. All or nothing for direction, not like joysticks.
        # Value comes back in an array.
##        hats = joystick.get_numhats()
##        #print "Number of hats: %f" %hats
##
##
##        for i in range( hats ):
##            hat = joystick.get_hat( i )
##            #print "Hat %f value: %s" % i,hat



    # ALL CODE TO DRAW SHOULD GO ABOVE THIS COMMENT

    # Go ahead and update the screen with what we've drawn.
##    pygame.display.flip()

    # Limit to 4 frames per second
    clock.tick(fps)



    # Get the currently pressed keys on the keyboard
    #PygameHandler(pygame.event.get())
##    if hadEvent or regularUpdate:
##        # Keys have changed, generate the command list based on keys
##        hadEvent = False
##        driveCommands = ['X', 'X', 'X', 'X']                    # Default to do not change
##        if AxesTilt:
##            driveCommands[0] = tilt
##            driveCommands[rightDrive - 1] = 'ON'

# Close the window and quit.
# If you forget this line, the program will 'hang'
# on exit if running from IDLE.

pygame.quit ()