Firmware simulation, Unity project, and sim-verified robot fixes
Simulation - sim/: the unchanged robot firmware compiled into hexapod_sim.dll against PC stand-ins (Arduino, Servo, Wire + register-level MPU6050, RF24 with matching-config radio), a kinematic body model (support plane + planted-foot odometry) that also drives the simulated IMU, and a headless smoke test (sim_smoke). build.ps1 builds and tests. - Hexapod_Simulation_Unity/ folded in (history kept in a bundle outside the repo). Assets/HexapodSim loads the DLL, drives the existing leg rig, and offers joystick / keyboard / robot-serial input plus a test panel. Robot (all found or confirmed in the simulation) - Config.h geometry taken from the Unity model. - IK femur elevation sign, gait stance direction, body pose rotation direction, yaw lever arm. - Stance feet integrated from the current command; gait restarts at mid-stance; heading integrates the applied turn rate, only while walking. - Leg clearance and reach checks (Limits::feetFeasible); walk command and body pose scaled as a whole, pose scale rate-limited, planted feet guarded so the body never snaps or legs collide. - Robot-gyro self-leveling as an integrator. - Heading mode: steering relative to the body; S walks backward. - Standing height from the remote (STANCE_HEIGHT_RANGE_MM). Protocol v3 (reflash both boards) - CmdPacket gains body_height (22 bytes). Remote - Height slider (Nextion hHeight/nHeight, serial [ ]). - Fixed unspecified I2C read order in RemoteIMU. LEGACY/: owner's old Nextion HMI and remote hardware assets. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
mangavalkandClaude Opus 5.5 committed
1 parent
91dac16fca
commit
935526eab5
145 files changed
+431258
-114
No files matched your search
@@ -0,0 +1,8 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 4231f40d01b9f79419ea7f6642ab44fc
|
||||
folderAsset: yes
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,20 @@
|
||||
<!-- auto-generated. do not modify by hand. -->
|
||||
<dependencies>
|
||||
<androidPackages>
|
||||
<repositories>
|
||||
<repository>https://unity3ddist.jfrog.io/artifactory/unity-mediation-mvn-prod-local/</repository>
|
||||
</repositories>
|
||||
<!-- Mediation Android SDK -->
|
||||
<androidPackage spec="com.unity3d.mediation:mediation-sdk:[1.0,2.0[" mediation-identifier="mediation-sdk" mediation-version="1.0" />
|
||||
<!-- Mediation SDK Android Adapters -->
|
||||
<androidPackage spec="com.unity3d.mediation:unityads-adapter:[1.0,2.0[" mediation-identifier="unityads-adapter" mediation-version="latest" />
|
||||
</androidPackages>
|
||||
<iosPods>
|
||||
<!-- Mediation iOS SDK -->
|
||||
<iosPod name="UnityMediationSdk" version="~> 1.0" source="'https://github.com/Unity-Technologies/unity-mediation-cocoapods-prod.git'" mediation-identifier="mediation-sdk" mediation-version="1.0" />
|
||||
<!-- Mediation Dependency Required For resolution -->
|
||||
<iosPod name="Protobuf" />
|
||||
<!-- Mediation SDK iOS Adapters -->
|
||||
<iosPod name="UnityMediationUnityAdapter" version="" source="'https://github.com/Unity-Technologies/unity-mediation-cocoapods-prod.git'" mediation-identifier="unityads-adapter" mediation-version="latest" />
|
||||
</iosPods>
|
||||
</dependencies>
|
||||
+7
@@ -0,0 +1,7 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 9cfd2c2dc7249104daf45c4ba00d4a3e
|
||||
TextScriptImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,190 @@
|
||||
using System;
|
||||
using UnityEngine;
|
||||
|
||||
public class Foot : Component
|
||||
{
|
||||
double radians(double angle)
|
||||
{
|
||||
return (Math.PI / 180) * angle;
|
||||
}
|
||||
|
||||
double degrees(double radians)
|
||||
{
|
||||
double degrees = (180 / Math.PI) * radians;
|
||||
return (degrees);
|
||||
}
|
||||
|
||||
// Inverse Kinemetics Region
|
||||
double l3, s;
|
||||
|
||||
public double l1=10; // top leg length
|
||||
public double l2=15; // bottom leg length
|
||||
static float knie_offset = 5.5f; // distancce from horziontal servo to knee servo.
|
||||
|
||||
public double X, Y, Z;
|
||||
public double rotationX=0, rotationY=0, rotationZ=0;
|
||||
public double O1, O2, O3;
|
||||
|
||||
public Vector3 transformation = new Vector3(0, 0, 0);
|
||||
public Vector3 transformation2 = new Vector3(0, 0, 0);
|
||||
|
||||
void RL3()
|
||||
{
|
||||
if ((X == 0) && (Z == 0))
|
||||
l3 = 0;
|
||||
else
|
||||
l3 = Math.Sqrt(((X * X) + (Z * Z)));
|
||||
|
||||
if (X < 0)
|
||||
l3 = l3 * -1;
|
||||
|
||||
//todo
|
||||
l3 -= knie_offset;
|
||||
}
|
||||
|
||||
double FO1()
|
||||
{
|
||||
if ((X != 0) & (Z != 0))
|
||||
{
|
||||
double a = (Z / X);
|
||||
return degrees(Math.Atan((a)));
|
||||
}
|
||||
else
|
||||
return degrees(Math.Atan(0.0)); // Math Error Protection.
|
||||
}
|
||||
|
||||
double T1()
|
||||
{
|
||||
RL3();
|
||||
|
||||
double x2 = X * X;
|
||||
double y2 = Y * Y;
|
||||
double z2 = Z * Z;
|
||||
|
||||
double t = Math.Sqrt(x2 + z2);
|
||||
t -= knie_offset;
|
||||
t = t * t;
|
||||
//todo: X,Z remove distance temp.
|
||||
s = Math.Sqrt(y2 + t);
|
||||
//s = Math.Sqrt(x2 + y2 + z2);
|
||||
|
||||
double t1 = 0;
|
||||
double s2 = s * s;
|
||||
|
||||
if ((Y == 0) && (l3 == 0))
|
||||
t1 = 0; // Math Error Protection.
|
||||
else
|
||||
t1 = degrees(Math.Atan2(l3, Y));
|
||||
|
||||
double a = (l1 * l1) - (l2 * l2) + s2;
|
||||
double b = 2 * l1 * s;
|
||||
double t2;
|
||||
if (b == 0)
|
||||
t2 = -180;// todo: figure out normal value;
|
||||
else
|
||||
t2 = degrees(Math.Acos(a / b));
|
||||
|
||||
double ret = 0;
|
||||
|
||||
//ret = t1 + Math.Abs(t2);
|
||||
ret = t1 + t2;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
double T3()
|
||||
{
|
||||
double t = (l1 * l1) + (l2 * l2) - (s * s);
|
||||
double t2 = (2 * l1 * l2);
|
||||
double result = degrees(Math.Acos(t / t2));
|
||||
return result;
|
||||
}
|
||||
|
||||
void RotateX()
|
||||
{
|
||||
double nY = Y * Math.Cos(radians(rotationX)) - Z * Math.Sin(radians(rotationX));
|
||||
double nZ = Y * Math.Sin(radians(rotationX)) + Z * Math.Cos(radians(rotationX));
|
||||
Y = nY;
|
||||
Z = nZ;
|
||||
}
|
||||
|
||||
void RotateY()
|
||||
{
|
||||
double nZ = Z * Math.Cos(radians(rotationY)) - X * Math.Sin(radians(rotationY));
|
||||
double nX = Z * Math.Sin(radians(rotationY)) + X * Math.Cos(radians(rotationY));
|
||||
Z = nZ;
|
||||
X = nX;
|
||||
}
|
||||
|
||||
void RotateZ()
|
||||
{
|
||||
double nX = X * Math.Cos(radians(rotationZ)) - Y * Math.Sin(radians(rotationZ));
|
||||
double nY = X * Math.Sin(radians(rotationZ)) + Y * Math.Cos(radians(rotationZ));
|
||||
X = nX;
|
||||
Y = nY;
|
||||
}
|
||||
|
||||
public double translateX = 0;
|
||||
public double translateY = 0;
|
||||
public double translateZ = 0;
|
||||
|
||||
public bool invert = false;
|
||||
|
||||
public Vector3 pos = new Vector3(0, 0, 0);
|
||||
|
||||
public void setleg()
|
||||
{
|
||||
X += translateX;
|
||||
Y += translateY;
|
||||
Z += translateZ;
|
||||
|
||||
X -= transformation.x;
|
||||
Y -= transformation.y;
|
||||
Z -= transformation.z;
|
||||
|
||||
/*RotateX();
|
||||
RotateY();
|
||||
RotateZ();*/
|
||||
|
||||
// the order of these rotation transformations is important!!!
|
||||
Vector3 v = new Vector3((float)X, (float)Y, (float)Z);
|
||||
v = Quaternion.Euler((float)0, (float)rotationY, (float)0) * v;
|
||||
v = Quaternion.Euler((float)rotationX, (float)0, (float)0) * v;
|
||||
v = Quaternion.Euler((float)0, (float)0, (float)rotationZ) * v;
|
||||
X = v.x;
|
||||
Y = v.y;
|
||||
Z = v.z;
|
||||
|
||||
X -= transformation2.x;
|
||||
Y -= transformation2.y;
|
||||
Z -= transformation2.z;
|
||||
|
||||
X -= translateX;
|
||||
Y -= translateY;
|
||||
Z -= translateZ;
|
||||
|
||||
// translate to my stupid formula coordinates.
|
||||
double tx = X;
|
||||
double ty = Y;
|
||||
double tz = Z;
|
||||
X = tz * (invert ? -1 : 1);
|
||||
Y = ty * -1;
|
||||
Z = tx * -1;
|
||||
|
||||
// run IK
|
||||
// add adjustment from knie
|
||||
|
||||
O1 = (90 - FO1());
|
||||
// remove adjustment from knie
|
||||
|
||||
O2 = T1();
|
||||
O3 = 180 - T3();
|
||||
|
||||
// translate to my stupid formula coordinates.
|
||||
X = tx;
|
||||
Y = ty;
|
||||
Z = tz;
|
||||
|
||||
pos = new Vector3((float)X * (invert ? -1 : 1), (float)Y, (float)Z * (invert ? -1 : 1));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
fileFormatVersion: 2
|
||||
guid: c9255406468231049ab79ec93b48f97e
|
||||
timeCreated: 1541795571
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,8 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 82967c40c881be34d8d12f945dd28bac
|
||||
folderAsset: yes
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,18 @@
|
||||
using UnityEngine;
|
||||
|
||||
namespace HexapodSim
|
||||
{
|
||||
// Adds the firmware simulation to whatever scene is playing, so Main.unity
|
||||
// works without editing the scene. To configure it in the Inspector
|
||||
// instead, add a HexapodSimulation component to any GameObject yourself;
|
||||
// the bootstrap then does nothing.
|
||||
static class HexapodSimBootstrap
|
||||
{
|
||||
[RuntimeInitializeOnLoadMethod(RuntimeInitializeLoadType.AfterSceneLoad)]
|
||||
static void AddSimulationIfMissing()
|
||||
{
|
||||
if (Object.FindAnyObjectByType<HexapodSimulation>() != null) return;
|
||||
new GameObject("HexapodSimulation").AddComponent<HexapodSimulation>();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 0c650988c59fd78469143f99ff54f18d
|
||||
@@ -0,0 +1,232 @@
|
||||
using System;
|
||||
using System.IO;
|
||||
using System.Runtime.InteropServices;
|
||||
using UnityEngine;
|
||||
|
||||
namespace HexapodSim
|
||||
{
|
||||
// Wire format, byte-for-byte identical to shared/HexapodProtocol/HexapodProtocol.h.
|
||||
// Checked against the DLL's sizeof(CmdPacket) at load.
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 2)]
|
||||
public struct CmdPacket
|
||||
{
|
||||
public short vx, vy, yaw_rate;
|
||||
public short rx, ry, rz;
|
||||
public short remote_roll, remote_pitch;
|
||||
public short body_height; // -1000 lowest .. 0 default .. 1000 highest
|
||||
public byte speed_scale, flags, seq;
|
||||
|
||||
public const byte FLAG_ARMED = 1 << 0;
|
||||
public const byte FLAG_STAND = 1 << 1;
|
||||
public const byte FLAG_WALK = 1 << 2;
|
||||
public const byte FLAG_MODE_B = 1 << 3;
|
||||
public const byte FLAG_RSTICK_XLATE = 1 << 4;
|
||||
public const byte FLAG_GAIT_WAVE = 1 << 5;
|
||||
public const byte FLAG_REMOTE_GYRO = 1 << 6;
|
||||
public const byte FLAG_ROBOT_GYRO = 1 << 7;
|
||||
}
|
||||
|
||||
// Mirrors HxGeometry in sim/HexapodSimApi.h.
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct HxGeometry
|
||||
{
|
||||
public int numLegs;
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] public float[] mountXmm;
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] public float[] mountYmm;
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 6)] public float[] mountAngleDeg;
|
||||
public float coxaMm, femurMm, tibiaMm;
|
||||
public float stanceRadiusMm, stanceHeightMm;
|
||||
public float coxaMinDeg, coxaMaxDeg, femurMinDeg, femurMaxDeg, tibiaMinDeg, tibiaMaxDeg;
|
||||
public float controlHz;
|
||||
public int servoUsAt0Deg, servoUsPerDeg;
|
||||
public float legClearanceMm;
|
||||
public float stanceHeightRangeMm;
|
||||
}
|
||||
|
||||
// Mirrors HxStatus in sim/HexapodSimApi.h.
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct HxStatus
|
||||
{
|
||||
public int robotState; // 0 DISARMED, 1 SIT, 2 STAND, 3 WALKING
|
||||
public float sitStandBlend;
|
||||
public int linkOk;
|
||||
public float headingDeg;
|
||||
public float imuRollDeg, imuPitchDeg;
|
||||
public int imuEnabled;
|
||||
public uint simTimeMs;
|
||||
public uint controlTicks;
|
||||
public int stanceMask;
|
||||
public float poseReachScale; // 1 = full commanded pose; less = scaled to what the legs can reach
|
||||
public float twistScale; // 1 = walk command as sent; less = slowed to a stride the legs can take
|
||||
public float stanceHeightMm; // current standing height (feet below hips)
|
||||
|
||||
public string StateName => robotState switch
|
||||
{
|
||||
0 => "DISARMED", 1 => "SIT", 2 => "STAND", 3 => "WALKING", _ => "?"
|
||||
};
|
||||
}
|
||||
|
||||
// Loads sim/build/hexapod_sim.dll - the unchanged robot firmware compiled for
|
||||
// the PC. The DLL is copied to a temp file and loaded by hand (not DllImport)
|
||||
// so Unity never locks the build output: rebuild with CMake, press Play or
|
||||
// "Reload firmware" and the new code runs. Each load is a fresh power-on.
|
||||
public sealed class HexapodSimNative : IDisposable
|
||||
{
|
||||
public const int ExpectedApiVersion = 5; // HXSIM_API_VERSION
|
||||
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate int IntFn();
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void VoidFn();
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void IntArgFn(int v);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void UIntArgFn(uint v);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void GeometryFn(out HxGeometry g);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void StatusFn(out HxStatus s);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void PacketFn(ref CmdPacket p, int len);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void ImuFn(float ax, float ay, float az, float gx, float gy, float gz);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void BytesInFn(byte[] bytes, int len);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate int BytesOutFn(byte[] bytes, int cap);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void ServosFn(int[] us, int[] attached, uint[] writes);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void FloatsFn(float[] values);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void TwoFloatsFn(float a, float b);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void BodyFn(float[] pos3, float[] quatWxyz4, out float yawDeg, out int contactMask);
|
||||
|
||||
IntPtr handle_;
|
||||
string tempCopy_;
|
||||
|
||||
IntFn apiVersion_, packetSize_;
|
||||
GeometryFn getGeometry_;
|
||||
IntArgFn setRadioPresent_, setImuPresent_;
|
||||
VoidFn start_;
|
||||
UIntArgFn step_;
|
||||
PacketFn radioSend_;
|
||||
ImuFn setImu_;
|
||||
BytesInFn serialInput_;
|
||||
BytesOutFn serialOutput_;
|
||||
ServosFn getServos_;
|
||||
FloatsFn getJoints_, getFeet_;
|
||||
StatusFn getStatus_;
|
||||
TwoFloatsFn setGroundTilt_;
|
||||
BodyFn getBody_;
|
||||
|
||||
public string LoadedFrom { get; private set; }
|
||||
|
||||
public static HexapodSimNative Load(string dllPath)
|
||||
{
|
||||
if (!File.Exists(dllPath))
|
||||
throw new FileNotFoundException(
|
||||
"Simulation DLL not found. Build it first: run .\\sim\\build.ps1 from the repo root in PowerShell.",
|
||||
dllPath);
|
||||
|
||||
var n = new HexapodSimNative();
|
||||
n.tempCopy_ = Path.Combine(Path.GetTempPath(), $"hexapod_sim_{Guid.NewGuid():N}.dll");
|
||||
File.Copy(dllPath, n.tempCopy_, true);
|
||||
n.handle_ = LoadLibraryW(n.tempCopy_);
|
||||
if (n.handle_ == IntPtr.Zero)
|
||||
{
|
||||
int err = Marshal.GetLastWin32Error();
|
||||
n.Dispose();
|
||||
throw new Exception($"LoadLibrary failed for {dllPath} (Win32 error {err}). Is it a 64-bit build?");
|
||||
}
|
||||
n.LoadedFrom = dllPath;
|
||||
|
||||
n.apiVersion_ = n.Fn<IntFn>("hxsim_api_version");
|
||||
n.packetSize_ = n.Fn<IntFn>("hxsim_packet_size");
|
||||
n.getGeometry_ = n.Fn<GeometryFn>("hxsim_get_geometry");
|
||||
n.setRadioPresent_ = n.Fn<IntArgFn>("hxsim_set_radio_present");
|
||||
n.setImuPresent_ = n.Fn<IntArgFn>("hxsim_set_imu_present");
|
||||
n.start_ = n.Fn<VoidFn>("hxsim_start");
|
||||
n.step_ = n.Fn<UIntArgFn>("hxsim_step");
|
||||
n.radioSend_ = n.Fn<PacketFn>("hxsim_radio_send");
|
||||
n.setImu_ = n.Fn<ImuFn>("hxsim_set_imu");
|
||||
n.serialInput_ = n.Fn<BytesInFn>("hxsim_serial_input");
|
||||
n.serialOutput_ = n.Fn<BytesOutFn>("hxsim_serial_output");
|
||||
n.getServos_ = n.Fn<ServosFn>("hxsim_get_servos");
|
||||
n.getJoints_ = n.Fn<FloatsFn>("hxsim_get_joints_deg");
|
||||
n.getFeet_ = n.Fn<FloatsFn>("hxsim_get_feet_mm");
|
||||
n.getStatus_ = n.Fn<StatusFn>("hxsim_get_status");
|
||||
n.setGroundTilt_ = n.Fn<TwoFloatsFn>("hxsim_set_ground_tilt");
|
||||
n.getBody_ = n.Fn<BodyFn>("hxsim_get_body");
|
||||
|
||||
int version = n.apiVersion_();
|
||||
if (version != ExpectedApiVersion)
|
||||
{
|
||||
n.Dispose();
|
||||
throw new Exception($"hexapod_sim.dll API version {version}, Unity scripts expect {ExpectedApiVersion}. Rebuild the DLL or update the scripts.");
|
||||
}
|
||||
int size = n.packetSize_();
|
||||
if (size != Marshal.SizeOf<CmdPacket>())
|
||||
{
|
||||
n.Dispose();
|
||||
throw new Exception($"CmdPacket is {size} bytes in the firmware but {Marshal.SizeOf<CmdPacket>()} in C#. Update HexapodSimNative.CmdPacket to match HexapodProtocol.h.");
|
||||
}
|
||||
return n;
|
||||
}
|
||||
|
||||
T Fn<T>(string name) where T : Delegate
|
||||
{
|
||||
IntPtr p = GetProcAddress(handle_, name);
|
||||
if (p == IntPtr.Zero) throw new Exception($"hexapod_sim.dll is missing export '{name}' - rebuild it.");
|
||||
return Marshal.GetDelegateForFunctionPointer<T>(p);
|
||||
}
|
||||
|
||||
public int PacketSize => packetSize_();
|
||||
public HxGeometry Geometry { get { getGeometry_(out var g); return g; } }
|
||||
public HxStatus Status { get { getStatus_(out var s); return s; } }
|
||||
|
||||
public void SetRadioPresent(bool present) => setRadioPresent_(present ? 1 : 0);
|
||||
public void SetImuPresent(bool present) => setImuPresent_(present ? 1 : 0);
|
||||
public void Start() => start_();
|
||||
public void Step(uint dtMicros) => step_(dtMicros);
|
||||
public void RadioSend(CmdPacket p) => radioSend_(ref p, Marshal.SizeOf<CmdPacket>());
|
||||
// Overrides the body-driven IMU with fixed readings from then on (normally unused).
|
||||
public void SetImu(Vector3 accelG, Vector3 gyroDps) =>
|
||||
setImu_(accelG.x, accelG.y, accelG.z, gyroDps.x, gyroDps.y, gyroDps.z);
|
||||
|
||||
public void SerialInput(string text)
|
||||
{
|
||||
if (string.IsNullOrEmpty(text)) return;
|
||||
byte[] b = System.Text.Encoding.ASCII.GetBytes(text);
|
||||
serialInput_(b, b.Length);
|
||||
}
|
||||
|
||||
readonly byte[] serialBuf_ = new byte[4096];
|
||||
public string SerialOutput()
|
||||
{
|
||||
int n = serialOutput_(serialBuf_, serialBuf_.Length);
|
||||
return n > 0 ? System.Text.Encoding.ASCII.GetString(serialBuf_, 0, n) : "";
|
||||
}
|
||||
|
||||
public void GetServos(int[] us, int[] attached, uint[] writes) => getServos_(us, attached, writes);
|
||||
public void GetJointsDeg(float[] deg18) => getJoints_(deg18);
|
||||
public void GetFeetMm(float[] xyz18) => getFeet_(xyz18);
|
||||
|
||||
public void SetGroundTilt(float rollDeg, float pitchDeg) => setGroundTilt_(rollDeg, pitchDeg);
|
||||
|
||||
readonly float[] bodyPos_ = new float[3], bodyQuat_ = new float[4];
|
||||
// Kinematic body from the DLL, robot-world frame. The rotation is a
|
||||
// robot-frame quaternion held in a Unity Quaternion for its math only.
|
||||
public void GetBody(out Vector3 posMm, out Quaternion rotRobot, out float yawDeg, out int contactMask)
|
||||
{
|
||||
getBody_(bodyPos_, bodyQuat_, out yawDeg, out contactMask);
|
||||
posMm = new Vector3(bodyPos_[0], bodyPos_[1], bodyPos_[2]);
|
||||
rotRobot = new Quaternion(bodyQuat_[1], bodyQuat_[2], bodyQuat_[3], bodyQuat_[0]);
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (handle_ != IntPtr.Zero) { FreeLibrary(handle_); handle_ = IntPtr.Zero; }
|
||||
if (tempCopy_ != null)
|
||||
{
|
||||
try { File.Delete(tempCopy_); } catch { /* still mapped on some systems; temp dir cleans up */ }
|
||||
tempCopy_ = null;
|
||||
}
|
||||
}
|
||||
|
||||
[DllImport("kernel32", SetLastError = true, CharSet = CharSet.Unicode)]
|
||||
static extern IntPtr LoadLibraryW(string path);
|
||||
[DllImport("kernel32", SetLastError = true)]
|
||||
[return: MarshalAs(UnmanagedType.Bool)]
|
||||
static extern bool FreeLibrary(IntPtr handle);
|
||||
[DllImport("kernel32", SetLastError = true, CharSet = CharSet.Ansi, ExactSpelling = true)]
|
||||
static extern IntPtr GetProcAddress(IntPtr handle, string name);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 5fd045740d26ccf4f98e1153df25add4
|
||||
@@ -0,0 +1,326 @@
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
using System.Text;
|
||||
using UnityEngine;
|
||||
|
||||
namespace HexapodSim
|
||||
{
|
||||
// Runs the real robot firmware (robot/, compiled into sim/build/hexapod_sim.dll)
|
||||
// against the existing Unity model:
|
||||
// input (SimRemote) -> CmdPacket over the simulated radio -> firmware loop()
|
||||
// -> 18 servo pulses -> joint rotations on the rig.
|
||||
// The body's motion (kinematic: it rests on its feet, grounded feet don't
|
||||
// slide) and the simulated MPU6050 readings are computed inside the DLL
|
||||
// (sim/SimBody.cpp), so the headless smoke test checks the same thing this
|
||||
// shows. Nothing here decides how the robot moves.
|
||||
[DefaultExecutionOrder(-100)]
|
||||
public class HexapodSimulation : MonoBehaviour
|
||||
{
|
||||
[Header("Firmware")]
|
||||
[Tooltip("Empty = <repo>/sim/build/hexapod_sim.dll")]
|
||||
public string dllPath = "";
|
||||
public bool radioPresent = true;
|
||||
public bool imuPresent = true;
|
||||
[Tooltip("Remote transmit rate. The real remote sends at 100 Hz.")]
|
||||
public float remoteTxHz = 100f;
|
||||
|
||||
[Header("Rig (auto-detected from the scene if empty)")]
|
||||
public Transform chassis;
|
||||
public Transform ground;
|
||||
public ForwardAxis forwardAxis = ForwardAxis.NegativeX;
|
||||
[Tooltip("Unity units per millimetre. The model is built in cm.")]
|
||||
public float unitsPerMm = 0.1f;
|
||||
[Tooltip("Firmware tibia angle when the rig's Knie rotation is 0 (tibia straight down from a horizontal femur).")]
|
||||
public float kneeRestDeg = -90f;
|
||||
|
||||
[Header("World")]
|
||||
[Tooltip("Tilts the ground (deg about robot X = roll, Y = pitch) to exercise the robot gyro.")]
|
||||
public Vector2 groundTiltDeg;
|
||||
public bool followCamera = true;
|
||||
|
||||
// ---- Runtime state, read by SimTestPanel ----
|
||||
public HexapodSimNative Native { get; private set; }
|
||||
public HxGeometry Geometry { get; private set; }
|
||||
public HxStatus Status { get; private set; }
|
||||
public string LoadError { get; private set; }
|
||||
public readonly List<string> SerialLog = new List<string>();
|
||||
public readonly List<string> RigWarnings = new List<string>();
|
||||
public float[] JointsDeg { get; } = new float[18];
|
||||
public float[] FeetMm { get; } = new float[18];
|
||||
public bool[] Contact { get; } = new bool[6];
|
||||
public int[] ServoUs { get; } = new int[18];
|
||||
public int[] ServoAttached { get; } = new int[18];
|
||||
public uint[] ServoWrites { get; } = new uint[18];
|
||||
public Vector3 BodyPosMm => bodyPos_;
|
||||
public float BodyYawDeg => yaw_;
|
||||
|
||||
SimRemote remote_;
|
||||
|
||||
class LegRig
|
||||
{
|
||||
public Transform root, hip1, hip2, knie, footMarker;
|
||||
public float coxaOffsetDeg;
|
||||
}
|
||||
readonly LegRig[] legs_ = new LegRig[6];
|
||||
|
||||
// Body pose from the DLL, robot-world frame (mm, Z up, ground z = 0 before tilt).
|
||||
float yaw_;
|
||||
Vector3 bodyPos_;
|
||||
Quaternion bodyRot_ = Quaternion.identity; // robot-frame rotation used as a math object
|
||||
|
||||
// Unity-side anchors captured at start.
|
||||
Vector3 originUnity_;
|
||||
Quaternion chassisRestRot_;
|
||||
Vector3 groundRestPos_;
|
||||
Quaternion groundRestRot_;
|
||||
Transform camera_;
|
||||
Vector3 cameraOffset_;
|
||||
StringBuilder serialLine_ = new StringBuilder();
|
||||
|
||||
static readonly string[] LegacyMovementScripts =
|
||||
{ "Walk", "RotationControl", "Leg", "HandleHoystick", "Serial2", "Serial3" };
|
||||
|
||||
// Robot-frame axes, for readability (Unity's Vector3.up etc. are Unity axes).
|
||||
static readonly Vector3 AxisX = new Vector3(1, 0, 0); // forward
|
||||
static readonly Vector3 AxisY = new Vector3(0, 1, 0); // left
|
||||
|
||||
void Awake()
|
||||
{
|
||||
DisableLegacyScripts();
|
||||
remote_ = GetComponent<SimRemote>();
|
||||
if (remote_ == null) remote_ = gameObject.AddComponent<SimRemote>();
|
||||
if (GetComponent<SimTestPanel>() == null) gameObject.AddComponent<SimTestPanel>();
|
||||
|
||||
if (chassis == null) chassis = FindChassis();
|
||||
if (ground == null) { var g = GameObject.Find("Plane"); if (g) ground = g.transform; }
|
||||
if (chassis == null) { LoadError = "No leg rig found (expected 6 objects named 'Leg' with hip1/Hip2/Knie)."; return; }
|
||||
|
||||
chassisRestRot_ = chassis.rotation;
|
||||
float groundY = ground ? ground.position.y : chassis.position.y - 150f * unitsPerMm;
|
||||
originUnity_ = new Vector3(chassis.position.x, groundY, chassis.position.z);
|
||||
if (ground) { groundRestPos_ = ground.position; groundRestRot_ = ground.rotation; }
|
||||
if (Camera.main) { camera_ = Camera.main.transform; cameraOffset_ = camera_.position - chassis.position; }
|
||||
|
||||
ReloadFirmware();
|
||||
}
|
||||
|
||||
void OnDestroy() => Native?.Dispose();
|
||||
|
||||
public string ResolvedDllPath =>
|
||||
!string.IsNullOrEmpty(dllPath)
|
||||
? dllPath
|
||||
: Path.GetFullPath(Path.Combine(Application.dataPath, "..", "..", "sim", "build", "hexapod_sim.dll"));
|
||||
|
||||
// Fresh power-on: loads the current DLL build and resets the body.
|
||||
public void ReloadFirmware()
|
||||
{
|
||||
Native?.Dispose();
|
||||
Native = null;
|
||||
LoadError = null;
|
||||
try
|
||||
{
|
||||
Native = HexapodSimNative.Load(ResolvedDllPath);
|
||||
Native.SetRadioPresent(radioPresent);
|
||||
Native.SetImuPresent(imuPresent);
|
||||
Geometry = Native.Geometry;
|
||||
BindRig();
|
||||
Native.SetGroundTilt(groundTiltDeg.x, groundTiltDeg.y);
|
||||
Native.Start();
|
||||
remote_.ResetState();
|
||||
SerialLog.Add($"--- firmware loaded: {ResolvedDllPath}");
|
||||
}
|
||||
catch (System.Exception e)
|
||||
{
|
||||
LoadError = e.Message;
|
||||
Debug.LogError("[HexapodSim] " + e.Message);
|
||||
Native?.Dispose();
|
||||
Native = null;
|
||||
}
|
||||
}
|
||||
|
||||
void FixedUpdate()
|
||||
{
|
||||
if (Native == null) return;
|
||||
float dt = Time.fixedDeltaTime;
|
||||
|
||||
Native.SetGroundTilt(groundTiltDeg.x, groundTiltDeg.y);
|
||||
|
||||
// Remote packets at the remote's rate, firmware time advanced in step.
|
||||
int sub = Mathf.Max(1, Mathf.RoundToInt(dt * remoteTxHz));
|
||||
float subDt = dt / sub;
|
||||
for (int n = 0; n < sub; n++)
|
||||
{
|
||||
remote_.Transmit(Native, subDt);
|
||||
Native.Step((uint)Mathf.RoundToInt(subDt * 1e6f));
|
||||
}
|
||||
|
||||
Status = Native.Status;
|
||||
Native.GetJointsDeg(JointsDeg);
|
||||
Native.GetFeetMm(FeetMm);
|
||||
Native.GetServos(ServoUs, ServoAttached, ServoWrites);
|
||||
Native.GetBody(out bodyPos_, out bodyRot_, out yaw_, out int contactMask);
|
||||
for (int i = 0; i < 6; i++) Contact[i] = (contactMask >> i & 1) != 0;
|
||||
DrainSerial();
|
||||
|
||||
ApplyBodyPose();
|
||||
ApplyJoints();
|
||||
}
|
||||
|
||||
void LateUpdate()
|
||||
{
|
||||
if (followCamera && camera_ && chassis) camera_.position = chassis.position + cameraOffset_;
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------
|
||||
// Rig
|
||||
// ------------------------------------------------------------------
|
||||
|
||||
void DisableLegacyScripts()
|
||||
{
|
||||
foreach (var mb in FindObjectsByType<MonoBehaviour>(FindObjectsInactive.Include))
|
||||
if (System.Array.IndexOf(LegacyMovementScripts, mb.GetType().Name) >= 0)
|
||||
mb.enabled = false;
|
||||
}
|
||||
|
||||
static List<Transform> FindLegRoots()
|
||||
{
|
||||
var result = new List<Transform>();
|
||||
foreach (var t in FindObjectsByType<Transform>())
|
||||
if (t.name == "Leg" && FindDeep(t, "hip1") != null) result.Add(t);
|
||||
return result;
|
||||
}
|
||||
|
||||
static Transform FindChassis()
|
||||
{
|
||||
var roots = FindLegRoots();
|
||||
return roots.Count > 0 ? roots[0].parent : null;
|
||||
}
|
||||
|
||||
static Transform FindDeep(Transform parent, string name)
|
||||
{
|
||||
foreach (Transform c in parent)
|
||||
{
|
||||
if (string.Equals(c.name, name, System.StringComparison.OrdinalIgnoreCase)) return c;
|
||||
var d = FindDeep(c, name);
|
||||
if (d) return d;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
// Matches each firmware leg (Config.h order) to the rig leg at its mount
|
||||
// position, and checks the rig against Config.h so a mismatch is loud.
|
||||
void BindRig()
|
||||
{
|
||||
RigWarnings.Clear();
|
||||
var roots = FindLegRoots();
|
||||
var g = Geometry;
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
Vector3 mountLocal = RobotFrame.ToUnity(new Vector3(g.mountXmm[i], g.mountYmm[i], 0), forwardAxis) * unitsPerMm;
|
||||
Transform best = null;
|
||||
float bestDist = float.MaxValue;
|
||||
foreach (var r in roots)
|
||||
{
|
||||
float dist = Vector3.Distance(chassis.InverseTransformPoint(r.position), mountLocal);
|
||||
if (dist < bestDist) { bestDist = dist; best = r; }
|
||||
}
|
||||
if (best == null) { legs_[i] = null; continue; }
|
||||
roots.Remove(best);
|
||||
|
||||
var leg = new LegRig
|
||||
{
|
||||
root = best,
|
||||
hip1 = FindDeep(best, "hip1"),
|
||||
hip2 = FindDeep(best, "Hip2"),
|
||||
knie = FindDeep(best, "Knie"),
|
||||
footMarker = FindDeep(best, "Point"),
|
||||
};
|
||||
if (bestDist / unitsPerMm > 5f)
|
||||
RigWarnings.Add($"leg {i}: rig hip is {bestDist / unitsPerMm:F0} mm from Config.h mount ({g.mountXmm[i]}, {g.mountYmm[i]})");
|
||||
|
||||
// Direction the rig's coxa points at rotation 0, in robot-frame degrees.
|
||||
Vector3 outward = RobotFrame.ToRobot(chassis.InverseTransformDirection(best.forward), forwardAxis);
|
||||
float rigMountDeg = Mathf.Atan2(outward.y, outward.x) * Mathf.Rad2Deg;
|
||||
leg.coxaOffsetDeg = RobotFrame.WrapDeg(g.mountAngleDeg[i] - rigMountDeg);
|
||||
if (Mathf.Abs(leg.coxaOffsetDeg) > 1f)
|
||||
RigWarnings.Add($"leg {i}: Config mount angle {g.mountAngleDeg[i]:F0} vs rig {rigMountDeg:F0} deg (compensated)");
|
||||
|
||||
if (leg.hip1 && leg.hip2 && leg.knie)
|
||||
{
|
||||
float rigCoxa = Vector3.Distance(leg.hip1.position, leg.hip2.position) / unitsPerMm;
|
||||
float rigFemur = Vector3.Distance(leg.hip2.position, leg.knie.position) / unitsPerMm;
|
||||
if (Mathf.Abs(rigCoxa - g.coxaMm) > 2f || Mathf.Abs(rigFemur - g.femurMm) > 2f)
|
||||
RigWarnings.Add($"leg {i}: rig coxa/femur {rigCoxa:F0}/{rigFemur:F0} mm vs Config {g.coxaMm:F0}/{g.femurMm:F0} mm");
|
||||
}
|
||||
else RigWarnings.Add($"leg {i}: missing hip1/Hip2/Knie under '{best.name}'");
|
||||
legs_[i] = leg;
|
||||
}
|
||||
foreach (var w in RigWarnings) Debug.LogWarning("[HexapodSim] " + w);
|
||||
}
|
||||
|
||||
// Firmware joint convention -> rig rotations (see sim/HexapodSimApi.cpp FK):
|
||||
// coxa + = CCW from above; Unity +Y rotation is clockwise from above.
|
||||
// femur/tibia + = up; Unity +X rotation tips the leg's +Z down.
|
||||
void ApplyJoints()
|
||||
{
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
var leg = legs_[i];
|
||||
if (leg == null || leg.hip1 == null) continue;
|
||||
float coxa = JointsDeg[i * 3], femur = JointsDeg[i * 3 + 1], tibia = JointsDeg[i * 3 + 2];
|
||||
leg.hip1.localRotation = Quaternion.Euler(0, -(coxa + leg.coxaOffsetDeg), 0);
|
||||
if (leg.hip2) leg.hip2.localRotation = Quaternion.Euler(-femur, 0, 0);
|
||||
if (leg.knie) leg.knie.localRotation = Quaternion.Euler(-(tibia - kneeRestDeg), 0, 0);
|
||||
|
||||
// The sphere marks where the firmware-side FK puts the foot. If it
|
||||
// doesn't sit on the tibia tip, the rig and Config.h disagree.
|
||||
if (leg.footMarker)
|
||||
leg.footMarker.position = chassis.TransformPoint(FootLocal(i));
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 FootLocal(int i) =>
|
||||
RobotFrame.ToUnity(new Vector3(FeetMm[i * 3], FeetMm[i * 3 + 1], FeetMm[i * 3 + 2]), forwardAxis) * unitsPerMm;
|
||||
|
||||
// ------------------------------------------------------------------
|
||||
// World
|
||||
// ------------------------------------------------------------------
|
||||
|
||||
// Robot frame: X forward, Y left (Unity's Vector3.right/up are Unity axes).
|
||||
Quaternion GroundRotation() =>
|
||||
Quaternion.AngleAxis(groundTiltDeg.x, AxisX) * Quaternion.AngleAxis(groundTiltDeg.y, AxisY);
|
||||
|
||||
// The robot-world frame is the chassis's starting orientation, so the
|
||||
// rig's local axes (which the Config.h mapping is defined against) and
|
||||
// the world stay consistent even if the chassis starts rotated.
|
||||
void ApplyBodyPose()
|
||||
{
|
||||
if (!chassis) return;
|
||||
chassis.position = originUnity_ + chassisRestRot_ * RobotFrame.ToUnity(bodyPos_, forwardAxis) * unitsPerMm;
|
||||
chassis.rotation = chassisRestRot_ * RobotFrame.ToUnity(bodyRot_, forwardAxis);
|
||||
if (ground)
|
||||
{
|
||||
Quaternion g = chassisRestRot_ * RobotFrame.ToUnity(GroundRotation(), forwardAxis) * Quaternion.Inverse(chassisRestRot_);
|
||||
ground.rotation = g * groundRestRot_;
|
||||
ground.position = originUnity_ + g * (groundRestPos_ - originUnity_);
|
||||
}
|
||||
}
|
||||
|
||||
void DrainSerial()
|
||||
{
|
||||
string s = Native.SerialOutput();
|
||||
foreach (char ch in s)
|
||||
{
|
||||
if (ch == '\n')
|
||||
{
|
||||
string line = serialLine_.ToString().TrimEnd('\r');
|
||||
serialLine_.Clear();
|
||||
SerialLog.Add(line);
|
||||
Debug.Log("[robot] " + line);
|
||||
}
|
||||
else serialLine_.Append(ch);
|
||||
}
|
||||
if (SerialLog.Count > 200) SerialLog.RemoveRange(0, SerialLog.Count - 200);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 5e27b78fcb1d2624d9e437ac5c6f8853
|
||||
@@ -0,0 +1,38 @@
|
||||
using UnityEngine;
|
||||
|
||||
namespace HexapodSim
|
||||
{
|
||||
// Which Unity axis the robot's forward (+X) maps to. The old scene marks
|
||||
// the legs on Unity -Z as "invertForLeftLeg", which makes -X forward.
|
||||
public enum ForwardAxis { NegativeX, PositiveX }
|
||||
|
||||
// Conversions between the firmware's body frame (right-handed, mm:
|
||||
// +X forward, +Y left, +Z up) and Unity (left-handed, Y up). Everything
|
||||
// physical in the simulation is computed in the robot frame and only
|
||||
// converted here, in one place.
|
||||
public static class RobotFrame
|
||||
{
|
||||
public static Vector3 ToUnity(Vector3 r, ForwardAxis f) =>
|
||||
f == ForwardAxis.NegativeX ? new Vector3(-r.x, r.z, -r.y) : new Vector3(r.x, r.z, r.y);
|
||||
|
||||
public static Vector3 ToRobot(Vector3 u, ForwardAxis f) =>
|
||||
f == ForwardAxis.NegativeX ? new Vector3(-u.x, -u.z, u.y) : new Vector3(u.x, u.z, u.y);
|
||||
|
||||
// A rotation expressed in the robot frame, as the equivalent Unity rotation.
|
||||
// (The mapping is a reflection, so this is M * R * M^-1, built from two axes.)
|
||||
public static Quaternion ToUnity(Quaternion robotRot, ForwardAxis f)
|
||||
{
|
||||
Vector3 fwd = ToUnity(robotRot * ToRobot(Vector3.forward, f), f);
|
||||
Vector3 up = ToUnity(robotRot * ToRobot(Vector3.up, f), f);
|
||||
return Quaternion.LookRotation(fwd, up);
|
||||
}
|
||||
|
||||
public static float WrapDeg(float d)
|
||||
{
|
||||
d %= 360f;
|
||||
if (d > 180f) d -= 360f;
|
||||
if (d < -180f) d += 360f;
|
||||
return d;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 1ce33c911afdf8f4a90bcdfe72c26093
|
||||
@@ -0,0 +1,255 @@
|
||||
using System;
|
||||
using UnityEngine;
|
||||
|
||||
namespace HexapodSim
|
||||
{
|
||||
// Stand-in for the handheld remote. Produces CmdPackets exactly per the
|
||||
// protocol contract and sends them over the simulated radio. Toggles and
|
||||
// interlocks mirror remote/RemoteState.cpp and remote/PacketBuilder.cpp -
|
||||
// keep them in sync if those change.
|
||||
//
|
||||
// Input sources:
|
||||
// Joystick - the real remote as a USB game controller, read through
|
||||
// Unity input axes "1".."6" like the old HandleHoystick.
|
||||
// Keyboard - test without any remote (keys listed in SimTestPanel).
|
||||
// RobotSerialHarness - no radio at all; typed keys go to the firmware's own
|
||||
// TEST_SERIAL_INPUT harness, like the Arduino Serial Monitor.
|
||||
// Toggles, the speed slider and the handset tilt sliders live in SimTestPanel.
|
||||
public class SimRemote : MonoBehaviour
|
||||
{
|
||||
public enum InputSource { Joystick, Keyboard, RobotSerialHarness }
|
||||
|
||||
[Serializable]
|
||||
public class AxisBinding
|
||||
{
|
||||
public string axis;
|
||||
public bool invert;
|
||||
public AxisBinding(string axis, bool invert) { this.axis = axis; this.invert = invert; }
|
||||
}
|
||||
|
||||
public InputSource source = InputSource.Keyboard;
|
||||
|
||||
// Defaults follow the old HandleHoystick: axes 4/6/5 were the walk stick
|
||||
// (b2/b1/b3), 2/1/3 the rotation stick (a1/a2/a3), with 1 and 2 inverted.
|
||||
// Which of 4/6 is forward vs sideways wasn't recoverable from the old code:
|
||||
// check the raw axis readout in the test panel and fix here.
|
||||
[Header("Joystick axes (Input Manager names)")]
|
||||
[Tooltip("Stick convention after invert: X + = right, Y + = forward, twist + = counterclockwise.")]
|
||||
public AxisBinding leftX = new AxisBinding("4", false);
|
||||
public AxisBinding leftY = new AxisBinding("6", false);
|
||||
public AxisBinding leftTwist = new AxisBinding("5", false);
|
||||
public AxisBinding rightX = new AxisBinding("2", true);
|
||||
public AxisBinding rightY = new AxisBinding("1", true);
|
||||
public AxisBinding rightTwist = new AxisBinding("3", false);
|
||||
[Range(0, 0.5f)] public float deadzone = 0.08f;
|
||||
|
||||
[Header("Operator state (same toggles as the Nextion screen)")]
|
||||
public bool armed;
|
||||
public bool stand;
|
||||
public bool gaitWave;
|
||||
public bool modeB;
|
||||
public bool rstickXlate;
|
||||
public bool remoteGyro;
|
||||
public bool robotGyro;
|
||||
[Range(0, 255)] public int speedTarget = 128;
|
||||
[Tooltip("Standing height setting, -1000 lowest .. 0 default .. 1000 highest (CmdPacket.body_height).")]
|
||||
[Range(-1000, 1000)] public int height = 0;
|
||||
|
||||
[Header("Simulated handset tilt (the remote's MPU6050)")]
|
||||
[Range(-30, 30)] public float handsetRollDeg;
|
||||
[Range(-30, 30)] public float handsetPitchDeg;
|
||||
public float handsetFullScaleDeg = 30f; // remote Config.h REMOTE_TILT_FULL_SCALE_DEG
|
||||
|
||||
[Header("Smoothing, as on the remote")]
|
||||
public float speedRatePerSec = 255f; // SPEED_SCALE_RATE_PER_SEC
|
||||
public float walkLingerSec = 0.25f; // WALK_LINGER_MS
|
||||
public float armCenterTolerance = 0.05f; // ARM_CENTER_TOLERANCE / 1000
|
||||
public float keyStickRatePerSec = 4f; // how fast a held key moves the virtual stick
|
||||
|
||||
// Latest processed sticks, -1..1: [L_X, L_Y, L_TWIST, R_X, R_Y, R_TWIST].
|
||||
public readonly float[] sticks = new float[6];
|
||||
public readonly float[] rawAxes = new float[6]; // Input axes "1".."6" as read
|
||||
public CmdPacket LastPacket { get; private set; }
|
||||
public string Message { get; private set; } = "";
|
||||
public bool JoystickAxesMissing { get; private set; }
|
||||
|
||||
float speedCurrent_ = 128;
|
||||
float lastWalkInputTime_ = -999f;
|
||||
byte seq_;
|
||||
string serialKeys_ = "";
|
||||
float messageUntil_;
|
||||
|
||||
public void ResetState()
|
||||
{
|
||||
armed = stand = false;
|
||||
speedCurrent_ = speedTarget;
|
||||
lastWalkInputTime_ = -999f;
|
||||
serialKeys_ = "";
|
||||
}
|
||||
|
||||
void Awake()
|
||||
{
|
||||
if (Input.GetJoystickNames().Length > 0 && !string.IsNullOrEmpty(Input.GetJoystickNames()[0]))
|
||||
source = InputSource.Joystick;
|
||||
}
|
||||
|
||||
void Update()
|
||||
{
|
||||
for (int i = 0; i < 6; i++) rawAxes[i] = ReadAxis((i + 1).ToString());
|
||||
|
||||
if (source == InputSource.RobotSerialHarness)
|
||||
{
|
||||
serialKeys_ += Input.inputString; // raw keystrokes for the firmware's harness
|
||||
return;
|
||||
}
|
||||
|
||||
HandleToggleKeys();
|
||||
if (source == InputSource.Joystick) ReadJoystick();
|
||||
else ReadKeyboardSticks(Time.deltaTime);
|
||||
if (Time.time > messageUntil_) Message = "";
|
||||
}
|
||||
|
||||
// ---- Operator commands (also called by SimTestPanel buttons) ----
|
||||
|
||||
public void ToggleArm()
|
||||
{
|
||||
if (armed) { Disarm(); return; }
|
||||
foreach (float v in sticks)
|
||||
if (Mathf.Abs(v) > armCenterTolerance) { Say("Center both sticks to arm"); return; }
|
||||
armed = true;
|
||||
}
|
||||
public void Disarm() { armed = false; stand = false; } // disarm always clears stand
|
||||
public void ToggleStand()
|
||||
{
|
||||
if (!armed) { Say("Arm first"); return; }
|
||||
stand = !stand;
|
||||
}
|
||||
public void ToggleRemoteGyro() => remoteGyro = !remoteGyro;
|
||||
|
||||
void Say(string msg) { Message = msg; messageUntil_ = Time.time + 2.5f; }
|
||||
|
||||
void HandleToggleKeys()
|
||||
{
|
||||
if (Input.GetKeyDown(KeyCode.Alpha1)) ToggleArm();
|
||||
if (Input.GetKeyDown(KeyCode.Alpha2)) ToggleStand();
|
||||
if (Input.GetKeyDown(KeyCode.Alpha3)) gaitWave = !gaitWave;
|
||||
if (Input.GetKeyDown(KeyCode.Alpha4)) modeB = !modeB;
|
||||
if (Input.GetKeyDown(KeyCode.Alpha5)) rstickXlate = !rstickXlate;
|
||||
if (Input.GetKeyDown(KeyCode.Alpha6)) ToggleRemoteGyro();
|
||||
if (Input.GetKeyDown(KeyCode.Alpha7)) robotGyro = !robotGyro;
|
||||
if (Input.GetKeyDown(KeyCode.Backspace)) Disarm();
|
||||
if (Input.GetKeyDown(KeyCode.Equals) || Input.GetKeyDown(KeyCode.KeypadPlus)) speedTarget = Mathf.Min(255, speedTarget + 25);
|
||||
if (Input.GetKeyDown(KeyCode.Minus) || Input.GetKeyDown(KeyCode.KeypadMinus)) speedTarget = Mathf.Max(0, speedTarget - 25);
|
||||
if (Input.GetKeyDown(KeyCode.PageUp)) height = Mathf.Min(1000, height + 100);
|
||||
if (Input.GetKeyDown(KeyCode.PageDown)) height = Mathf.Max(-1000, height - 100);
|
||||
}
|
||||
|
||||
// ---- Sticks ----
|
||||
|
||||
float ReadAxis(string name)
|
||||
{
|
||||
try { return Input.GetAxis(name); }
|
||||
catch (ArgumentException) { JoystickAxesMissing = true; return 0; } // axis not defined in Input Manager
|
||||
}
|
||||
|
||||
float Read(AxisBinding b)
|
||||
{
|
||||
float v = ReadAxis(b.axis) * (b.invert ? -1 : 1);
|
||||
float mag = Mathf.Abs(v);
|
||||
if (mag <= deadzone) return 0;
|
||||
return Mathf.Sign(v) * Mathf.Clamp01((mag - deadzone) / (1 - deadzone));
|
||||
}
|
||||
|
||||
void ReadJoystick()
|
||||
{
|
||||
sticks[0] = Read(leftX);
|
||||
sticks[1] = Read(leftY);
|
||||
sticks[2] = Read(leftTwist);
|
||||
sticks[3] = Read(rightX);
|
||||
sticks[4] = Read(rightY);
|
||||
sticks[5] = Read(rightTwist);
|
||||
}
|
||||
|
||||
// WASD + Q/E = left stick (walk, turn), IJKL + U/O = right stick.
|
||||
void ReadKeyboardSticks(float dt)
|
||||
{
|
||||
float Key(KeyCode pos, KeyCode neg) => (Input.GetKey(pos) ? 1 : 0) - (Input.GetKey(neg) ? 1 : 0);
|
||||
float[] target =
|
||||
{
|
||||
Key(KeyCode.D, KeyCode.A), Key(KeyCode.W, KeyCode.S), Key(KeyCode.Q, KeyCode.E),
|
||||
Key(KeyCode.L, KeyCode.J), Key(KeyCode.I, KeyCode.K), Key(KeyCode.U, KeyCode.O),
|
||||
};
|
||||
for (int i = 0; i < 6; i++) sticks[i] = Mathf.MoveTowards(sticks[i], target[i], keyStickRatePerSec * dt);
|
||||
}
|
||||
|
||||
// ---- Transmit ----
|
||||
|
||||
// Called by HexapodSimulation once per remote TX period.
|
||||
public void Transmit(HexapodSimNative native, float dt)
|
||||
{
|
||||
if (source == InputSource.RobotSerialHarness)
|
||||
{
|
||||
native.SerialInput(serialKeys_); // no radio: the firmware's harness injects its own packets
|
||||
serialKeys_ = "";
|
||||
return;
|
||||
}
|
||||
speedCurrent_ = Mathf.MoveTowards(speedCurrent_, speedTarget, speedRatePerSec * dt);
|
||||
LastPacket = BuildPacket();
|
||||
native.RadioSend(LastPacket);
|
||||
}
|
||||
|
||||
static short Wire(float v) => (short)Mathf.RoundToInt(Mathf.Clamp(v, -1f, 1f) * 1000f);
|
||||
|
||||
// Same mapping as remote/PacketBuilder.cpp.
|
||||
CmdPacket BuildPacket()
|
||||
{
|
||||
var p = new CmdPacket
|
||||
{
|
||||
seq = seq_++,
|
||||
speed_scale = (byte)Mathf.RoundToInt(speedCurrent_),
|
||||
body_height = (short)Mathf.Clamp(height, -1000, 1000), // a setting: sent even while disarmed
|
||||
};
|
||||
byte f = 0;
|
||||
if (armed) f |= CmdPacket.FLAG_ARMED;
|
||||
if (stand) f |= CmdPacket.FLAG_STAND;
|
||||
if (modeB) f |= CmdPacket.FLAG_MODE_B;
|
||||
if (rstickXlate) f |= CmdPacket.FLAG_RSTICK_XLATE;
|
||||
if (gaitWave) f |= CmdPacket.FLAG_GAIT_WAVE;
|
||||
if (robotGyro) f |= CmdPacket.FLAG_ROBOT_GYRO;
|
||||
|
||||
if (!armed) { p.flags = f; return p; } // disarmed: neutral command
|
||||
|
||||
// Left stick: forward -> +vx, right -> -vy (vy + = left), twist CCW -> +yaw_rate.
|
||||
p.vx = Wire(sticks[1]);
|
||||
p.vy = Wire(-sticks[0]);
|
||||
p.yaw_rate = Wire(sticks[2]);
|
||||
|
||||
if (rstickXlate)
|
||||
{
|
||||
p.rx = Wire(sticks[4]); // X offset: forward
|
||||
p.ry = Wire(-sticks[3]); // Y offset: right -> -Y
|
||||
p.rz = Wire(sticks[5]); // Z offset: twist CCW -> up
|
||||
}
|
||||
else
|
||||
{
|
||||
p.rx = Wire(sticks[3]); // roll: right side down
|
||||
p.ry = Wire(sticks[4]); // pitch: nose down
|
||||
p.rz = Wire(sticks[5]); // yaw: CCW
|
||||
}
|
||||
|
||||
bool walkInput = p.vx != 0 || p.vy != 0 || (!modeB && p.yaw_rate != 0);
|
||||
if (walkInput) lastWalkInputTime_ = Time.time;
|
||||
if (stand && (walkInput || Time.time - lastWalkInputTime_ < walkLingerSec)) f |= CmdPacket.FLAG_WALK;
|
||||
|
||||
if (remoteGyro)
|
||||
{
|
||||
f |= CmdPacket.FLAG_REMOTE_GYRO;
|
||||
p.remote_roll = Wire(handsetRollDeg / handsetFullScaleDeg);
|
||||
p.remote_pitch = Wire(handsetPitchDeg / handsetFullScaleDeg);
|
||||
}
|
||||
|
||||
p.flags = f;
|
||||
return p;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
fileFormatVersion: 2
|
||||
guid: d1e34fa033f570c48aba85f690477c41
|
||||
@@ -0,0 +1,160 @@
|
||||
using System.Text;
|
||||
using UnityEngine;
|
||||
|
||||
namespace HexapodSim
|
||||
{
|
||||
// On-screen control/status panel (IMGUI, so it needs no scene setup).
|
||||
// Every remote toggle, the speed slider, simulated handset tilt, ground
|
||||
// tilt, firmware reload, live robot state, the packet on air and the
|
||||
// robot's serial output. F1 hides it.
|
||||
public class SimTestPanel : MonoBehaviour
|
||||
{
|
||||
public bool visible = true;
|
||||
|
||||
HexapodSimulation sim_;
|
||||
SimRemote remote_;
|
||||
Vector2 scroll_;
|
||||
GUIStyle mono_;
|
||||
|
||||
static readonly string[] SourceNames = { "Joystick", "Keyboard", "Robot serial" };
|
||||
|
||||
void Awake()
|
||||
{
|
||||
sim_ = GetComponent<HexapodSimulation>();
|
||||
remote_ = GetComponent<SimRemote>();
|
||||
}
|
||||
|
||||
void Update()
|
||||
{
|
||||
if (Input.GetKeyDown(KeyCode.F1)) visible = !visible;
|
||||
}
|
||||
|
||||
void OnGUI()
|
||||
{
|
||||
if (!visible || sim_ == null || remote_ == null) return;
|
||||
if (mono_ == null)
|
||||
mono_ = new GUIStyle(GUI.skin.label) { font = Font.CreateDynamicFontFromOSFont("Consolas", 12), fontSize = 12, wordWrap = false };
|
||||
|
||||
GUILayout.BeginArea(new Rect(10, 10, 390, Screen.height - 20), GUI.skin.box);
|
||||
scroll_ = GUILayout.BeginScrollView(scroll_);
|
||||
|
||||
GUILayout.Label("<b>Hexapod firmware simulation</b> (F1 hides)", new GUIStyle(GUI.skin.label) { richText = true });
|
||||
if (sim_.LoadError != null)
|
||||
{
|
||||
GUILayout.Label("<color=#ff6060>" + sim_.LoadError + "</color>", new GUIStyle(GUI.skin.label) { richText = true, wordWrap = true });
|
||||
if (GUILayout.Button("Retry loading firmware")) sim_.ReloadFirmware();
|
||||
GUILayout.EndScrollView();
|
||||
GUILayout.EndArea();
|
||||
return;
|
||||
}
|
||||
|
||||
// ---- Robot status (read from the firmware's own globals) ----
|
||||
var s = sim_.Status;
|
||||
GUILayout.Label(
|
||||
$"state {s.StateName,-8} blend {s.sitStandBlend:F2} link {(s.linkOk != 0 ? "OK" : "DOWN")}\n" +
|
||||
$"heading {s.headingDeg,7:F1} IMU r {s.imuRollDeg,5:F1} p {s.imuPitchDeg,5:F1} {(s.imuEnabled != 0 ? "on" : "off")}\n" +
|
||||
$"t {s.simTimeMs / 1000f,7:F1}s ticks {s.controlTicks} stance {StanceString(s.stanceMask)}\n" +
|
||||
$"body pose {s.poseReachScale * 100f,3:F0}% walk {s.twistScale * 100f,3:F0}%" +
|
||||
$"{(s.poseReachScale < 0.995f || s.twistScale < 0.995f ? " (limited: reach / leg clearance)" : "")}\n" +
|
||||
$"body x {sim_.BodyPosMm.x,7:F0} y {sim_.BodyPosMm.y,7:F0} z {sim_.BodyPosMm.z,5:F0} mm yaw {sim_.BodyYawDeg,6:F1}",
|
||||
mono_);
|
||||
foreach (var w in sim_.RigWarnings)
|
||||
GUILayout.Label("<color=#ffb040>rig: " + w + "</color>", new GUIStyle(GUI.skin.label) { richText = true, wordWrap = true, fontSize = 11 });
|
||||
if (GUILayout.Button("Reload firmware (fresh power-on, picks up a new DLL build)")) sim_.ReloadFirmware();
|
||||
|
||||
// ---- Input source ----
|
||||
GUILayout.Space(6);
|
||||
remote_.source = (SimRemote.InputSource)GUILayout.Toolbar((int)remote_.source, SourceNames);
|
||||
if (remote_.source == SimRemote.InputSource.RobotSerialHarness)
|
||||
{
|
||||
GUILayout.Label("Keys go to robot.ino's TEST_SERIAL_INPUT harness (no radio):\n" +
|
||||
"a arm s sit/stand w/x fwd/back q/e strafe L/R\n" +
|
||||
"z/c yaw g gait m mode space stop", mono_);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (remote_.source == SimRemote.InputSource.Keyboard)
|
||||
GUILayout.Label("WASD walk Q/E turn IJKL + U/O right stick\n1 arm 2 stand 3 gait 4 mode 5 R-stick 6/7 gyros\nBackspace disarm +/- speed PgUp/PgDn height", mono_);
|
||||
if (remote_.source == SimRemote.InputSource.Joystick && remote_.JoystickAxesMissing)
|
||||
GUILayout.Label("Input axes \"1\"..\"6\" are missing from the Input Manager.");
|
||||
|
||||
// ---- Toggles: same set as the remote's touchscreen ----
|
||||
GUILayout.BeginHorizontal();
|
||||
if (GUILayout.Button(remote_.armed ? "ARMED" : "DISARMED")) remote_.ToggleArm();
|
||||
if (GUILayout.Button("DISARM")) remote_.Disarm();
|
||||
if (GUILayout.Button(remote_.stand ? "STAND" : "SIT")) remote_.ToggleStand();
|
||||
GUILayout.EndHorizontal();
|
||||
GUILayout.BeginHorizontal();
|
||||
if (GUILayout.Button(remote_.gaitWave ? "WAVE" : "TRIPOD")) remote_.gaitWave = !remote_.gaitWave;
|
||||
if (GUILayout.Button(remote_.modeB ? "HEADING" : "DIRECT")) remote_.modeB = !remote_.modeB;
|
||||
if (GUILayout.Button(remote_.rstickXlate ? "R: MOVE" : "R: ROTATE")) remote_.rstickXlate = !remote_.rstickXlate;
|
||||
GUILayout.EndHorizontal();
|
||||
GUILayout.BeginHorizontal();
|
||||
remote_.remoteGyro = GUILayout.Toggle(remote_.remoteGyro, " remote gyro");
|
||||
remote_.robotGyro = GUILayout.Toggle(remote_.robotGyro, " robot gyro");
|
||||
GUILayout.EndHorizontal();
|
||||
if (!string.IsNullOrEmpty(remote_.Message)) GUILayout.Label("> " + remote_.Message);
|
||||
|
||||
GUILayout.Label($"speed {remote_.speedTarget * 100 / 255}%");
|
||||
remote_.speedTarget = Mathf.RoundToInt(GUILayout.HorizontalSlider(remote_.speedTarget, 0, 255));
|
||||
|
||||
var geo = sim_.Geometry;
|
||||
float targetMm = geo.stanceHeightMm + remote_.height / 1000f * geo.stanceHeightRangeMm;
|
||||
GUILayout.Label($"height {targetMm:F0} mm (now {sim_.Status.stanceHeightMm:F0}) PgUp/PgDn");
|
||||
remote_.height = Mathf.RoundToInt(GUILayout.HorizontalSlider(remote_.height, -1000, 1000) / 50f) * 50;
|
||||
|
||||
GUILayout.Label($"handset tilt roll {remote_.handsetRollDeg:F0} pitch {remote_.handsetPitchDeg:F0} deg");
|
||||
remote_.handsetRollDeg = GUILayout.HorizontalSlider(remote_.handsetRollDeg, -30, 30);
|
||||
remote_.handsetPitchDeg = GUILayout.HorizontalSlider(remote_.handsetPitchDeg, -30, 30);
|
||||
}
|
||||
|
||||
GUILayout.Label($"ground tilt roll {sim_.groundTiltDeg.x:F0} pitch {sim_.groundTiltDeg.y:F0} deg (robot gyro test)");
|
||||
sim_.groundTiltDeg.x = GUILayout.HorizontalSlider(sim_.groundTiltDeg.x, -15, 15);
|
||||
sim_.groundTiltDeg.y = GUILayout.HorizontalSlider(sim_.groundTiltDeg.y, -15, 15);
|
||||
if (GUILayout.Button("Level ground")) sim_.groundTiltDeg = Vector2.zero;
|
||||
|
||||
// ---- Live values ----
|
||||
GUILayout.Space(6);
|
||||
var st = remote_.sticks;
|
||||
var raw = remote_.rawAxes;
|
||||
var p = remote_.LastPacket;
|
||||
GUILayout.Label(
|
||||
$"sticks L {st[0],5:F2} {st[1],5:F2} {st[2],5:F2} R {st[3],5:F2} {st[4],5:F2} {st[5],5:F2}\n" +
|
||||
$"axes 1-6 {raw[0],5:F2} {raw[1],5:F2} {raw[2],5:F2} {raw[3],5:F2} {raw[4],5:F2} {raw[5],5:F2}\n" +
|
||||
$"packet #{p.seq,3} vx {p.vx,5} vy {p.vy,5} yaw {p.yaw_rate,5}\n" +
|
||||
$" r {p.rx,5} {p.ry,5} {p.rz,5} tilt {p.remote_roll,5} {p.remote_pitch,5}\n" +
|
||||
$" height {p.body_height,5} speed {p.speed_scale,3} flags {System.Convert.ToString(p.flags, 2).PadLeft(8, '0')}",
|
||||
mono_);
|
||||
GUILayout.Label(JointTable(), mono_);
|
||||
|
||||
GUILayout.Label("robot serial:", mono_);
|
||||
var log = new StringBuilder();
|
||||
for (int i = Mathf.Max(0, sim_.SerialLog.Count - 10); i < sim_.SerialLog.Count; i++) log.AppendLine(sim_.SerialLog[i]);
|
||||
GUILayout.Label(log.ToString(), mono_);
|
||||
|
||||
GUILayout.EndScrollView();
|
||||
GUILayout.EndArea();
|
||||
}
|
||||
|
||||
static string StanceString(int mask)
|
||||
{
|
||||
var sb = new StringBuilder(6);
|
||||
for (int i = 0; i < 6; i++) sb.Append((mask & (1 << i)) != 0 ? '#' : '.');
|
||||
return sb.ToString();
|
||||
}
|
||||
|
||||
// Per leg: decoded joint angles, whether the servo got fresh pulses
|
||||
// (a leg that stops updating while armed = IK rejected the target),
|
||||
// and whether the foot is on the ground.
|
||||
string JointTable()
|
||||
{
|
||||
var sb = new StringBuilder("leg coxa femur tibia att gnd\n");
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
sb.Append($"{i,3} {sim_.JointsDeg[i * 3],6:F1} {sim_.JointsDeg[i * 3 + 1],6:F1} {sim_.JointsDeg[i * 3 + 2],6:F1}");
|
||||
sb.Append($" {(sim_.ServoAttached[i * 3] != 0 ? " y " : " - ")} {(sim_.Contact[i] ? " # " : " . ")}\n");
|
||||
}
|
||||
return sb.ToString();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
fileFormatVersion: 2
|
||||
guid: dc5806c61edfd2c4e8ef048e706a10b2
|
||||
@@ -0,0 +1,10 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 5f676c461da1c254990d7719d77e6250
|
||||
folderAsset: yes
|
||||
timeCreated: 1641405303
|
||||
licenseType: Free
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,72 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using UnityEngine;
|
||||
|
||||
namespace Assets.JoyStick
|
||||
{
|
||||
class HandleHoystick : MonoBehaviour
|
||||
{
|
||||
public RotationControl bot;
|
||||
public Walk walk;
|
||||
|
||||
public void Update()
|
||||
{
|
||||
float a2 = Remap(Input.GetAxis("1"), 1, -127, -1, 127);
|
||||
float a1 = Remap(Input.GetAxis("2"), 1, -127, -1, 127);
|
||||
float a3 = Remap(Input.GetAxis("3"), -1, -127, 1, 127);
|
||||
|
||||
float b2 = Remap(Input.GetAxis("4"), -1, -127, 1, 127);
|
||||
float b3 = Remap(Input.GetAxis("5"), -1, -127, 1, 127);
|
||||
float b1 = Remap(Input.GetAxis("6"), -1, -127, 1, 127);
|
||||
|
||||
//Debug.Log(Input.GetAxis("4"));
|
||||
//Debug.Log(Remap(Input.GetAxis("4"), -1, -127, 1, 127));
|
||||
|
||||
bot.rotation = new Quaternion(limit(a2, -20, 20), -1 * limit(a3, -20, 20), -1 * limit(a1, -20, 20),
|
||||
1);
|
||||
|
||||
float speed = (float)(Math.Sqrt(b1 * b1 + b2 * b2) / 127) * 3;
|
||||
walk.walkspeed = speed < 0.5f ? 0 : speed;
|
||||
|
||||
float rotation = -1 * limit(b3 / 4, -5, 5);
|
||||
walk.centerRotation = rotation < 0.5f && rotation > -0.5f ? 0 : rotation;
|
||||
|
||||
walk.walkDirection = (-1 * ConvertRadiansToDegrees((float)Math.Atan2(b2, b1)));
|
||||
|
||||
float rotationabs = Math.Abs(walk.centerRotation);
|
||||
float speed2 = ((6 / 3) * walk.walkspeed) + ((6 / 5) * rotationabs);
|
||||
speed2 = speed2 < 6 ? speed2 : 6;
|
||||
walk.speed2 = (int)(10 - speed2);
|
||||
}
|
||||
|
||||
public float limit(float input, float min, float max)
|
||||
{
|
||||
input = input / 6;
|
||||
return (input < max & input > min) ? input : input < min ? min : max;
|
||||
}
|
||||
|
||||
public float ConvertRadiansToDegrees(float radians)
|
||||
{
|
||||
float degrees = (float)((180 / Math.PI) * radians);
|
||||
return (degrees);
|
||||
}
|
||||
|
||||
public float Remap(float from, float fromMin, float toMin, float fromMax, float toMax)
|
||||
{
|
||||
var fromAbs = from - fromMin;
|
||||
var fromMaxAbs = fromMax - fromMin;
|
||||
|
||||
var normal = fromAbs / fromMaxAbs;
|
||||
|
||||
var toMaxAbs = toMax - toMin;
|
||||
var toAbs = toMaxAbs * normal;
|
||||
|
||||
var to = toAbs + toMin;
|
||||
|
||||
return to;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
fileFormatVersion: 2
|
||||
guid: c5ce8a2605d301e44a4a210aa345ed62
|
||||
timeCreated: 1641405303
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,131 @@
|
||||
using UnityEngine;
|
||||
|
||||
public class Leg : MonoBehaviour {
|
||||
public Transform Hip1;
|
||||
public Transform Hip2;
|
||||
public Transform Knie;
|
||||
|
||||
// rotation angles for different servos
|
||||
public double a1 = 0;
|
||||
public double a2 = 0;
|
||||
public double a3 = 0;
|
||||
|
||||
public Vector3 point = new Vector3(10, 10, 0);
|
||||
public Quaternion rotation = new Quaternion(0, 0, 0, 0);
|
||||
|
||||
public Vector3 transformationBeforeRotation = new Vector3(0, 0, 0);
|
||||
public Vector3 transformationAfterRotation = new Vector3(0, 0, 0);
|
||||
|
||||
public bool invertForLeftLeg = false;
|
||||
|
||||
// translation to change origin to middle of body
|
||||
public double translateX = 0;
|
||||
public double translateY = 0;
|
||||
public double translateZ = 0;
|
||||
|
||||
public GameObject pointShouldBe;
|
||||
//public Vector3 pos = new Vector3(0, 0, 0);
|
||||
|
||||
public int startServo = -1;
|
||||
|
||||
private Foot foot = new Foot();
|
||||
|
||||
// Use this for initialization
|
||||
void Start () {
|
||||
|
||||
}
|
||||
|
||||
// Update is called once per frame
|
||||
public void FixedUpdate()
|
||||
{
|
||||
foot.X = point.x;
|
||||
foot.Y = point.y;
|
||||
foot.Z = point.z;
|
||||
|
||||
foot.rotationX = rotation.x;
|
||||
foot.rotationY = rotation.y;
|
||||
foot.rotationZ = rotation.z;
|
||||
|
||||
foot.translateX = translateX;
|
||||
foot.translateY = translateY;
|
||||
foot.translateZ = translateZ;
|
||||
|
||||
// transform before rotation.
|
||||
foot.transformation = transformationBeforeRotation;
|
||||
|
||||
// transform after rotation.
|
||||
foot.transformation2 = transformationAfterRotation;
|
||||
|
||||
foot.invert = invertForLeftLeg;
|
||||
|
||||
foot.setleg();
|
||||
|
||||
if (!invertForLeftLeg)
|
||||
{
|
||||
a1 = foot.O1 - 90;
|
||||
a2 = 90 - foot.O2;
|
||||
a3 = foot.O3 - 90;
|
||||
}
|
||||
else
|
||||
{
|
||||
a1 = (foot.O1 - 90)*-1;
|
||||
a2 = 90 - foot.O2;
|
||||
a3 = foot.O3 - 90;
|
||||
}
|
||||
|
||||
if (!invertForLeftLeg && startServo != -1)
|
||||
{
|
||||
// Serial.Instance.send(startServo,
|
||||
// (int)Remap((float)a1, -90, 90, 2500, 500),
|
||||
// (int)Remap((float)a2, -90, 90, 2500, 500), // 0 is middel
|
||||
// (int)Remap((float)a3, 90, -90, 2500, 500)); // 0 is middel
|
||||
if(false)
|
||||
Serial.Instance.send(startServo,
|
||||
(int)Remap((float)a1, -90, 90, 0, 180),
|
||||
(int)Remap((float)a2, 90, -90, 0, 180), // 0 is middel
|
||||
(int)Remap((float)a3, -90, 90, 0, 180)); // 0 is middel
|
||||
}
|
||||
else if (invertForLeftLeg && startServo != -1)
|
||||
{
|
||||
double a1 = foot.O1 - 90;
|
||||
double a2 = 90 - foot.O2;
|
||||
double a3 = foot.O3 - 90;
|
||||
|
||||
// Serial.Instance.send(startServo,
|
||||
// (int)Remap((float)a1, -90, 90, 500, 2500),
|
||||
// (int)Remap((float)a2, -90, 90, 500, 2500), // 0 is middel
|
||||
// (int)Remap((float)a3, 90, -90, 500, 2500)); // 0 is middel
|
||||
if (false)
|
||||
Serial.Instance.send(startServo,
|
||||
(int)Remap((float)a1, 90, -90, 0, 180),
|
||||
(int)Remap((float)a2, -90, 90, 0, 180), // 0 is middel
|
||||
(int)Remap((float)a3, 90, -90, 0, 180)); // 0 is middel
|
||||
}
|
||||
|
||||
Hip1.transform.localRotation = Quaternion.Euler(0, (float)a1, 0);
|
||||
Hip2.transform.localRotation = Quaternion.Euler((float)a2, 0, 0);
|
||||
Knie.transform.localRotation = Quaternion.Euler((float)a3, 0, 0);
|
||||
|
||||
pointShouldBe.transform.localPosition = foot.pos;
|
||||
}
|
||||
|
||||
public Vector3 getLocalPos()
|
||||
{
|
||||
return point;
|
||||
}
|
||||
|
||||
public Vector3 getWorldPos()
|
||||
{
|
||||
return point + new Vector3((float)translateX, (float)translateY, (float)translateZ);
|
||||
}
|
||||
|
||||
public void setWorldPos(Vector3 pos)
|
||||
{
|
||||
this.point = pos - new Vector3((float)translateX, (float)translateY, (float)translateZ);
|
||||
}
|
||||
|
||||
public static float Remap(float value, float from1, float to1, float from2, float to2)
|
||||
{
|
||||
return (value - from1) / (to1 - from1) * (to2 - from2) + from2;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 3c4e50b7048aa1f4aaee65a71c7626c3
|
||||
timeCreated: 1541794260
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
File diff suppressed because it is too large.
Load diff
@@ -0,0 +1,10 @@
|
||||
fileFormatVersion: 2
|
||||
guid: c2064a01ffb68a84fa55bd109ccf70a4
|
||||
timeCreated: 1541791156
|
||||
licenseType: Free
|
||||
NativeFormatImporter:
|
||||
externalObjects: {}
|
||||
mainObjectFileID: 100100000
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,8 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 547028cd316445c4097ebf3bd027bd9f
|
||||
folderAsset: yes
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
File diff suppressed because it is too large.
Load diff
@@ -0,0 +1,9 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 5f4a5f0170e50d546b983a411916f66d
|
||||
timeCreated: 1541789909
|
||||
licenseType: Free
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
Binary file not shown.
@@ -0,0 +1,8 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 2acd50ffdfdd676458a2e9b2e5c19f5c
|
||||
NativeFormatImporter:
|
||||
externalObjects: {}
|
||||
mainObjectFileID: 112000000
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
Binary file not shown.
@@ -0,0 +1,119 @@
|
||||
fileFormatVersion: 2
|
||||
guid: a66121207c396d94b8b3bd5b43a6c459
|
||||
TextureImporter:
|
||||
internalIDToNameTable: []
|
||||
externalObjects: {}
|
||||
serializedVersion: 13
|
||||
mipmaps:
|
||||
mipMapMode: 0
|
||||
enableMipMap: 1
|
||||
sRGBTexture: 1
|
||||
linearTexture: 0
|
||||
fadeOut: 0
|
||||
borderMipMap: 0
|
||||
mipMapsPreserveCoverage: 0
|
||||
alphaTestReferenceValue: 0.5
|
||||
mipMapFadeDistanceStart: 1
|
||||
mipMapFadeDistanceEnd: 3
|
||||
bumpmap:
|
||||
convertToNormalMap: 0
|
||||
externalNormalMap: 0
|
||||
heightScale: 0.25
|
||||
normalMapFilter: 0
|
||||
flipGreenChannel: 0
|
||||
isReadable: 0
|
||||
streamingMipmaps: 0
|
||||
streamingMipmapsPriority: 0
|
||||
vTOnly: 0
|
||||
ignoreMipmapLimit: 0
|
||||
grayScaleToAlpha: 0
|
||||
generateCubemap: 6
|
||||
cubemapConvolution: 1
|
||||
seamlessCubemap: 1
|
||||
textureFormat: 1
|
||||
maxTextureSize: 2048
|
||||
textureSettings:
|
||||
serializedVersion: 2
|
||||
filterMode: 2
|
||||
aniso: 0
|
||||
mipBias: 0
|
||||
wrapU: 1
|
||||
wrapV: 1
|
||||
wrapW: 1
|
||||
nPOTScale: 1
|
||||
lightmap: 0
|
||||
compressionQuality: 50
|
||||
spriteMode: 0
|
||||
spriteExtrude: 1
|
||||
spriteMeshType: 1
|
||||
alignment: 0
|
||||
spritePivot: {x: 0.5, y: 0.5}
|
||||
spritePixelsToUnits: 100
|
||||
spriteBorder: {x: 0, y: 0, z: 0, w: 0}
|
||||
spriteGenerateFallbackPhysicsShape: 1
|
||||
alphaUsage: 1
|
||||
alphaIsTransparency: 0
|
||||
spriteTessellationMethod: 0
|
||||
spriteTessellationDetail: -1
|
||||
spriteGeometrySubdivision: -1
|
||||
textureType: 0
|
||||
textureShape: 2
|
||||
singleChannelComponent: 0
|
||||
flipbookRows: 1
|
||||
flipbookColumns: 1
|
||||
maxTextureSizeSet: 0
|
||||
compressionQualitySet: 0
|
||||
textureFormatSet: 0
|
||||
ignorePngGamma: 0
|
||||
applyGammaDecoding: 0
|
||||
swizzle: 50462976
|
||||
cookieLightType: 0
|
||||
platformSettings:
|
||||
- serializedVersion: 4
|
||||
buildTarget: DefaultTexturePlatform
|
||||
maxTextureSize: 2048
|
||||
resizeAlgorithm: 0
|
||||
textureFormat: -1
|
||||
textureCompression: 1
|
||||
compressionQuality: 100
|
||||
crunchedCompression: 0
|
||||
allowsAlphaSplitting: 0
|
||||
overridden: 0
|
||||
ignorePlatformSupport: 0
|
||||
androidETC2FallbackOverride: 0
|
||||
forceMaximumCompressionQuality_BC6H_BC7: 0
|
||||
- serializedVersion: 4
|
||||
buildTarget: Standalone
|
||||
maxTextureSize: 2048
|
||||
resizeAlgorithm: 0
|
||||
textureFormat: -1
|
||||
textureCompression: 1
|
||||
compressionQuality: 50
|
||||
crunchedCompression: 0
|
||||
allowsAlphaSplitting: 0
|
||||
overridden: 0
|
||||
ignorePlatformSupport: 0
|
||||
androidETC2FallbackOverride: 0
|
||||
forceMaximumCompressionQuality_BC6H_BC7: 0
|
||||
spriteSheet:
|
||||
serializedVersion: 2
|
||||
sprites: []
|
||||
outline: []
|
||||
customData:
|
||||
physicsShape: []
|
||||
bones: []
|
||||
spriteID:
|
||||
internalID: 0
|
||||
vertices: []
|
||||
indices:
|
||||
edges: []
|
||||
weights: []
|
||||
secondaryTextures: []
|
||||
spriteCustomMetadata:
|
||||
entries: []
|
||||
nameFileIdTable: {}
|
||||
mipmapLimitGroupName:
|
||||
pSDRemoveMatte: 0
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,77 @@
|
||||
%YAML 1.1
|
||||
%TAG !u! tag:unity3d.com,2011:
|
||||
--- !u!850595691 &4890085278179872738
|
||||
LightingSettings:
|
||||
m_ObjectHideFlags: 0
|
||||
m_CorrespondingSourceObject: {fileID: 0}
|
||||
m_PrefabInstance: {fileID: 0}
|
||||
m_PrefabAsset: {fileID: 0}
|
||||
m_Name: MainSettings
|
||||
serializedVersion: 10
|
||||
m_EnableBakedLightmaps: 1
|
||||
m_EnableRealtimeLightmaps: 1
|
||||
m_RealtimeEnvironmentLighting: 1
|
||||
m_BounceScale: 1
|
||||
m_AlbedoBoost: 1
|
||||
m_IndirectOutputScale: 1
|
||||
m_UsingShadowmask: 1
|
||||
m_BakeBackend: 1
|
||||
m_LightmapMaxSize: 1024
|
||||
m_LightmapSizeFixed: 0
|
||||
m_UseMipmapLimits: 1
|
||||
m_BakeResolution: 40
|
||||
m_Padding: 2
|
||||
m_LightmapCompression: 3
|
||||
m_LightmapPackingMode: 1
|
||||
m_LightmapPackingMethod: 0
|
||||
m_XAtlasPackingAttempts: 16384
|
||||
m_XAtlasBruteForce: 0
|
||||
m_XAtlasBlockAlign: 0
|
||||
m_XAtlasRepackUnderutilizedLightmaps: 1
|
||||
m_AO: 0
|
||||
m_AOMaxDistance: 1
|
||||
m_CompAOExponent: 1
|
||||
m_CompAOExponentDirect: 0
|
||||
m_ExtractAO: 0
|
||||
m_MixedBakeMode: 2
|
||||
m_LightmapsBakeMode: 1
|
||||
m_FilterMode: 1
|
||||
m_LightmapParameters: {fileID: 15204, guid: 0000000000000000f000000000000000, type: 0}
|
||||
m_ExportTrainingData: 0
|
||||
m_EnableWorkerProcessBaking: 1
|
||||
m_TrainingDataDestination: TrainingData
|
||||
m_RealtimeResolution: 2
|
||||
m_ForceWhiteAlbedo: 0
|
||||
m_ForceUpdates: 0
|
||||
m_PVRCulling: 1
|
||||
m_PVRSampling: 1
|
||||
m_PVRDirectSampleCount: 32
|
||||
m_PVRSampleCount: 512
|
||||
m_PVREnvironmentSampleCount: 512
|
||||
m_PVREnvironmentReferencePointCount: 2048
|
||||
m_LightProbeSampleCountMultiplier: 4
|
||||
m_PVRBounces: 2
|
||||
m_PVRMinBounces: 2
|
||||
m_PVREnvironmentImportanceSampling: 0
|
||||
m_PVRFilteringMode: 2
|
||||
m_PVRDenoiserTypeDirect: 0
|
||||
m_PVRDenoiserTypeIndirect: 0
|
||||
m_PVRDenoiserTypeAO: 0
|
||||
m_PVRFilterTypeDirect: 0
|
||||
m_PVRFilterTypeIndirect: 0
|
||||
m_PVRFilterTypeAO: 0
|
||||
m_PVRFilteringGaussRadiusDirect: 1
|
||||
m_PVRFilteringGaussRadiusIndirect: 1
|
||||
m_PVRFilteringGaussRadiusAO: 1
|
||||
m_PVRFilteringAtrousPositionSigmaDirect: 0.5
|
||||
m_PVRFilteringAtrousPositionSigmaIndirect: 2
|
||||
m_PVRFilteringAtrousPositionSigmaAO: 1
|
||||
m_RespectSceneVisibilityWhenBakingGI: 0
|
||||
m_DirectLightSamplingMode: 2
|
||||
m_DirectRISCandidateCount: 4
|
||||
m_IndirectLightSamplingMode: 1
|
||||
m_IndirectRISCandidateCount: 1
|
||||
m_LightAccelerationStructure: 0
|
||||
m_LightGridMaxCells: 262144
|
||||
m_DirectEmissiveSamplingMode: 2
|
||||
m_IndirectEmissiveSamplingMode: 1
|
||||
@@ -0,0 +1,8 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 0a9c1a93ed0b509459670ebd77736273
|
||||
NativeFormatImporter:
|
||||
externalObjects: {}
|
||||
mainObjectFileID: 4890085278179872738
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,9 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 333d24e71d12446ba5c77815e64b0ca8
|
||||
folderAsset: yes
|
||||
timeCreated: 1448926447
|
||||
licenseType: Pro
|
||||
DefaultImporter:
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,9 @@
|
||||
fileFormatVersion: 2
|
||||
guid: e105e00cdce8456482d26b1fcd1ca47d
|
||||
folderAsset: yes
|
||||
timeCreated: 1448926516
|
||||
licenseType: Pro
|
||||
DefaultImporter:
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
LFS
BIN
Binary file not shown.
+90
@@ -0,0 +1,90 @@
|
||||
fileFormatVersion: 2
|
||||
guid: a1e8415cfe85469f9aa0aea941166031
|
||||
labels:
|
||||
- gvh
|
||||
- gvh_targets-editor
|
||||
- gvh_version-1.2.135.0
|
||||
PluginImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
iconMap: {}
|
||||
executionOrder: {}
|
||||
defineConstraints: []
|
||||
isPreloaded: 0
|
||||
isOverridable: 0
|
||||
isExplicitlyReferenced: 0
|
||||
validateReferences: 0
|
||||
platformData:
|
||||
- first:
|
||||
: Any
|
||||
second:
|
||||
enabled: 0
|
||||
settings:
|
||||
Exclude Android: 1
|
||||
Exclude Editor: 0
|
||||
Exclude Linux64: 1
|
||||
Exclude OSXUniversal: 1
|
||||
Exclude Win: 1
|
||||
Exclude Win64: 1
|
||||
Exclude iOS: 1
|
||||
- first:
|
||||
Android: Android
|
||||
second:
|
||||
enabled: 0
|
||||
settings:
|
||||
CPU: ARMv7
|
||||
- first:
|
||||
Any:
|
||||
second:
|
||||
enabled: 0
|
||||
settings: {}
|
||||
- first:
|
||||
Editor: Editor
|
||||
second:
|
||||
enabled: 1
|
||||
settings:
|
||||
CPU: AnyCPU
|
||||
DefaultValueInitialized: true
|
||||
OS: OSX
|
||||
- first:
|
||||
Standalone: Linux64
|
||||
second:
|
||||
enabled: 0
|
||||
settings:
|
||||
CPU: x86_64
|
||||
- first:
|
||||
Standalone: OSXUniversal
|
||||
second:
|
||||
enabled: 0
|
||||
settings:
|
||||
CPU: None
|
||||
- first:
|
||||
Standalone: Win
|
||||
second:
|
||||
enabled: 0
|
||||
settings:
|
||||
CPU: x86
|
||||
- first:
|
||||
Standalone: Win64
|
||||
second:
|
||||
enabled: 0
|
||||
settings:
|
||||
CPU: x86_64
|
||||
- first:
|
||||
Windows Store Apps: WindowsStoreApps
|
||||
second:
|
||||
enabled: 0
|
||||
settings:
|
||||
CPU: AnyCPU
|
||||
- first:
|
||||
iPhone: iOS
|
||||
second:
|
||||
enabled: 0
|
||||
settings:
|
||||
AddToEmbeddedBinaries: false
|
||||
CPU: AnyCPU
|
||||
CompileFlags:
|
||||
FrameworkDependencies:
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
LFS
BIN
Binary file not shown.
+37
@@ -0,0 +1,37 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 97738b29c18143b0a469277b09d35889
|
||||
labels:
|
||||
- gvh
|
||||
- gvh_targets-editor
|
||||
- gvh_version-1.2.135.0
|
||||
PluginImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
iconMap: {}
|
||||
executionOrder: {}
|
||||
defineConstraints: []
|
||||
isPreloaded: 0
|
||||
isOverridable: 0
|
||||
isExplicitlyReferenced: 0
|
||||
validateReferences: 1
|
||||
platformData:
|
||||
- first:
|
||||
Any:
|
||||
second:
|
||||
enabled: 0
|
||||
settings: {}
|
||||
- first:
|
||||
Editor: Editor
|
||||
second:
|
||||
enabled: 1
|
||||
settings:
|
||||
DefaultValueInitialized: true
|
||||
- first:
|
||||
Windows Store Apps: WindowsStoreApps
|
||||
second:
|
||||
enabled: 0
|
||||
settings:
|
||||
CPU: AnyCPU
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
BIN
Binary file not shown.
+37
@@ -0,0 +1,37 @@
|
||||
fileFormatVersion: 2
|
||||
guid: bb6999c8a5ce4ba99688ec579babe5b7
|
||||
labels:
|
||||
- gvh
|
||||
- gvh_targets-editor
|
||||
- gvh_version-1.2.135.0
|
||||
PluginImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
iconMap: {}
|
||||
executionOrder: {}
|
||||
defineConstraints: []
|
||||
isPreloaded: 0
|
||||
isOverridable: 0
|
||||
isExplicitlyReferenced: 0
|
||||
validateReferences: 1
|
||||
platformData:
|
||||
- first:
|
||||
Any:
|
||||
second:
|
||||
enabled: 0
|
||||
settings: {}
|
||||
- first:
|
||||
Editor: Editor
|
||||
second:
|
||||
enabled: 1
|
||||
settings:
|
||||
DefaultValueInitialized: true
|
||||
- first:
|
||||
Windows Store Apps: WindowsStoreApps
|
||||
second:
|
||||
enabled: 0
|
||||
settings:
|
||||
CPU: AnyCPU
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
BIN
Binary file not shown.
+37
@@ -0,0 +1,37 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 369cd88e7aa04bd98e0f6c229f7c1510
|
||||
labels:
|
||||
- gvh
|
||||
- gvh_targets-editor
|
||||
- gvh_version-1.2.135.0
|
||||
PluginImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
iconMap: {}
|
||||
executionOrder: {}
|
||||
defineConstraints: []
|
||||
isPreloaded: 0
|
||||
isOverridable: 0
|
||||
isExplicitlyReferenced: 0
|
||||
validateReferences: 1
|
||||
platformData:
|
||||
- first:
|
||||
Any:
|
||||
second:
|
||||
enabled: 0
|
||||
settings: {}
|
||||
- first:
|
||||
Editor: Editor
|
||||
second:
|
||||
enabled: 1
|
||||
settings:
|
||||
DefaultValueInitialized: true
|
||||
- first:
|
||||
Windows Store Apps: WindowsStoreApps
|
||||
second:
|
||||
enabled: 0
|
||||
settings:
|
||||
CPU: AnyCPU
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
+4
@@ -0,0 +1,4 @@
|
||||
Assets/PlayServicesResolver/Editor/Google.IOSResolver.dll
|
||||
Assets/PlayServicesResolver/Editor/Google.JarResolver.dll
|
||||
Assets/PlayServicesResolver/Editor/Google.VersionHandler.dll
|
||||
Assets/PlayServicesResolver/Editor/Google.VersionHandlerImpl.dll
|
||||
+11
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 85ae9201cbc641ffa2fa8aab6c5e9093
|
||||
labels:
|
||||
- gvh
|
||||
- gvh_manifest
|
||||
- gvh_version-1.2.135.0
|
||||
TextScriptImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,8 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 734e9804abc7323409a6f1b70fb777cb
|
||||
folderAsset: yes
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1 @@
|
||||
{"androidStore":"GooglePlay"}
|
||||
@@ -0,0 +1,7 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 37bb84e1c4c7b9b4ba0a24545afb3aec
|
||||
TextScriptImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,104 @@
|
||||
using System.Collections;
|
||||
using System.Collections.Generic;
|
||||
using UnityEngine;
|
||||
|
||||
public class rotation
|
||||
{
|
||||
public float min = -15;
|
||||
public float max = 15;
|
||||
public float speed = 0.3f;
|
||||
public bool dir = false;
|
||||
public bool enabled = true;
|
||||
|
||||
public float i = 0;
|
||||
|
||||
public rotation(float speed, float min, float max)
|
||||
{
|
||||
this.speed = speed;
|
||||
this.min = min;
|
||||
this.max = max;
|
||||
}
|
||||
|
||||
public float update()
|
||||
{
|
||||
if (enabled)
|
||||
{
|
||||
if (!dir && i < max)
|
||||
{
|
||||
i += speed;
|
||||
}
|
||||
else if (!dir)
|
||||
{
|
||||
dir = !dir;
|
||||
i = (float)max;
|
||||
}
|
||||
else if (dir && i > min)
|
||||
{
|
||||
i -= speed;
|
||||
}
|
||||
else if (dir)
|
||||
{
|
||||
dir = !dir;
|
||||
i = (float)min;
|
||||
}
|
||||
}
|
||||
|
||||
return i;
|
||||
}
|
||||
}
|
||||
|
||||
public class RotationControl : MonoBehaviour
|
||||
{
|
||||
public Vector3 transformation = new Vector3(0, 0, 0);
|
||||
public Vector3 transformation2 = new Vector3(0, 0, 0);
|
||||
|
||||
public Quaternion rotation = new Quaternion(0, 0, 0, 0);
|
||||
public List<GameObject> rotation2 = new List<GameObject>();
|
||||
public GameObject body;
|
||||
public GameObject body2;
|
||||
|
||||
public rotation r1 = new rotation(0.1f, -10, 10);
|
||||
public rotation r2 = new rotation(0.2f, -10, 10);
|
||||
public rotation r3 = new rotation(0.3f, -10, 10);
|
||||
|
||||
public rotation r4 = new rotation(0.06f, -3, 3);
|
||||
public rotation r5 = new rotation(0.08f, -3, 3);
|
||||
public rotation r6 = new rotation(0.1f, -3, 3);
|
||||
|
||||
public rotation r7 = new rotation(0.1f, -3, 3);
|
||||
public rotation r8 = new rotation(0.2f, -3, 3);
|
||||
public rotation r9 = new rotation(0.3f, -3, 3);
|
||||
|
||||
// Use this for initialization
|
||||
void Start ()
|
||||
{
|
||||
}
|
||||
|
||||
// Update is called once per frame
|
||||
void FixedUpdate()
|
||||
{
|
||||
//rotation.x = r1.update();
|
||||
//rotation.y = r2.update();
|
||||
//rotation.z = r3.update();
|
||||
|
||||
//transformationBeforeRotation.x = r4.update();
|
||||
//transformationBeforeRotation.y = r5.update();
|
||||
//transformationBeforeRotation.z = r6.update();
|
||||
|
||||
/*transformationAfterRotation.x = r7.update();
|
||||
transformationAfterRotation.y = r8.update();
|
||||
transformationAfterRotation.z = r9.update();*/
|
||||
|
||||
body.transform.eulerAngles = new Vector3(rotation.x*-1, rotation.y*-1, rotation.z*-1);
|
||||
body.transform.position = transformation;
|
||||
body2.transform.localPosition = transformation2;
|
||||
|
||||
for (int index = 0; index < rotation2.Count; ++index)
|
||||
{
|
||||
rotation2[index].GetComponent<Leg>().rotation = rotation;
|
||||
rotation2[index].GetComponent<Leg>().transformationBeforeRotation = transformation;
|
||||
rotation2[index].GetComponent<Leg>().transformationAfterRotation = transformation2;
|
||||
rotation2[index].GetComponent<Leg>().FixedUpdate();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 30ea8dc0510444c42ae4b4db1f5ee2bd
|
||||
timeCreated: 1541847613
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,10 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 942facdab943ceb4ca29453f4ee4d1b3
|
||||
folderAsset: yes
|
||||
timeCreated: 1641404792
|
||||
licenseType: Free
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,119 @@
|
||||
using System;
|
||||
using UnityEngine;
|
||||
using System.Collections;
|
||||
using System.IO.Ports;
|
||||
using Random = System.Random;
|
||||
|
||||
public class Serial
|
||||
{
|
||||
private static Serial instance;
|
||||
private int rnd = 0;
|
||||
|
||||
private Serial()
|
||||
{
|
||||
}
|
||||
|
||||
~Serial()
|
||||
{
|
||||
serialPort.Close();
|
||||
serialPort.Dispose();
|
||||
}
|
||||
|
||||
public static Serial Instance
|
||||
{
|
||||
get
|
||||
{
|
||||
if (instance == null)
|
||||
{
|
||||
instance = new Serial();
|
||||
}
|
||||
|
||||
return instance;
|
||||
}
|
||||
}
|
||||
|
||||
public bool enabled = false;
|
||||
|
||||
public SerialPort serialPort;
|
||||
|
||||
public int[] values = new int[18];
|
||||
|
||||
public static string ByteArrayToString(byte[] ba)
|
||||
{
|
||||
return BitConverter.ToString(ba).Replace("-", " ");
|
||||
}
|
||||
|
||||
public void send(int start, int a, int b, int c)
|
||||
{
|
||||
if (serialPort == null)
|
||||
{
|
||||
serialPort = new SerialPort(@"\\.\" + "COM4", 250000);
|
||||
|
||||
try
|
||||
{
|
||||
serialPort.Parity = Parity.Even;
|
||||
serialPort.StopBits = StopBits.One;
|
||||
serialPort.DataBits = 8;
|
||||
serialPort.Open();
|
||||
serialPort.DiscardOutBuffer();
|
||||
serialPort.DiscardInBuffer();
|
||||
System.Threading.Thread.Sleep(1000);
|
||||
|
||||
enabled = true;
|
||||
}
|
||||
catch
|
||||
{
|
||||
enabled = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (enabled)
|
||||
{
|
||||
values[start] = a;
|
||||
values[++start] = b;
|
||||
values[++start] = c;
|
||||
|
||||
if (serialPort is SerialPort && start == 17)
|
||||
{
|
||||
// string send = "s";
|
||||
byte[] send = new byte[18+1+2];
|
||||
send[18] = 0x00;
|
||||
send[19] = 0xfe;
|
||||
send[20] = 0xff;
|
||||
// send[21] = 0xff;
|
||||
|
||||
// if (rnd == 0)
|
||||
// {
|
||||
//rnd = 1;
|
||||
for (int i = 0; i < 18; i++)
|
||||
{
|
||||
//string crc = ((i) * (values[i])) + "";
|
||||
//send = send + (i).ToString() + "_" + (values[i]).ToString() + "," + crc + "=";
|
||||
byte[] intBytes = BitConverter.GetBytes(values[i]);
|
||||
if (BitConverter.IsLittleEndian)
|
||||
Array.Reverse(intBytes);
|
||||
send[i] = intBytes[3];
|
||||
send[18] += send[i];
|
||||
}
|
||||
|
||||
// Debug.Log(ByteArrayToString(send));
|
||||
serialPort.Write(send, 0, 18+1+2);
|
||||
serialPort.BaseStream.Flush();
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// rnd = 0;
|
||||
// for (int i = 10; i < 18; i++)
|
||||
// {
|
||||
// string crc = ((i + 3) * (values[i])) + "";
|
||||
//
|
||||
// send = send + (i + 2).ToString() + "_" + (values[i]).ToString() + "," + crc + "=";
|
||||
// }
|
||||
//
|
||||
// Debug.Log(send);
|
||||
// serialPort.Write(send);
|
||||
// }
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
fileFormatVersion: 2
|
||||
guid: c99b6d9374ff6d3419c7692fdf3c1791
|
||||
timeCreated: 1542057315
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,70 @@
|
||||
/*using System;
|
||||
using UnityEngine;
|
||||
using System.Collections;
|
||||
using System.IO.Ports;
|
||||
using System.Linq;
|
||||
|
||||
public class Serial2 : MonoBehaviour{
|
||||
|
||||
public float x;
|
||||
public float y;
|
||||
|
||||
public RotationControl bot;
|
||||
|
||||
~Serial2() {
|
||||
serialPort.Close();
|
||||
}
|
||||
|
||||
public bool enabled = false;
|
||||
|
||||
SerialPort serialPort;
|
||||
|
||||
public void Update()
|
||||
{
|
||||
if (serialPort == null)
|
||||
{
|
||||
serialPort = new SerialPort(@"\\.\" + "COM7", 115200);
|
||||
|
||||
try
|
||||
{
|
||||
serialPort.NewLine = "\n";
|
||||
|
||||
serialPort.Open();
|
||||
serialPort.DiscardOutBuffer();
|
||||
serialPort.DiscardInBuffer();
|
||||
|
||||
enabled = true;
|
||||
}
|
||||
catch
|
||||
{
|
||||
enabled = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (enabled)
|
||||
{
|
||||
if (serialPort is SerialPort)
|
||||
{
|
||||
string received = serialPort.ReadLine();
|
||||
string[] data = received.Split(',');
|
||||
x = ConvertRadiansToDegrees(float.Parse(data.First()));
|
||||
y = ConvertRadiansToDegrees(float.Parse(data.Last()));
|
||||
|
||||
bot.rotation = new Quaternion(-1* limit(y, -30, 30), 0, -1* limit(x, -30, 30), 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public float limit(float input, float min, float max)
|
||||
{
|
||||
return (input < max & input > min) ? input : input < min ? min : max;
|
||||
}
|
||||
|
||||
public float ConvertRadiansToDegrees(float radians)
|
||||
{
|
||||
float degrees = (float) ((180 / Math.PI) * radians);
|
||||
return (degrees);
|
||||
}
|
||||
|
||||
}
|
||||
*/
|
||||
@@ -0,0 +1,13 @@
|
||||
fileFormatVersion: 2
|
||||
guid: ac6468bb4d2734947a923ea42d6d8192
|
||||
timeCreated: 1557262171
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,170 @@
|
||||
//using System;
|
||||
//using UnityEngine;
|
||||
//using System.Collections;
|
||||
//using System.IO.Ports;
|
||||
//using System.Linq;
|
||||
//using UnityEngine.UI;
|
||||
|
||||
//public class Serial3 : MonoBehaviour{
|
||||
|
||||
// public RotationControl bot;
|
||||
// public Walk walk;
|
||||
|
||||
// public float a1, a2, a3, b1, b2, b3;
|
||||
|
||||
// // ~Serial3()
|
||||
// // {
|
||||
// // serialPort.Close();
|
||||
// // serialPort.Dispose();
|
||||
// // }
|
||||
|
||||
// public bool enabled = false;
|
||||
// public bool enabled_send = false;
|
||||
|
||||
// SerialPort serialPort;
|
||||
|
||||
// public void Update()
|
||||
// {
|
||||
// if (serialPort == null)
|
||||
// {
|
||||
// serialPort = new SerialPort(@"\\.\" + "COM7", 230400);
|
||||
|
||||
// try
|
||||
// {
|
||||
// serialPort.NewLine = "\r\n";
|
||||
|
||||
//// serialPort.DataReceived += new SerialDataReceivedEventHandler(DataReceivedHandler);
|
||||
|
||||
// serialPort.Open();
|
||||
// serialPort.DiscardOutBuffer();
|
||||
// serialPort.DiscardInBuffer();
|
||||
|
||||
// enabled = true;
|
||||
// }
|
||||
// catch
|
||||
// {
|
||||
// enabled = false;
|
||||
// }
|
||||
// }
|
||||
|
||||
// if (enabled)
|
||||
// {
|
||||
// serialPort = Serial.Instance.serialPort;
|
||||
// Serial.Instance.enabled = enabled_send;
|
||||
// if (serialPort is SerialPort)
|
||||
// {
|
||||
// string received = serialPort.ReadLine();
|
||||
// string[] data = received.Split(',');
|
||||
|
||||
// foreach (string d in data)
|
||||
// {
|
||||
|
||||
// string[] ch = d.Split('_');
|
||||
|
||||
// int channel = int.Parse(ch.First());
|
||||
// float value = float.Parse(ch.Last());
|
||||
|
||||
// switch (channel)
|
||||
// {
|
||||
// case 0:
|
||||
// a1 = value - 127;
|
||||
// break;
|
||||
// case 1:
|
||||
// a2 = value - 127;
|
||||
// break;
|
||||
// case 2:
|
||||
// a3 = value - 127;
|
||||
// break;
|
||||
// case 3:
|
||||
// b1 = value - 127;
|
||||
// break;
|
||||
// case 4:
|
||||
// b2 = value - 127;
|
||||
// break;
|
||||
// case 5:
|
||||
// b3 = value - 127;
|
||||
// break;
|
||||
// }
|
||||
|
||||
// bot.rotation = new Quaternion(limit(a2, -20, 20), -1 * limit(a3, -20, 20), -1 * limit(a1, -20, 20),
|
||||
// 1);
|
||||
|
||||
// float speed = (float) (Math.Sqrt(b1 * b1 + b2 * b2) / 127) * 3;
|
||||
// walk.walkspeed = speed < 0.5f ? 0 : speed;
|
||||
|
||||
// float rotation = -1 * limit(b3/4, -5, 5);
|
||||
// walk.centerRotation = rotation < 0.5f && rotation > -0.5f ? 0 : rotation;
|
||||
|
||||
// walk.walkDirection = (-1 * ConvertRadiansToDegrees((float) Math.Atan2(b2, b1)));
|
||||
|
||||
// float rotationabs = Math.Abs(walk.centerRotation);
|
||||
// float speed2 = ((6 / 3) * walk.walkspeed) + ((6/5) * rotationabs);
|
||||
// speed2 = speed2 < 6 ? speed2 : 6;
|
||||
// walk.speed2 = (int)(10 - speed2);
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
// private void DataReceivedHandler(object sender, SerialDataReceivedEventArgs e)
|
||||
// {
|
||||
// SerialPort serialPort = (SerialPort)sender;
|
||||
|
||||
// if (enabled)
|
||||
// {
|
||||
// if (serialPort is SerialPort)
|
||||
// {
|
||||
// string received = serialPort.ReadLine();
|
||||
// string[] data = received.Split(',');
|
||||
|
||||
// foreach (string d in data)
|
||||
// {
|
||||
// // if (data == "\r")
|
||||
// // continue;
|
||||
|
||||
// string[] ch = d.Split('_');
|
||||
|
||||
// int channel = int.Parse(ch.First());
|
||||
// float value = float.Parse(ch.Last());
|
||||
|
||||
// switch (channel)
|
||||
// {
|
||||
// case 0:
|
||||
// a1 = value;
|
||||
// break;
|
||||
// case 1:
|
||||
// a2 = value;
|
||||
// break;
|
||||
// case 2:
|
||||
// a3 = value;
|
||||
// break;
|
||||
// case 3:
|
||||
// b1 = value;
|
||||
// break;
|
||||
// case 4:
|
||||
// b2 = value;
|
||||
// break;
|
||||
// case 5:
|
||||
// b3 = value;
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
|
||||
// // bot.rotation = new Quaternion(-1* limit(y, -30, 30), 0, -1* limit(x, -30, 30), 1);
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
// public float limit(float input, float min, float max)
|
||||
// {
|
||||
// input = input / 6;
|
||||
// return (input < max & input > min) ? input : input < min ? min : max;
|
||||
// }
|
||||
|
||||
// public float ConvertRadiansToDegrees(float radians)
|
||||
// {
|
||||
// float degrees = (float)((180 / Math.PI) * radians);
|
||||
// return (degrees);
|
||||
// }
|
||||
|
||||
//}
|
||||
@@ -0,0 +1,13 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 7fa025fa8dcb3a249b67fc0e2fe67b44
|
||||
timeCreated: 1558207463
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -0,0 +1,234 @@
|
||||
using System.Collections;
|
||||
using System.Collections.Generic;
|
||||
using UnityEngine;
|
||||
|
||||
public class Walk : MonoBehaviour
|
||||
{
|
||||
public List<GameObject> legs = new List<GameObject>();
|
||||
private List<Vector3> legsInitial = new List<Vector3>();
|
||||
|
||||
// Use this for initialization
|
||||
void Start()
|
||||
{
|
||||
for (int index = 0; index < legs.Count; ++index)
|
||||
{
|
||||
legsInitial.Add(legs[index].GetComponent<Leg>().point);
|
||||
step2.Add(0);
|
||||
step6.Add(false);
|
||||
//step3.Add(0);
|
||||
//step4.Add(new Vector3(0, 0, 0));
|
||||
step5.Add(new Vector3());
|
||||
|
||||
freeze.Add(false);
|
||||
freeze_y.Add(false);
|
||||
}
|
||||
}
|
||||
|
||||
struct step
|
||||
{
|
||||
public Vector3 pos;
|
||||
public float rotation;
|
||||
public float height;
|
||||
public bool inAir;
|
||||
public bool inRest;
|
||||
|
||||
public step(Vector3 p, float r, float h, bool i, bool ir)
|
||||
{
|
||||
pos = p;
|
||||
rotation = r;
|
||||
height = 0;
|
||||
inAir = i;
|
||||
inRest = ir;
|
||||
}
|
||||
}
|
||||
|
||||
// 1, 3, 5 -= 3
|
||||
|
||||
int offset(int i, int offset, int max)
|
||||
{
|
||||
return i - offset < 0 ? (max - (offset - i)) : i - offset;
|
||||
}
|
||||
|
||||
step[] s = {
|
||||
new step(new Vector3(-1, 0, 0), -1, 0, false, false),
|
||||
|
||||
new step(new Vector3(0, -2, 0), 0, 0, false, false),
|
||||
|
||||
new step(new Vector3(0, 0, 0), 0, 0, false, false), // rest
|
||||
|
||||
new step(new Vector3(1, 0, 0), 1, 0, false, true), // down middle
|
||||
new step(new Vector3(1, 0, 0), 1, 0, false, false),
|
||||
|
||||
new step(new Vector3(0, 0, 0), 0, 0, false, false), // rest
|
||||
|
||||
new step(new Vector3(0, 2, 0), 0, 0, false, false),
|
||||
new step(new Vector3(-1, 0, 0), -1, 0, true, false),
|
||||
};
|
||||
|
||||
public int st = 5;
|
||||
public int speed2 = 30;
|
||||
int speed = 10;
|
||||
|
||||
public float walkDirection = 0;
|
||||
public float centerRotation = 0;
|
||||
|
||||
int size = 8;
|
||||
|
||||
public float walkspeed = 1;
|
||||
|
||||
private List<float> step2 = new List<float>();
|
||||
//private List<float> step3 = new List<float>();
|
||||
//private List<Vector3> step4 = new List<Vector3>();
|
||||
private List<Vector3> step5 = new List<Vector3>();
|
||||
private List<bool> step6 = new List<bool>();
|
||||
|
||||
private List<bool> freeze = new List<bool>();
|
||||
private List<bool> freeze_y = new List<bool>();
|
||||
|
||||
// Update is called once per frame
|
||||
void FixedUpdate()
|
||||
{
|
||||
if (speed <= 0)
|
||||
{
|
||||
if (st >= size - 1)
|
||||
st = 0;
|
||||
else
|
||||
{
|
||||
st++;
|
||||
|
||||
if (centerRotation == 0 && walkspeed == 0)
|
||||
{
|
||||
for (int index = 0; index < legs.Count; index++)
|
||||
{
|
||||
int t = st;
|
||||
if (index == 1 || index == 3 || index == 5)
|
||||
{
|
||||
t = offset(st, 4, size);
|
||||
}
|
||||
|
||||
if (s[t].inRest)
|
||||
{
|
||||
Vector3 lp = legs[index].GetComponent<Leg>().getLocalPos(); // + s[t].pos;
|
||||
//lp.y -= 2;
|
||||
if ((Mathf.Abs(lp.y - (legsInitial[index].y - 2)) < 0.0001f)
|
||||
&& (Mathf.Abs(lp.z - legsInitial[index].z) < 0.0001f)
|
||||
&& (Mathf.Abs(lp.x - legsInitial[index].x) < 1.0001f))
|
||||
{
|
||||
freeze[index] = true;
|
||||
freeze_y[index] = true;
|
||||
}
|
||||
}
|
||||
|
||||
/*else
|
||||
{
|
||||
freeze[index] = false;
|
||||
}*/
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int index = 0; index < legs.Count; index++)
|
||||
{
|
||||
freeze[index] = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int index = 0; index < legs.Count; ++index)
|
||||
{
|
||||
int t = st;
|
||||
|
||||
if (true)
|
||||
{
|
||||
// Hex walk
|
||||
if (index == 1 || index == 3 || index == 5)
|
||||
{
|
||||
t = offset(st, 4, size);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// One leg at a time walk
|
||||
switch (index)
|
||||
{
|
||||
case 0:
|
||||
t = offset(st, 0, size);
|
||||
break;
|
||||
case 1:
|
||||
t = offset(st, 4, size);
|
||||
break;
|
||||
case 2:
|
||||
t = offset(st, 2, size);
|
||||
break;
|
||||
case 3:
|
||||
t = offset(st, 3, size);
|
||||
break;
|
||||
case 4:
|
||||
t = offset(st, 1, size);
|
||||
break;
|
||||
case 5:
|
||||
t = offset(st, 5, size);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!freeze[index])
|
||||
{
|
||||
if (s[t].inAir == false)
|
||||
{
|
||||
step2[index] = (centerRotation * s[t].rotation) / speed2;
|
||||
|
||||
Vector3 t2 = s[t].pos;
|
||||
t2.x *= walkspeed;
|
||||
// rotate x,y around rotation.
|
||||
if (freeze_y[index])
|
||||
{
|
||||
t2.y = 0;
|
||||
}
|
||||
|
||||
step5[index] = (Quaternion.Euler(0, walkDirection, 0) * t2) / speed2;
|
||||
|
||||
step6[index] = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
Vector3 t2 = (legsInitial[index] + s[t].pos) - legs[index].GetComponent<Leg>().point;
|
||||
step5[index] = t2 / speed2;
|
||||
|
||||
step6[index] = false;
|
||||
freeze_y[index] = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
step5[index] = new Vector3(0, 0, 0);
|
||||
step6[index] = false;
|
||||
}
|
||||
}
|
||||
|
||||
speed = speed2;
|
||||
}
|
||||
else
|
||||
{
|
||||
speed--;
|
||||
makestep();
|
||||
}
|
||||
}
|
||||
|
||||
void makestep()
|
||||
{
|
||||
for (int index = 0; index < legs.Count; ++index)
|
||||
{
|
||||
legs[index].GetComponent<Leg>().point += step5[index];
|
||||
if (step6[index])
|
||||
{
|
||||
//legs[index].GetComponent<Leg>().point += step5[index];
|
||||
|
||||
// rotate x,y around rotation.
|
||||
legs[index].GetComponent<Leg>().setWorldPos(
|
||||
Quaternion.Euler(0, step2[index], 0) * legs[index].GetComponent<Leg>().getWorldPos()
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
fileFormatVersion: 2
|
||||
guid: a4266f2a53eea3a43972b07e077212bc
|
||||
timeCreated: 1542307321
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
Reference in new issue
Block a user