43 lines
1.4 KiB
C#
43 lines
1.4 KiB
C#
using UnityEngine;
|
|
|
|
namespace NanoBrain.Braitenberg {
|
|
using Unity;
|
|
|
|
public class Motor : MonoBehaviour {
|
|
public string outputNeuronName;
|
|
public float speed;
|
|
public float maxTorque = 1;
|
|
|
|
public WheelCollider wheelCollider;
|
|
|
|
protected Cluster brain;
|
|
public Neuron motorNeuron;
|
|
|
|
protected virtual void Awake() {
|
|
Vehicle vehicle = GetComponentInParent<Vehicle>();
|
|
if (vehicle != null)
|
|
brain = vehicle.brain;
|
|
if (brain != null)
|
|
motorNeuron = brain.GetNeuron(outputNeuronName);
|
|
wheelCollider = GetComponent<WheelCollider>();
|
|
}
|
|
|
|
void FixedUpdate() {
|
|
if (motorNeuron == null)
|
|
return;
|
|
|
|
this.speed = motorNeuron.outputValue.z;
|
|
//Debug.DrawRay(this.transform.position, this.transform.forward * this.speed, Color.magenta);
|
|
|
|
float desiredRpm = speed * 60; // target wheel RPM
|
|
float currentRpm = wheelCollider.rpm;
|
|
float rpmError = desiredRpm - currentRpm;
|
|
float kp = 0.02f; // proportional gain — tune
|
|
float torque = kp * rpmError;
|
|
wheelCollider.motorTorque = Mathf.Clamp(torque, -maxTorque, maxTorque);
|
|
if (float.IsNaN(wheelCollider.rpm))
|
|
Debug.Log($"{speed} {currentRpm / 60} {torque}");
|
|
}
|
|
}
|
|
|
|
} |