ProsthesisInterface.py

Go to the documentation of this file.

#!/usr/bin/env python3
# Example of interaction with a BLE UART device using a UART service
# implementation.
# Author: Tony DiCola
#import Adafruit_BluefruitLE
#from Adafruit_BluefruitLE.services import UART
 
import re
import numpy as np
 
# Get the BLE provider for the current platform.
#ble = Adafruit_BluefruitLE.get_provider()
 
import sys, traceback, Ice
import IceStorm
import time
 
#from bmp280 import BMP280
 
status = 0
ic = None
 
def main():
 
    ########################################################## ICE part
    try:
        properties = Ice.createProperties(sys.argv)
        properties.load("./default.generated.cfg")
        properties.load("./default.cfg")
 
        init_data = Ice.InitializationData()
        init_data.properties = properties
 
        ic = Ice.initialize(init_data)
 
        Ice.loadSlice('-I%s %s -DMININTERFACE' % (Ice.getSliceDir(), 'ProsthesisInterface.ice'))
        print("loadSlice")
        import armarx
 
 
        obj = ic.stringToProxy("IceStorm/TopicManager")
        print("stringToProxy")
 
        topicMan = IceStorm.TopicManagerPrx.checkedCast(obj)
        print("checkedCast")
        topicName = "ProsthesisMotorValues"
        print(topicName)
 
        try:
            topic = topicMan.retrieve(topicName);
        except IceStorm.NoSuchTopic:
            print("No such topic.")
            try:
 
                topic = topicMan.create(topicName)
                print("Try to create topic.")
 
            except IceStorm.TopicExists:
                # if the topic has been created in the meanwhile
                # retry the retrieval.
                topic = topicMan.retrieve(topicName)
                print("Try again.")
 
        pub = topic.getPublisher().ice_oneway();
        prosthesis = armarx.ProsthesisListenerInterfacePrx.uncheckedCast(pub);
 
 
        #base = ic.stringToProxy("SimpleprosthesisInterfaceUnit")
 
        #prosthesis = armarx.SimpleprosthesisInterfacePrx.checkedCast(base)
        if not prosthesis:
                raise RuntimeError("Invalid proxy")
 
        #val = 0
        #print(val)
    
    except:
        traceback.print_exc()
        status = 1
    '''
    ############################################################## BLE part
 
    motorData = np.zeros((2,2))
 
    # Clear any cached data because both bluez and CoreBluetooth have issues with
    # caching data and it going stale.
    ble.clear_cached_data()
 
    # Get the first available BLE network adapter and make sure it's powered on.
    adapter = ble.get_default_adapter()
    adapter.power_on()
    print('Using adapter: {0}'.format(adapter.name))
 
    # Disconnect any currently connected UART devices.  Good for cleaning up and
    # starting from a fresh state.
    print('Disconnecting any connected UART devices...')
    UART.disconnect_devices()
 
    # Scan for UART devices.
    print('Searching for UART device...')
    try:
        adapter.start_scan()
        # Search for the first UART device found (will time out after 60 seconds
        # but you can specify an optional timeout_sec parameter to change it).
        device = UART.find_device()
        if device is None:
            raise RuntimeError('Failed to find UART device!')
    finally:
        # Make sure scanning is stopped before exiting.
        adapter.stop_scan()
 
    print('Connecting to device...')
    print(device)
    device.connect()  # Will time out after 60 seconds, specify timeout_sec parameter
                      # to change the timeout.
 
    # Once connected do everything else in a try/finally to make sure the device
    # is disconnected when done.
    try:
        # Wait for service discovery to complete for the UART service.  Will
        # time out after 60 seconds (specify timeout_sec parameter to override).
        print('Discovering services...')
        UART.discover(device, timeout_sec=10)
 
        # Once service discovery is complete create an instance of the service
        # and start interacting with it.
        uart = UART(device)
        print("registered")
        # Write a string to the TX characteristic.
        #uart.write('g1\r\n')
        #print("Sent 'Hello world!' to the device.")
 
        # Now wait up to one minute to receive data from the device.
        rawString = None
        while(1):
                #print('Waiting up to 60 seconds to receive data from the device...')
                received = uart.read(timeout_sec=60)
                if received is not None:
                        # Received data, print it out.
                        #print('Received: {0}'.format(received))
                        rawString = (rawString if rawString else "") + received
                #else:
                        # Timeout waiting for data, None is returned.
                        #print('Received no data!')
 
                received = None
                lineSep = rawString.split('\n')
                while len(lineSep) > 1:
                        fullLine = lineSep.pop(0)
                        print(fullLine)
                        dataList = [float(s) for s in re.findall(r'-?\d+\.?\d*', fullLine)]
                        motorNo = dataList[0]
                        motorPos = dataList[1]
                        motorPwm = dataList[2]
                        motorData[0,0] = motorPos
                        motorData[0,1] = motorPwm
                        #print('Motor No. %f at position %f with PWM %f.' %(motorNo, motorPos, motorPwm))
 
                rawString = lineSep[0]
                #sensor.readTemperature()
                values = armarx.ProsthesisMotorValues(name = "motorValues", position1 = motorData[0,0], pwm1 = motorData[0,1], position2=motorData[1,0], pwm2 = motorData[1,1])
                #values.name = "Test"
                prosthesis.reportMotorValues(values)
                #time.sleep(0.01)
                #val = val + 1
                #print(val)
 
    finally:
        # Make sure device is disconnected on exit.
        device.disconnect()
 
        if ic:
            #clean up
            try:
                ic.destroy()
            except:
                traceback.print_exc()
                status = 1
 
        sys.exit(status)
    '''
# Initialize the BLE system.  MUST be called before other BLE calls!
#ble.initialize()
 
# Start the mainloop to process BLE events, and run the provided function in
# a background thread.  When the provided main function stops running, returns
# an integer status code, or throws an error the program will exit.
#ble.run_mainloop_with(main)
main()
values = armarx.ProsthesisMotorValues(name = "motorValues", position1 = 0, pwm1 = 1, position2 = 2, pwm2 = 3)
                #values.name = "Test"
prosthesis.reportMotorValues(values)