main.py

import machine 
from machine import Pin, Timer,ADC,PWM
import sys
from time import sleep_ms
import ubluetooth
import esp32
from esp32 import raw_temperature
import _thread
import time
import math
runSavedCode=True #can be set to false to prevent saved code from interrupting the loading of a new program
runProgram=True #keep the program running in a loop
programStatus=0
lastData=""
lastDataTime=-1
import math
def waveSynth(freq,power,duration=1,waveform="sine",lowcut=0,highcut=100, phaseOffset=[0,0,0],stepSize=0.2):
    powerCheck=[len(freq), len(power)]
    if max(powerCheck) != min(powerCheck):
        return -1 #arrays are not the right size
    else:
        while len(phaseOffset) < len(freq):
            phaseOffset.append(0)
        motors=[] 
        totalPoints=(6.28*duration)/stepSize
        timing=[]
        for motor in range(0,len(freq)):
            #generate a sine wave
            temp=[]
            x=0 
            while (round(x,2) < (6.28*duration)):
                #set up the timing array
                if motor == 0:
                    timing.append(duration/totalPoints)
                tempVal=(math.sin((x+phaseOffset[motor])*freq[motor])+1)*power[motor]/2 #scale in the x domain (to control frequency) and in the y domain to control how the power is mpped to motor power
                if waveform == "square": #if user asked for a square wave then threshold the sine wave
                    if (tempVal > 50):
                        tempVal=100
                    else:
                        tempVal=0
                if tempVal < lowcut:
                    tempVal=lowcut
                if tempVal > highcut:
                    tempVal=highcut
                temp.append(int(tempVal))
                x=x+stepSize
            motors.append(temp)
        return [motors,timing]
def combineWaves(wave1,wave2,weight=1.0):
    
    """combines two waveforms (with equal length) toghether.
    
    Arguments:
    wave1 -- list containing points in the first waveform
    wave2 -- list contianing points in the second waveform
    weight -- how much to weight the second waveform relative to the first. 1.0 is equal weight
    """
    newWave=[]
    if len(wave1) != len(wave2):
        print("Error, the two waves are not equal length")
        return -1
    else:
        for i in range(0,len(wave1)):
            #We combine add the two waves but scale the second by weight. Then we scale them so that the overall sum is the same as the original waveform
            temp=((wave1[i]/2)+((wave2[i]/2)*weight))
            #find the amplitude of the new point compared to the old point, and use this to rescale the wave so that the amplitude doesn't change
            scale=(1+weight)/2 
            newWave.append(int(temp/scale))
            
    return newWave
def modulate(wave1,wave2):
    
    """modulates wave1, computing wave=wave1*(wave2/100)
    
    Arguments:
    wave1 -- list containing points in the first waveform
    wave2 -- list contianing points in the second waveform
    """
    newWave=[]
    if len(wave1) != len(wave2):
        print("Error, the two waves are not equal length")
        return -1
    else:
        for i in range(0,len(wave1)):
            newWave.append(int(wave1[i]*(wave2[i]/100))) 
            
    return newWave

def heatMotors(duration): #experimental, tries to create heat but not motion on the vibration motors
    print("initial temp:"+str(raw_temperature())) 
    pin1=PWM(Pin(12,Pin.OUT),freq=10000000,duty=700) 
    pin2=PWM(Pin(14,Pin.OUT),freq=10000000,duty=700)
    pin3=PWM(Pin(33,Pin.OUT),freq=10000000,duty=700)
    time.sleep(duration) 
    pin1.deinit()
    pin2.deinit()
    pin3.deinit()
    print("final temp:"+str(raw_temperature()))
    
def chirpMotors(): #creates a breif motor chirp as confirmation
    pin1=PWM(Pin(12,Pin.OUT),freq=1000,duty=500) 
    pin2=PWM(Pin(14,Pin.OUT),freq=1000,duty=500)
    pin3=PWM(Pin(33,Pin.OUT),freq=1000,duty=500)
    time.sleep(0.1)
    pin1.duty(0)
    pin2.duty(0)
    pin3.duty(0)
    time.sleep(0.1)
    pin1.duty(500)
    pin2.duty(500)
    pin3.duty(500)
    time.sleep(0.1)
    pin1.deinit()
    pin2.deinit()
    pin3.deinit()
    
def vibePattern(motorPower,timing, loop=0,pwmFreq=1000): #execute a custom vibration pattern
   #the current PWM system has some weird behavior (I think a bug) so we are doing this custom bitbang pwm for now
    if len(motorPower) == 3: 
        powerCheck=[len(motorPower[0]), len(motorPower[1]),len(motorPower[2]),len(timing)]
        if max(powerCheck) == min(powerCheck): #check to make sure the lists are all the right length
            numSegments=len(motorPower[0])
            totalLoops=loop
            pin1=PWM(Pin(12,Pin.OUT),freq=pwmFreq,duty=0) 
            pin2=PWM(Pin(14,Pin.OUT),freq=pwmFreq,duty=0)
            pin3=PWM(Pin(33,Pin.OUT),freq=pwmFreq,duty=0)
            while totalLoops >= 0:
                totalLoops=totalLoops-1
                
                for segment in range(0,numSegments):
                    pin1.duty(motorPower[0][segment]*10)
                    pin2.duty(motorPower[1][segment]*10)
                    pin3.duty(motorPower[2][segment]*10)
                    time.sleep(timing[segment])
            pin1.deinit() 
            pin2.deinit()
            pin3.deinit() 
            return True
        else:
            print("Motor sequences don't agree")
            return -1
    else:
        print("Wrong number of motor sequences")
        return -2
    
    
def runProgramFromStorage():
    global programStatus
    print("Attempting to run program")
    if runSavedCode: 
        progFile=open("program.py")
        program=progFile.read() 
        progFile.flush()
        progFile.close()
        print(program)
        
        while runProgram:
            exec(program,globals(),locals())
        programStatus=2
        

def handleMessage(message):
    global lastData
    global lastDataTime 
    if "PROG:" in message:
        print("Loading new program...")
        program=message.replace("ENDMSG","").replace("PROG:","")
        print("Program")
        print(program)
        progFile=open("program.py",'w')
        progFile.write(program)
        progFile.flush()
        progFile.close()
        chirpMotors()
        machine.reset() 
    else:
        lastData=message
        lastDataTime=time.time()
        

class BLE():
 
    
    def __init__(self, name):
        self.isConnected=False
        self.name = name
        self.ble = ubluetooth.BLE()
        self.ble.active(True)

        
        self.disconnected()
        self.ble.irq(self.ble_irq)
        self.register()
        self.advertiser()
        self.messageBuffer=""

    
    def connected(self):
        self.isConnected=True
        


    def disconnected(self):
        self.isConnected=False
        
    

    def ble_irq(self, event, data):
        global messageBuffer
        if event == 1:
            '''Central connected'''
            self.connected()
    
        elif event == 2:
            '''Central disconnected'''
            self.register()
            self.advertiser()
            self.disconnected()
        
        elif event == 3:
            '''New message received'''
            global runSavedCode         
            runSavedCode=False
            buffer = self.ble.gatts_read(self.rx)
            message = buffer.decode('UTF-8')[:-1]
            self.messageBuffer=self.messageBuffer+message
            print(str(len(self.messageBuffer)))
            if ("ENDMSG") in self.messageBuffer:
                print(self.messageBuffer)
                handleMessage(self.messageBuffer)
                self.messageBuffer=""
                

            
    def register(self):
        
        # Nordic UART Service (NUS)
        NUS_UUID = '6E400001-B5A3-F393-E0A9-E50E24DCCA9E'
        RX_UUID = '6E400002-B5A3-F393-E0A9-E50E24DCCA9E'
        TX_UUID = '6E400003-B5A3-F393-E0A9-E50E24DCCA9E'
            
        BLE_NUS = ubluetooth.UUID(NUS_UUID)
        BLE_RX = (ubluetooth.UUID(RX_UUID), ubluetooth.FLAG_WRITE)
        BLE_TX = (ubluetooth.UUID(TX_UUID), ubluetooth.FLAG_NOTIFY)
            
        BLE_UART = (BLE_NUS, (BLE_TX, BLE_RX,))
        SERVICES = (BLE_UART, )
        ((self.tx, self.rx,), ) = self.ble.gatts_register_services(SERVICES)

    def send(self,data): #format the data for transmission to the companion app
        MTU=5 
        temp=""
        data=data+"ENDMSG"
        for char in data:
            if len(temp) <= MTU:
                temp=temp+char
            else:
                self.sendll(temp)
                temp=char
        self.sendll(temp) 
    def sendll(self, data): #low level send, doesn't check the data size etc
        self.ble.gatts_notify(0, self.tx, data)


    def advertiser(self):
        name = bytes(self.name, 'UTF-8')
        self.ble.gap_advertise(100, bytearray('\x02\x01\x02') + bytearray((len(name) + 1, 0x09)) + name)
        
# test
p12=Pin(12,Pin.OUT);
p12.value(False)
p14=Pin(14,Pin.OUT);
p14.value(False)
p33=Pin(33,Pin.OUT);
p33.value(False)
#toggle the program on or off
spEnabled=True
if (machine.reset_cause() ==1): #this means it's a poweron reset, i.e. the user pressed the reset button and we should toggle whether the program runs
    try:
        rebootFile=open("runProgram.txt",'r')
        result=rebootFile.read()
        rebootFile.close()
        rebootFile=open("runProgram.txt",'w')
        print(result)
        if "true" in result:
            spEnabled=True
            print("Prog enabled")
            rebootFile.write("false")
        else:
            spEnabled=False
            print("Prog disabled")

            chirpMotors()
            rebootFile.write("true")
        rebootFile.flush()
        rebootFile.close()
    except Exception:
        print("Error checking reboot status")
        rebootFile=open("runProgram.txt",'w')
        rebootFile.write("true")
        rebootFile.flush()
        rebootFile.close()

    

ble = BLE("OpenHaptic")
batteryVoltage=ADC(Pin(35)) 
batteryVoltage.atten(ADC.ATTN_11DB) #full range
if(spEnabled):
    _thread.start_new_thread(runProgramFromStorage, ()) #run the stored program
while True:
    if ble.isConnected: #if we have a connection, send a status message every couple of seconds
        try:
            print("sending status")
            ble.send("ST:"+str(round((batteryVoltage.read()/4095)*6.6,2))+":"+str(programStatus))
        except Exception: #this can break due to interference by a program
            pass
    time.sleep(2)