car/lib/Pwm.js

// PCA9685 PWM控制模块
const i2cBus = require("i2c-bus")
const { Pca9685Driver } = require("pca9685")
const RPIO = require("rpio");

class ContrlService {
    constructor() {
        // PWM 和 舵机配置
        this.pwmOption = {
            i2c: i2cBus.openSync(1), // 树莓派1：scl1
            address: 0x40, //板子地址
            frequency: 50, //频率
            debug: false
        }
        //PCA 板子位置电调+舵机
        this.MoAndSero = {
            m1: 1,
            m2: 0,
            s1: 2,
            s2: 3
        }
        // prio init
        RPIO.init({ mapping: "gpio" });
        // 关闭继电器
        RPIO.open(6, RPIO.OUTPUT, RPIO.LOW);
        RPIO.open(13, RPIO.OUTPUT, RPIO.LOW);
        RPIO.open(19, RPIO.OUTPUT, RPIO.LOW);
        // init PWM
        this.PWM = new Pca9685Driver(this.pwmOption, er => {
            if (er) {
                console.error("Error initializing PCA9685");
            }
        })
        // 解锁计数器
        this.Snum = 0
        // 解锁状态
        this.enable = false
        // 摇杆解锁
        this.contrlEnable = false
        // 摇杆中立值110-120
        this.centerNum = 120
    }
    // 设置pwm
    async changPWM(params) {
        const { v0, v1, v2, v3 } = params
        if (typeof (v0) == 'number' && typeof (v2) == 'number' && typeof (v1) == 'number' && typeof (v3) == 'number') {
            // 内八解锁
            if (this.enable) {
                //中位值
                if (!this.contrlEnable) {
                    this.PWM.setPulseLength(this.MoAndSero.m1, this.Motor2pwm(Math.abs(this.centerNum)));
                    this.PWM.setPulseLength(this.MoAndSero.m2, this.Motor2pwm(Math.abs(this.centerNum)));
                }
                //上推解锁
                if (!this.contrlEnable && +v3 < 200) this.contrlEnable = true
                // PWM操作
                if (this.contrlEnable) {
                    // 注意中位误差
                    if (Math.abs(+v3 - this.centerNum) < 20) {
                        this.PWM.setPulseLength(this.MoAndSero.m1, this.Motor2pwm(Math.abs(+v2 - 255)));
                        this.PWM.setPulseLength(this.MoAndSero.m2, this.Motor2pwm(Math.abs(+v2 - 255)));
                    } else {
                        // 分左右 注意左右误差
                        if ((+v3 - this.centerNum) > 20) {
                            // 右
                            const a = this.centerNum - (+v3 - this.centerNum)
                            const b = this.centerNum + (+v3 - this.centerNum)
                            this.PWM.setPulseLength(this.MoAndSero.m1, this.Motor2pwm(+a));
                            this.PWM.setPulseLength(this.MoAndSero.m2, this.Motor2pwm(+b));
                        } else if (+v3 - this.centerNum < -20) {
                            // 左
                            const a = this.centerNum - (+v3 - this.centerNum)
                            const b = this.centerNum + (+v3 - this.centerNum)
                            this.PWM.setPulseLength(this.MoAndSero.m1, this.Motor2pwm(+a));
                            this.PWM.setPulseLength(this.MoAndSero.m2, this.Motor2pwm(+b));
                        }
                    }

                }
                //云台
                if (v0 || v1) {
                    const a = +v1 > 200 ? 200 : +v1
                    this.PWM.setPulseLength(this.MoAndSero.s1, this.Servo2pwm(+v0));
                    this.PWM.setPulseLength(this.MoAndSero.s2, this.Servo2pwm(+a));
                }
                console.log('前后', v3, '左右', v2, '云台', v0, v1)
            } else {
                this.unLOCK(v0, v1, v2, v3)
            }
        } else {
            // 遥杆数据错误，断开继电器
            this.stop()
            process.exit(1)
            return
        }
    }

    /**
     * 内八解锁
     * @param {*} v0 
     * @param {*} v1 
     * @param {*} v2 
     * @param {*} v3 
     */
    unLOCK(v0, v1, v2, v3) {
        // 未解锁=>进行解锁:发送端的频率是0.05S，0.05*60 = 3S        
        // 内八 ：v0<50,v1>200,v2>200,v3<50
        if (+v0 <= 60 && +v1 >= 190 && +v3 >= 190 && +v2 <= 60) {
            if (this.Snum >= 0 && this.Snum <= 51) this.Snum++
        } else {
            this.Snum = 0
        }
        if (this.Snum == 50) {
            //继电器开灯
            RPIO.open(19, RPIO.OUTPUT, RPIO.HIGH);
            //空闲继电器
            //RPIO.open(13, RPIO.OUTPUT, RPIO.HIGH);
            //空闲继电器
            //RPIO.open(6, RPIO.OUTPUT, RPIO.HIGH);
            this.enable = true
        }
        console.log('解锁中', v0, v1, v2, v3)
    }
    /**
     * 电调换算比例：摇杆范围0-255，PWM范围500~1500~2500
     * @param {number} v 
     * @returns 
     */
    Motor2pwm(v) {
        // 使用低速
        let num = 128
        if (v < 116) {
            num = 116;
        } else if (v >= 120 && v <= 131) {
            num = 128;
        } else if (v > 140) {
            num = 140;
        }
        return parseInt(num / (255 / 2000)) + 500
    }
    /**
     * 舵机换算比例：摇杆范围0-256，PWM范围1000~1500~2000
     * @param {number} v 
     * @returns 
     */
    Servo2pwm(v) {
        return parseInt(v / (256 / 1000)) + 1000
    }

    // 关闭
    stop() {
        if (this.PWM) this.PWM.dispose()
        RPIO.open(6, RPIO.OUTPUT, RPIO.LOW);
        RPIO.open(13, RPIO.OUTPUT, RPIO.LOW);
        RPIO.open(19, RPIO.OUTPUT, RPIO.LOW);
        this.PWM = null
    }
}
module.exports = new ContrlService()