#模拟一个电机加减速-----------------------------------------------
#仿真电机模型,输入pwm,输出转速,根据时间转速改变
#输入pwm,pwm进行一定增益作为积分累加到转速上面,一个pwm对应一个最大转速
#带一个惯性变量,达到最大值之后有一个惯性累加到转速上面



# motorSpeedMin = 10
# motorSpeedMax = 1000
# motorPwmMin = 100
# motorPwmMax = 1500

# def pwm_to_speed(pre_pwm):
#     global motorSpeedMin
#     global motorSpeedMax
#     global motorPwmMin
#     global motorPwmMax

#     if pre_pwm < motorPwmMin:
#         return 0
#     pwm2speed = (motorSpeedMax - motorSpeedMin) * (pre_pwm - motorPwmMin) / (motorPwmMax - motorPwmMin) + motorSpeedMin 
#     return pwm2speed


# lvSum = 0
# lvCoe = 4
# lvSpeed = 0
# def motor_mode(pwm):
#     global lvSum
#     global lvSpeed
#     global lvSpeed

#     newSpeed = pwm_to_speed(pwm)
#     lvSum = lvSum + newSpeed - lvSpeed
#     lvSpeed = lvSum/lvCoe
#     return lvSpeed

# def motor_reset():
#     global lvSum
#     global lvSpeed
#     global lvSpeed

#     lvSum = 0
#     lvCoe = 4
#     lvSpeed = 0
# #模拟一个电机加减速=========================================================
    
class k_motor:
    motorSpeedMin = 10
    motorSpeedMax = 1000
    motorPwmMin = 100
    motorPwmMax = 1500

    lvSum = 0
    lvCoe = 4
    lvSpeed = 0

    def __init__(self):
        print("init a motor mode")

    def pwm_to_speed(self, pre_pwm):
        # global motorSpeedMin
        # global motorSpeedMax
        # global motorPwmMin
        # global motorPwmMax

        if pre_pwm < self.motorPwmMin:
            return 0
        pwm2speed = (self.motorSpeedMax - self.motorSpeedMin) * (pre_pwm - self.motorPwmMin) / (self.motorPwmMax - self.motorPwmMin) + self.motorSpeedMin 
        return pwm2speed

    def motor_mode(self, pwm):
        # global lvSum
        # global lvSpeed
        # global lvSpeed

        newSpeed = self.pwm_to_speed(pwm)
        self.lvSum = self.lvSum + newSpeed - self.lvSpeed
        self.lvSpeed = self.lvSum/self.lvCoe
        return self.lvSpeed

    def motor_reset(self):
        # global lvSum
        # global lvSpeed
        # global lvSpeed

        self.lvSum = 0
        self.lvCoe = 4
        self.lvSpeed = 0



if __name__ == "__main__":

    M = k_motor()

    print(M.pwm_to_speed(100))
    print(M.pwm_to_speed(1400))
    print(M.pwm_to_speed((1500 - 100)/2 + 100))