57 lines
1.2 KiB
Go
57 lines
1.2 KiB
Go
|
package message
|
|||
|
|
|
|||
|
|
import (
|
|||
|
|
"encoding/binary"
|
|||
|
|
"math"
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
// 速度转转速计算公式：
|
|||
|
|
// 转速=车速 ÷ 车轮周长
|
|||
|
|
// 车的轮径为d，车速为v1，电机转速为v2
|
|||
|
|
// 公式为：V2=V1÷（πd）×60
|
|||
|
|
type ElectricMachinery struct {
|
|||
|
|
Speed float32 // 车速,m/s
|
|||
|
|
WheelDiameter int32 // 轮径,mm
|
|||
|
|
IsBack bool // 是否倒车
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
func (t *ElectricMachinery) Encode() []byte {
|
|||
|
|
b := []byte{0x01, 0x10, 0x60, 0xff, 0x00, 0x02, 0x04}
|
|||
|
|
rotarySpeed := t.Speed * 60 / (float32(math.Pi) * float32(t.WheelDiameter) / 1000)
|
|||
|
|
if t.IsBack {
|
|||
|
|
b = binary.BigEndian.AppendUint32(b, uint32(0-rotarySpeed))
|
|||
|
|
} else {
|
|||
|
|
b = binary.BigEndian.AppendUint32(b, uint32(rotarySpeed))
|
|||
|
|
}
|
|||
|
|
crc := chkcrc(b) // crc校验码
|
|||
|
|
b = binary.BigEndian.AppendUint16(b, crc)
|
|||
|
|
return b
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 生成crc
|
|||
|
|
func chkcrc(data []byte) uint16 {
|
|||
|
|
var crc uint16 = 0xFFFF
|
|||
|
|
for i := 0; i < len(data); i++ {
|
|||
|
|
crc = calccrc(data[i], crc)
|
|||
|
|
}
|
|||
|
|
hi := crc % 256
|
|||
|
|
lo := crc / 256
|
|||
|
|
crc = (hi << 8) | lo
|
|||
|
|
return crc
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 计算crc
|
|||
|
|
func calccrc(crcbuf byte, crc uint16) uint16 {
|
|||
|
|
crc = crc ^ uint16(crcbuf)
|
|||
|
|
for i := 0; i < 8; i++ {
|
|||
|
|
chk := crc & 1
|
|||
|
|
crc = crc >> 1
|
|||
|
|
crc = crc & 0x7fff
|
|||
|
|
if chk == 1 {
|
|||
|
|
crc = crc ^ 0xa001
|
|||
|
|
}
|
|||
|
|
crc = crc & 0xffff
|
|||
|
|
}
|
|||
|
|
return crc
|
|||
|
|
}
|