PID制御でゲームを作ってみよう

PIDgame.go

package main

import (
	"fmt"
	"image/color"
	"log"
	"math"

	"github.com/hajimehoshi/ebiten"
	"github.com/hajimehoshi/ebiten/ebitenutil"
)

const (
	screenWidth  = 640
	screenHeight = 480
)

type Player struct {
	px float64
	py float64

	vx float64
	vy float64

	kp float64
	ki float64
	kd float64

	diffArrayX []float64
	diffArrayY []float64
}

type TargetPoint struct {
	radius float64
	px     float64
	py     float64
}

type Stage struct {
	player          *Player
	targetPointList []TargetPoint

	currentTargetId int
	windX           float64
	windY           float64
	accelLimit      float64
	speedLimit      float64
}

var stage Stage

func initStage() {
	stage.player = new(Player)

	stage.accelLimit = 1
	stage.speedLimit = 10

	stage.windX = 0
	stage.windY = 0.2

	stage.targetPointList = []TargetPoint{
		TargetPoint{
			radius: 20,
			px:     30,
			py:     40,
		},
		TargetPoint{
			radius: 20,
			px:     300,
			py:     40,
		},

		TargetPoint{
			radius: 20,
			px:     330,
			py:     350,
		},

		TargetPoint{
			radius: 20,
			px:     40,
			py:     400,
		},

		TargetPoint{
			radius: 20,
			px:     200,
			py:     220,
		},
	}
}

func (player *Player) update(tp TargetPoint, accelLimit float64, speedLimit float64) {
	Px := tp.px - player.px
	Py := tp.py - player.py

	player.diffArrayX = append(player.diffArrayX, Px)
	player.diffArrayY = append(player.diffArrayY, Py)

	arrayLimit := 20
	if len(player.diffArrayX) > arrayLimit {
		player.diffArrayX = player.diffArrayX[1:]
		player.diffArrayY = player.diffArrayY[1:]
	}

	Ix := 0.0
	Iy := 0.0

	{
		start := len(player.diffArrayX) - 10
		if start < 0 {
			start = 0
		}

		for i := start; i < len(player.diffArrayX); i++ {
			Ix += player.diffArrayX[i]
			Iy += player.diffArrayY[i]
		}
	}

	Dx := 0.0
	Dy := 0.0

	{
		start := len(player.diffArrayX) - 10
		if start < 0 {
			start = 0
		}
		end := len(player.diffArrayX) - 1

		Dx = player.diffArrayX[end] - player.diffArrayX[start]
		Dy = player.diffArrayY[end] - player.diffArrayY[start]
	}

	accelX := player.kp*Px + player.ki*Ix + player.kd*Dx
	accelY := player.kp*Py + player.ki*Iy + player.kd*Dy

	accelVec := math.Sqrt(accelX*accelX + accelY*accelY)
	if accelVec > accelLimit {
		accelX *= accelLimit / accelVec
		accelY *= accelLimit / accelVec
	}

	player.vx *= 0.98
	player.vy *= 0.98

	player.vx += accelX
	player.vy += accelY

	speed := math.Sqrt(player.vx*player.vx + player.vy*player.vy)
	if speed > speedLimit {
		player.vx *= speedLimit / speed
		player.vy *= speedLimit / speed
	}

	player.px += player.vx
	player.py += player.vy
}

func update(screen *ebiten.Image) error {

	// kp
	if ebiten.IsKeyPressed(ebiten.KeyQ) {
		stage.player.kp += 0.001
	}

	if ebiten.IsKeyPressed(ebiten.KeyA) {
		stage.player.kp -= 0.001
	}

	if ebiten.IsKeyPressed(ebiten.KeyZ) {
		stage.player.kp = 0.0
	}

	// ki
	if ebiten.IsKeyPressed(ebiten.KeyW) {
		stage.player.ki += 0.001
	}

	if ebiten.IsKeyPressed(ebiten.KeyS) {
		stage.player.ki -= 0.001
	}

	if ebiten.IsKeyPressed(ebiten.KeyX) {
		stage.player.ki = 0.0
	}

	// kd
	if ebiten.IsKeyPressed(ebiten.KeyE) {
		stage.player.kd += 0.001
	}

	if ebiten.IsKeyPressed(ebiten.KeyD) {
		stage.player.kd -= 0.001
	}

	if ebiten.IsKeyPressed(ebiten.KeyC) {
		stage.player.kd = 0.0
	}

	nextTarget := stage.targetPointList[stage.currentTargetId]
	stage.player.update(nextTarget, stage.accelLimit, stage.speedLimit)

	stage.player.vx += stage.windX
	stage.player.vy += stage.windY

	// hit check
	{
		dx := stage.player.px - nextTarget.px
		dy := stage.player.py - nextTarget.py

		r := math.Sqrt(dx*dx + dy*dy)
		if r < nextTarget.radius {
			stage.currentTargetId += 1
			if stage.currentTargetId == len(stage.targetPointList) {
				stage.currentTargetId = 0
			}

		}

	}

	// draw check point
	for _, tp := range stage.targetPointList {
		ebitenutil.DrawRect(
			screen,
			tp.px, tp.py,
			32, 32,
			color.RGBA{0xff, 0xff, 0x00, 0xff})
	}

	// draw player
	ebitenutil.DrawRect(
		screen,
		stage.player.px, stage.player.py,
		32, 32,
		color.RGBA{0xff, 0xff, 0xff, 0xff})

	ebitenutil.DebugPrint(screen, fmt.Sprintf("FPS: %f", ebiten.CurrentFPS()))

	fmt.Print(stage.currentTargetId, stage.player.kp, stage.player.ki, stage.player.kd, "\n")
	fmt.Print(stage.player.diffArrayX, "\n")
	return nil
}

func main() {
	initStage()
	if err := ebiten.Run(update, screenWidth, screenHeight, 1, "PID game"); err != nil {
		log.Fatal(err)
	}
}