rts-sim-module/sys/device_sys/psd_motor.go

package device_sys

import (
	"joylink.club/ecs"
	"joylink.club/ecs/filter"
	"joylink.club/rtsssimulation/component"
	"joylink.club/rtsssimulation/consts"
)

type PsdMotorSys struct {
	query *ecs.Query
}

func NewPsdMotorSys() *PsdMotorSys {
	return &PsdMotorSys{
		query: ecs.NewQuery(filter.Contains(component.PsdMotorStateType)),
	}
}

func (s *PsdMotorSys) Update(world ecs.World) {
	var rate int32 = 10000 / 4
	s.query.Each(world, func(entry *ecs.Entry) {
		psdMotorState := component.PsdMotorStateType.Get(entry)
		if psdMotorState.Gm_Td {
			gm(psdMotorState, rate)
		} else if psdMotorState.Km4_Td {
			km4(psdMotorState, rate)
		} else if psdMotorState.Km8_Td {
			km8(psdMotorState, rate)
		}
	})
}

func gm(psdMotorState *component.PsdMotorState, rate int32) {
	if psdMotorState.Km4 != consts.TwoPosMin {
		newRate := psdMotorState.Km4 - rate
		if newRate < consts.TwoPosMin {
			psdMotorState.Km4 = consts.TwoPosMin
		} else {
			psdMotorState.Km4 = newRate
		}
	}
	if psdMotorState.Km8_4 != consts.TwoPosMin {
		newRate := psdMotorState.Km8_4 - rate
		if newRate < consts.TwoPosMin {
			psdMotorState.Km8_4 = consts.TwoPosMin
		} else {
			psdMotorState.Km8_4 = newRate
		}
	}
}

func km4(psdMotorState *component.PsdMotorState, rate int32) {
	if psdMotorState.Km4 != consts.TwoPosMax {
		newRate := psdMotorState.Km4 + rate
		if newRate > consts.TwoPosMax {
			psdMotorState.Km4 = consts.TwoPosMax
		} else {
			psdMotorState.Km4 = newRate
		}
	}
}

func km8(psdMotorState *component.PsdMotorState, rate int32) {
	if psdMotorState.Km4 != consts.TwoPosMax {
		newRate := psdMotorState.Km4 + rate
		if newRate > consts.TwoPosMax {
			psdMotorState.Km4 = consts.TwoPosMax
		} else {
			psdMotorState.Km4 = newRate
		}
	}
	if psdMotorState.Km8_4 != consts.TwoPosMax {
		newRate := psdMotorState.Km8_4 + rate
		if newRate > consts.TwoPosMax {
			psdMotorState.Km8_4 = consts.TwoPosMax
		} else {
			psdMotorState.Km8_4 = newRate
		}
	}
}