from ina219 import INA219, DeviceRangeError
from time import sleep
import time
import math
try:
    import RPi.GPIO as GPIO
except RuntimeError:
    print("Error importing RPi.GPIO!")
GPIO.setmode(GPIO.BOARD)

SHUNT_OHMS = 0.1
#MAX_EXPECTED_AMPS = 2.0
MAX_EXPECTED_AMPS = 0.3
ina = INA219(SHUNT_OHMS, MAX_EXPECTED_AMPS)
#ina.configure(ina.RANGE_16V, ina.GAIN_1_40MV)
ina.configure(ina.RANGE_16V)
marchearret = ''
# résistance interne du moteur (ohm)
#r = 3.17 # Ω mot. inconnu
r = 1.18 # Ω mot. Opitec
temps = 0

def read_ina219():
    global r, poids, forcedynamometre
    brocheHall = 11
    # On est obligé de mettre à up la broche de Hall, car sinon, elle
    # est aléatoirement fixée.
    GPIO.setup(brocheHall, GPIO.IN, pull_up_down=GPIO.PUD_UP)
    nbtours = 100
    n = 0
    drapeau = 'declanche'
    temps0 = time.time()
    while (n <= nbtours):
        etatBrocheHall = GPIO.input(brocheHall)
        if (etatBrocheHall == GPIO.LOW):
            if (drapeau != 'enclanche'):
                n = n + 1
                #print('Enclanché')
                drapeau = 'enclanche'
        else:
            #print('-')
            drapeau = 'declanche'
        time.sleep(0.001)
    temps = time.time()-temps0
    frequence = nbtours/temps
    omega = 2*math.pi*frequence
    print(temps)
    print('Fréquence mot. : {0:0.3f}trs/s'.format(frequence))
    print('Vit. angulaire : {0:0.3f}rad/s'.format(omega))
    try:
        print('Bus Voltage: {0:0.3f}V'.format(ina.voltage()))
        print('Shunt Voltage: {0:0.3f}mV'.format(ina.shunt_voltage()))
        print('Bus Current: {0:0.3f}mA\n'.format(ina.current()))
        eprime = ina.voltage()-r*(ina.current()/1000)
        fichier = open("data_mot_hall_poids.txt", "a")
        fichier.write('{0:0.3f}'.format(omega))
        fichier.write(' {0:0.3f}'.format(eprime))
        fichier.write(' {0:0.3f}'.format(ina.voltage()))
        fichier.write(' {0:0.3f}'.format(ina.current()))
        iw = frequence * 0.001/temps
        fichier.write(' {0:0.3f}'.format(iw))
        iv = ina.voltage()*0.005
        ia = ina.current()*0.005
        ieprime = iv + r*(ina.current()/1000)*(0.5/r+(ia/1000)/(ina.current()/1000))
        fichier.write(' {0:0.3f}'.format(ieprime))
        fichier.write(' {0:0.3f}'.format(iv))
        fichier.write(' {0:0.3f}'.format(ia))
        fichier.write(' {0:0.2f}'.format(poids))
        fichier.write(' {0:0.2f}\n'.format(forcedynamometre))
        fichier.close()

    except DeviceRangeError as e:
        # Current out of device range with specified shunt resister
        print(e)

while marchearret != 'q':
    marchearret = input('Tapez ENTER pour enregistrer une mesure, q pour quitter et n pour une nouvelle série de mesure : ')
    if marchearret != 'q':
        poids = float(input('Entrez la masse pendante : '))
        forcedynamometre = float(input('Entrez la force lue sur le dynamomètre : '))
        if marchearret == 'n':
            fichier = open("data_mot_hall_poids.txt", "a")
            fichier.write('\n\n')
            fichier.close()
        read_ina219()