car/lib/Pwm.js

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: 0,
            m2: 1,
            s1: 2,
            s2: 3
        }
        // prio init
        RPIO.init({ mapping: "gpio" });
        // 继电器解锁
        RPIO.open(22, RPIO.OUTPUT, RPIO.LOW);
        RPIO.open(23, RPIO.OUTPUT, RPIO.LOW);
        // 开灯
        RPIO.open(22, RPIO.OUTPUT, RPIO.HIGH);
        // init PWM
        this.PWM = new Pca9685Driver(this.pwmOption, er => {
            if (er) {
                console.error("Error initializing PCA9685");
            }
        })
        // 解锁计数器
        this.Snum = 0
        // 解锁状态
        this.enable = false
    }
    // 设置pwm
    async changPWM (params) {
        const { v0, v1, v2, v3 } = params
        if (typeof (v0) == 'number' || typeof (v3) == 'number') {
            if (this.enable) {
                // 解锁之后需要2秒给电调中立值
                if (this.Snum >= 50 && this.Snum <= 61) this.Snum++
                if (this.Snum >= 60) {
                   if (Math.abs(+v2 - 128) < 20) {
                        console.log('前后', v3)
                        this.PWM.setPulseLength(this.MoAndSero.m1, this.Motor2pwm(Math.abs(+v3 - 256)));
                        this.PWM.setPulseLength(this.MoAndSero.m2, this.Motor2pwm(Math.abs(+v3 - 256)));
                    } else {
                        // 分左右
                        if ((+v2 - 128) > 20) {
                            // 右
                            console.log('右', +v2);
                            const a = 128 - (+v2 - 128)
                            const b = 128 + (+v2 - 128)
                            this.PWM.setPulseLength(this.MoAndSero.m1, this.Motor2pwm(+a));
                            this.PWM.setPulseLength(this.MoAndSero.m2, this.Motor2pwm(+b));
                        } else if (+v2 - 128 < -20) {
                            // 左
                            console.log('左', +v2);
                            const a = 128 - (+v2 - 128)
                            const b = 128 + (+v2 - 128)
                            this.PWM.setPulseLength(this.MoAndSero.m1, this.Motor2pwm(+a));
                            this.PWM.setPulseLength(this.MoAndSero.m2, this.Motor2pwm(+b));
                        }
                    }
                    //云台
                    if (v0 || v1) {
                        console.log('云台', 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));
                    }
                }
            } else {
                this.unLOCK(v0, v1, v2, v3)
            }
        } else {
            console.log('参数格式错误，自动略过');
            return
        }
    }

    /**
     * 内八解锁
     * @param {*} v0 
     * @param {*} v1 
     * @param {*} v2 
     * @param {*} v3 
     */
    unLOCK (v0, v1, v2, v3) {
        // 未解锁=>进行解锁:发送端的频率是0.04S，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(23, RPIO.OUTPUT, RPIO.HIGH);
            this.enable = true
        }
        console.log('解锁中', v0, v1, v2, v3)
    }
    /**
     * 电调换算比例：摇杆范围0-256，PWM范围500~1500~2500
     * @param {number} v 
     * @returns 
     */
    Motor2pwm (v) {
        // 分为6挡位,中值128,步进256/6
        const num = parseInt(256 / 6)
        // 方向调换
        const dirc = Math.abs(v - 256)
        // 真实值
        let value = 128
        // 步进取中间值
        if (dirc > 0 && dirc < num) {
            value = num / 2
        } else if (dirc > num && dirc <= num * 2) {
            value = num + (num / 2)
        } else if (dirc > num * 2 && dirc < 128) {
            value = 128 - (num / 2)
        } else if (dirc > 128 && dirc <= (128 + num)) {
            value = 128 + (num / 2)
        } else if (dirc > (128 + num) && dirc <= (128 + num * 2)) {
            value = 128 + num + (num / 2)
        } else if (dirc > (128 + num * 2) && dirc <= 256) {
            value = 256 - (num / 2)
        } else {
            value = 128
        }

        return parseInt(value / (256 / 2000)) + 500
    }
    /**
     * 舵机换算比例：摇杆范围0-256，PWM范围1000~1500~2000
     * @param {number} v 
     * @returns 
     */
    Servo2pwm (v) {
        return parseInt(v / (256 / 1000)) + 1000
    }
}
module.exports = new ContrlService()