import numpy as np
import matplotlib.pyplot as plt


#3相霍尔时间转换问题
#A相作为捕捉计算速度,BC相只做io中断作为相位计算使用

#电机参数
Mpoles = 5#极对数


#需要计算的数据
# eThetaInc = 0#点角速度增量 rad
# eOmega = 0#电角速度 rad/s
# mOmega = 0#机械角速度 rad/s
# mSpeed = 0#机械转速  rpm


#定时器计数值转电角速度
timerFrq = 72000000 #hz 定时器基准频率
timerDiv = 128 #定时器分频
pwmFrq = 16000 #hz

def count_conver_to_eOmega(cnt):
    #计算公式  rad/s
    #角速度 = 弧度(a相转换弧度-PI-180°) / 时间(计算值*单个计算值的时间 = div/Frq)
    #omega = pi / (cnt * 1 / (frq/div) )   v = s/t
    # omega = np.pi / (cnt * (1 / (timerFrq / timerDiv)))
    omega = np.pi * timerFrq / timerDiv / cnt
    return omega

def eOmega_conver_to_count(omega):
    #cnt = pi / omega * (frq / div)  t = s / v
    cnt = (np.pi / omega) / (1 / (timerFrq / timerDiv))
    return cnt

def rpm_to_eOmega(rpm):
    #rpm 转/分钟 -> 电角速度 rad/秒
    # rps = rpm / 60 |  omega = rps * (2 * PI)
    #公式 角速度(rad/s) = (rpm / 60) * (2 * Pi)
    #电角速度 = 机械角速度 * poles
    omega = rpm * 2 * np.pi / 60 * Mpoles
    return omega

def eOmega_to_rpm(omega):
    #电角速度(rad/s)-转-转速(rpm)
    rpm = omega * 60 / (2 * np.pi * Mpoles)
    return rpm

def theta_inc_per_pwm(omega):
    #输入电角速度,输出每个pwm周期内递增的弧度(电角度)
    #输出标幺化Q15格式
    # 角速度*pwm周期
    # theta_inc = omega * ( 1 / pwmFrq) * (32767 / (2 * np.pi) ) 
    theta_inc = omega * 32767 / 2 / np.pi / pwmFrq
    return theta_inc

def theta_to_index(theta, total):
    index = theta * total / 32767
    return index

def count_to_theta_inc_pre_pwm(cnt):
    # omega = np.pi * timerFrq / timerDiv / cnt
    # theta_inc = omega * 32767 / 2 / np.pi / pwmFrq
    # theta_inc = np.pi * timerFrq / timerDiv / cnt * 32767 / 2 / np.pi / pwmFrq
    theta_inc = timerFrq / timerDiv / cnt * 32767 / 2 / pwmFrq
    return theta_inc

#得出结论是  角度做标幺化(0-32768),方便计算sin cos 查表(0-512)

#----速度做标幺化的优势是什么??
speedBase = 3100    #标幺化基准值
speedMax = 3000
speedMin = 500
speedToBaseCoe = 32767 / speedBase
baseToSpeedCoe = speedBase / 32767

#============================
if __name__ == "__main__":


    speed = speedToBaseCoe * 500
    speed = 5000
    eOmega = rpm_to_eOmega(speed)
    speed = eOmega_to_rpm(eOmega)
    print("speed:", speed, "eOmega:", eOmega)

    cnt = eOmega_conver_to_count(eOmega)
    eOmega = count_conver_to_eOmega(cnt)
    
    print("cnt:", cnt, "eOmega:", eOmega)

    theta_inc = theta_inc_per_pwm(eOmega)
    print("theta_inc:", theta_inc, "eOmega:", eOmega)
    theta_inc = count_to_theta_inc_pre_pwm(cnt)
    print("theta_inc:", theta_inc, "eOmega:", eOmega)
    nums = 2 * np.pi / eOmega / ( 1 / pwmFrq) 
    print("theta_inc:", nums * theta_inc)
    
    # cnt = 800
    # for i in range(300):
    #     cnt  = cnt + 5
    #     eOmega = count_conver_to_eOmega(cnt)
    #     speed = eOmega_to_rpm(eOmega)
    #     print(eOmega, " ", speed)