private async Task GetFullStatus()
{
var data = await dev.ReadBufferDataAsync((byte)Command.GetFullStatus1, 9).ConfigureAwait(false);
var irun = (data[1] >> 4) & 0x0f;
runningCurrent = (RunningCurrent)irun;
var ihold = data[1] & 0x0f;
holdingCurrent = (HoldingCurrent)irun;
var vmax = (data[2] >> 4) & 0x0f;
maxVelocity = (MaxVelocityType)vmax;
var vmin = data[2] & 0x0f;
minVelocityFactorThirtySeconds = vmin == 0 ? 32 : vmin;
var acc = data[3] & 0x0f;
acceleration = (Accel)acc;
accelShape = (data[3] & 0x80) > 0;
var _stepMode = (data[3] >> 5) & 0x03;
stepMode = (StepModeType)_stepMode;
shaftDirection = (data[3] & 0x10) > 0;
await GetPositionStatus().ConfigureAwait(false);
}
public byte[] Serialize()
{
using (MemoryStream stream = new MemoryStream())
{
using (BinaryWriter writer = new BinaryWriter(stream))
{
writer.Write(FrameOffset);
writer.Write(FrameRate);
writer.Write(FrameSize);
writer.Write(StateID);
writer.Write(Speed.Serialize());
writer.Write(Accel.Serialize());
writer.Write(FrameStart);
writer.Write(FrameEnd);
writer.Write(NextAnimation);
writer.Write(NextFrame);
writer.Write(NumStateChanges);
writer.Write(StateChangeOffset);
writer.Write(NumAnimCommands);
writer.Write(AnimCommand);
}
return(stream.ToArray());
}
}
public OdomSubscriberHelper (NodeHandle nh, string topic, Pose pose, Twist twist, Accel acceleration, Header header)
{
pose_ = pose;
twist_ = twist;
acceleration_ = acceleration;
header_ = header;
odom_sub_ = nh.subscribe<Odometry> ( topic, 1, odomCallback );
}
public StateSubscriberHelper (NodeHandle nh, string topic, Pose pose, Twist twist, Accel accel, Header header)
{
pose_ = pose;
twist_ = twist;
accel_ = accel;
header_ = header;
available_ = false;
tf_sub_ = nh.subscribe<TransformStamped> ( topic, 1, tfCb );
}
void Translate(Vector3 dir, ref float elapsedSec)
{
var t = elapsedSec / MoveSec;
var accel = Accel.Evaluate(1f - (t * t));
transform.Translate(dir * DeltaTime * accel * Speed);
elapsedSec += DeltaTime;
}
public override void Enter()
{
base.Enter();
//sets the player's speed to 0
Accel.ResetSpeed();
//sets newPos to the recovery lane
newPos = new Vector3(29.7f, lValues.GetRecHeight(), 2.47f);
Debug.Log("entered lane rec");
}
public void AccelTest1()
{
Accel g = 9.8 * m / s / s;
Accel g2 = 9.8 * ms2;
Velocity vel = g * 5 * sec;
Length len = vel * 5 * sec;
Assert.True(vel.MetresPerSecond == 49.0);
Assert.True(len.Metres == 245.0);
}
/// <summary>
/// Write instruction operands into bytecode stream.
/// </summary>
/// <param name="writer">Bytecode writer.</param>
public override void WriteOperands(WordWriter writer)
{
RayQuery.Write(writer);
Accel.Write(writer);
RayFlags.Write(writer);
CullMask.Write(writer);
RayOrigin.Write(writer);
RayTMin.Write(writer);
RayDirection.Write(writer);
RayTMax.Write(writer);
}
public void updateAndEstimate (Time time, Pose pose, Twist twist, Accel accel)
{
if ( last_pose_ != null )
{
double dt = ( time - last_time_ ).data.toSec ();
double roll, pitch, yaw, last_roll, last_pitch, last_yaw;
tf.net.emQuaternion q = new tf.net.emQuaternion ( pose.orientation );
tf.net.emVector3 v = q.getRPY ();
roll = v.x;
pitch = v.y;
yaw = v.z;
q = new tf.net.emQuaternion ( last_pose_.orientation );
v = q.getRPY ();
last_roll = v.x;
last_pitch = v.y;
last_yaw = v.z;
// fromMsg ( pose.orientation, ref q );
// Matrix3x3 ( q ).getRPY ( roll, pitch, yaw );
//
// fromMsg ( last_pose_.orientation, q );
// Matrix3x3 ( q ).getRPY ( last_roll, last_pitch, last_yaw );
twist.linear.x = ( pose.position.x - last_pose_.position.x ) / dt;
twist.linear.y = ( pose.position.y - last_pose_.position.y ) / dt;
twist.linear.z = ( pose.position.z - last_pose_.position.z ) / dt;
twist.angular.x = differenceWithWraparound ( roll, last_roll ) / dt;
twist.angular.y = differenceWithWraparound ( pitch, last_pitch ) / dt;
twist.angular.z = differenceWithWraparound ( yaw, last_yaw ) / dt;
if ( last_twist_ != null )
{
accel.linear.x = ( twist.linear.x - last_twist_.linear.x ) / dt;
accel.linear.y = ( twist.linear.y - last_twist_.linear.y ) / dt;
accel.linear.z = ( twist.linear.z - last_twist_.linear.z ) / dt;
accel.angular.x = ( twist.angular.x - last_twist_.angular.x ) / dt;
accel.angular.y = ( twist.angular.y - last_twist_.angular.y ) / dt;
accel.angular.z = ( twist.angular.z - last_twist_.angular.z ) / dt;
last_twist_ = twist;
} else
{
last_twist_ = twist;
}
last_pose_ = pose;
} else
{
last_pose_ = pose;
}
last_time_ = time;
}
private void DisplaySmoothed(FileReader.AnalyzedResult result, int smoothFactor)
{
Spo2.Clear();
Bpm.Clear();
Accel.Clear();
Spo2.AddRange(SmoothList(result.Spo2List, smoothFactor));
Bpm.AddRange(SmoothList(result.BpmList, smoothFactor));
//Spo2.AddRange(result.Spo2List);
//Bpm.AddRange(result.BpmList);
Accel.AddRange(result.AccelList);
}
public void FuncKeyCodes()
{
var a = new Accel()
{
Key = "F1"
};
Assert.AreEqual("____:F1", a.GetKeyCode());
a.Key = "F12";
Assert.AreEqual("____:F12", a.GetKeyCode());
a.Key = "F3";
Assert.AreEqual("____:F3", a.GetKeyCode());
}
/// <summary>
/// Calculate number of words to fit complete instruction bytecode.
/// </summary>
/// <returns>Number of words in instruction bytecode.</returns>
public override uint GetWordCount()
{
uint wordCount = 0;
wordCount += RayQuery.GetWordCount();
wordCount += Accel.GetWordCount();
wordCount += RayFlags.GetWordCount();
wordCount += CullMask.GetWordCount();
wordCount += RayOrigin.GetWordCount();
wordCount += RayTMin.GetWordCount();
wordCount += RayDirection.GetWordCount();
wordCount += RayTMax.GetWordCount();
return(wordCount);
}
/// <summary>
/// Write instruction operands into bytecode stream.
/// </summary>
/// <param name="writer">Bytecode writer.</param>
public override void WriteOperands(WordWriter writer)
{
Accel.Write(writer);
RayFlags.Write(writer);
CullMask.Write(writer);
SBTOffset.Write(writer);
SBTStride.Write(writer);
MissIndex.Write(writer);
RayOrigin.Write(writer);
RayTmin.Write(writer);
RayDirection.Write(writer);
RayTmax.Write(writer);
PayloadId.Write(writer);
}
public void ArrowKeyCodes()
{
var a = new Accel()
{
Key = "Left"
};
Assert.AreEqual("____:ArrowLeft", a.GetKeyCode());
a.Key = "Right";
Assert.AreEqual("____:ArrowRight", a.GetKeyCode());
a.Key = "Up";
Assert.AreEqual("____:ArrowUp", a.GetKeyCode());
a.Key = "Down";
Assert.AreEqual("____:ArrowDown", a.GetKeyCode());
}
public void SimpleKeyCodes()
{
var a = new Accel()
{
Key = "A"
};
Assert.AreEqual("____:KeyA", a.GetKeyCode());
a.Key = "Z";
Assert.AreEqual("____:KeyZ", a.GetKeyCode());
a.Key = "0";
Assert.AreEqual("____:Digit0", a.GetKeyCode());
a.Key = "9";
Assert.AreEqual("____:Digit9", a.GetKeyCode());
}
/// <summary>
/// Calculate number of words to fit complete instruction bytecode.
/// </summary>
/// <returns>Number of words in instruction bytecode.</returns>
public override uint GetWordCount()
{
uint wordCount = 0;
wordCount += Accel.GetWordCount();
wordCount += RayFlags.GetWordCount();
wordCount += CullMask.GetWordCount();
wordCount += SBTOffset.GetWordCount();
wordCount += SBTStride.GetWordCount();
wordCount += MissIndex.GetWordCount();
wordCount += RayOrigin.GetWordCount();
wordCount += RayTmin.GetWordCount();
wordCount += RayDirection.GetWordCount();
wordCount += RayTmax.GetWordCount();
wordCount += PayloadId.GetWordCount();
return(wordCount);
}
public void KeyWithModifiers()
{
var a = new Accel()
{
Key = "Ctrl + F1"
};
Assert.AreEqual("C___:F1", a.GetKeyCode());
a.Key = "Alt+Down";
Assert.AreEqual("_A__:ArrowDown", a.GetKeyCode());
a.Key = "Shift + A";
Assert.AreEqual("__S_:KeyA", a.GetKeyCode());
a.Key = "Meta+Z";
Assert.AreEqual("___M:KeyZ", a.GetKeyCode());
a.Key = "Ctrl + Alt + F1";
Assert.AreEqual("CA__:F1", a.GetKeyCode());
a.Key = "Ctrl+Shift + Down";
Assert.AreEqual("C_S_:ArrowDown", a.GetKeyCode());
}
public AccelStamped(std_msgs.Header _header, Accel _accel)
{
header = _header;
accel = _accel;
}
// Constructor Functions
public AccelStamped()
{
header = new std_msgs.Header();
accel = new Accel();
}
/// \brief Reads and interprets data reported by the Wii Remote.
/// \return On success, > 0, < 0 on failure, 0 if nothing has been recieved.
///
/// Wii Remote reads function similarly to a Queue, in FIFO (first in, first out) order.
/// For example, if two reports were sent since the last \c ReadWiimoteData() call,
/// this call will only read and interpret the first of those two (and "pop" it off
/// of the queue). So, in order to make sure you don't fall behind the Wiimote's update
/// frequency, you can do something like this (in a game loop for example):
///
/// \code
/// Wii Remote wiimote;
/// int ret;
/// do
/// {
/// ret = wiimote.ReadWiimoteData();
/// } while (ret > 0);
/// \endcode
public int ReadWiimoteData()
{
byte[] buf = new byte[22];
int status = WiimoteManager.RecieveRaw(hidapi_handle, buf);
if (status <= 0)
{
return(status); // Either there is some sort of error or we haven't recieved anything
}
int typesize = GetInputDataTypeSize((InputDataType)buf[0]);
byte[] data = new byte[typesize];
for (int x = 0; x < data.Length; x++)
{
data[x] = buf[x + 1];
}
if (WiimoteManager.Debug_Messages)
{
Debug.Log("Recieved: [" + buf[0].ToString("X").PadLeft(2, '0') + "] " + BitConverter.ToString(data));
}
// Variable names used throughout the switch/case block
byte[] buttons;
byte[] accel;
byte[] ext = null;
byte[] ir;
switch ((InputDataType)buf[0]) // buf[0] is the output ID byte
{
case InputDataType.STATUS_INFO: // done.
buttons = new byte[] { data[0], data[1] };
byte flags = data[2];
byte battery_level = data[5];
Button.InterpretData(buttons);
bool old_ext_connected = Status.ext_connected;
byte[] total = new byte[] { flags, battery_level };
Status.InterpretData(total);
if (expecting_status_report)
{
expecting_status_report = false;
}
else // We haven't requested any data report type, meaning a controller has connected.
{
SendDataReportMode(last_report_type); // If we don't update the data report mode, no updates will be sent
}
if (Status.ext_connected != old_ext_connected && Type != WiimoteType.PROCONTROLLER)
{
if (Status.ext_connected) // The Wii Remote doesn't allow reading from the extension identifier
{ // when nothing is connected.
Debug.Log("An extension has been connected.");
if (current_ext != ExtensionController.MOTIONPLUS)
{
ActivateExtension();
RequestIdentifyExtension(); // Identify what extension was connected.
}
else
{
ExpectingWiiMotionPlusSwitch = false;
}
}
else
{
if (!ExpectingWiiMotionPlusSwitch)
{
_current_ext = ExtensionController.NONE;
}
Debug.Log("An extension has been disconnected.");
}
}
break;
case InputDataType.READ_MEMORY_REGISTERS: // done.
buttons = new byte[] { data[0], data[1] };
Button.InterpretData(buttons);
if (CurrentReadData == null)
{
Debug.LogWarning("Recived Register Read Report when none was expected. Ignoring.");
return(status);
}
byte size = (byte)((data[2] >> 4) + 0x01);
byte error = (byte)(data[2] & 0x0f);
// Error 0x07 means reading from a write-only register
// Offset 0xa600fa is for the Wii Motion Plus. This error code can be expected behavior in this case.
if (error == 0x07)
{
if (CurrentReadData.Offset != 0xa600fa)
{
Debug.LogError("Wiimote reports Read Register error 7: Attempting to read from a write-only register (" + CurrentReadData.Offset.ToString("x") + "). Aborting read.");
}
CurrentReadData = null;
return(status);
}
// lowOffset is reversed because the Wii Remote reports are in Big Endian order
ushort lowOffset = BitConverter.ToUInt16(new byte[] { data[4], data[3] }, 0);
ushort expected = (ushort)CurrentReadData.ExpectedOffset;
if (expected != lowOffset)
{
Debug.LogWarning("Expected Register Read Offset (" + expected + ") does not match reported offset from Wii Remote (" + lowOffset + ")");
}
byte[] read = new byte[size];
for (int x = 0; x < size; x++)
{
read[x] = data[x + 5];
}
CurrentReadData.AppendData(read);
if (CurrentReadData.ExpectedOffset >= CurrentReadData.Offset + CurrentReadData.Size)
{
CurrentReadData = null;
}
break;
case InputDataType.ACKNOWLEDGE_OUTPUT_REPORT:
buttons = new byte[] { data[0], data[1] };
Button.InterpretData(buttons);
// TODO: doesn't do any actual error handling, or do any special code about acknowledging the output report.
break;
case InputDataType.REPORT_BUTTONS: // done.
buttons = new byte[] { data[0], data[1] };
Button.InterpretData(buttons);
break;
case InputDataType.REPORT_BUTTONS_ACCEL: // done.
buttons = new byte[] { data[0], data[1] };
Button.InterpretData(buttons);
accel = new byte[] { data[0], data[1], data[2], data[3], data[4] };
Accel.InterpretData(accel);
break;
case InputDataType.REPORT_BUTTONS_EXT8: // done.
buttons = new byte[] { data[0], data[1] };
Button.InterpretData(buttons);
ext = new byte[8];
for (int x = 0; x < ext.Length; x++)
{
ext[x] = data[x + 2];
}
if (_Extension != null)
{
_Extension.InterpretData(ext);
}
break;
case InputDataType.REPORT_BUTTONS_ACCEL_IR12: // done.
buttons = new byte[] { data[0], data[1] };
Button.InterpretData(buttons);
accel = new byte[] { data[0], data[1], data[2], data[3], data[4] };
Accel.InterpretData(accel);
ir = new byte[12];
for (int x = 0; x < 12; x++)
{
ir[x] = data[x + 5];
}
Ir.InterpretData(ir);
break;
case InputDataType.REPORT_BUTTONS_EXT19: // done.
buttons = new byte[] { data[0], data[1] };
Button.InterpretData(buttons);
ext = new byte[19];
for (int x = 0; x < ext.Length; x++)
{
ext[x] = data[x + 2];
}
if (_Extension != null)
{
_Extension.InterpretData(ext);
}
break;
case InputDataType.REPORT_BUTTONS_ACCEL_EXT16: // done.
buttons = new byte[] { data[0], data[1] };
Button.InterpretData(buttons);
accel = new byte[] { data[0], data[1], data[2], data[3], data[4] };
Accel.InterpretData(accel);
ext = new byte[16];
for (int x = 0; x < ext.Length; x++)
{
ext[x] = data[x + 5];
}
if (_Extension != null)
{
_Extension.InterpretData(ext);
}
break;
case InputDataType.REPORT_BUTTONS_IR10_EXT9: // done.
buttons = new byte[] { data[0], data[1] };
Button.InterpretData(buttons);
ir = new byte[10];
for (int x = 0; x < 10; x++)
{
ir[x] = data[x + 2];
}
Ir.InterpretData(ir);
ext = new byte[9];
for (int x = 0; x < 9; x++)
{
ext[x] = data[x + 12];
}
if (_Extension != null)
{
_Extension.InterpretData(ext);
}
break;
case InputDataType.REPORT_BUTTONS_ACCEL_IR10_EXT6: // done.
buttons = new byte[] { data[0], data[1] };
Button.InterpretData(buttons);
accel = new byte[] { data[0], data[1], data[2], data[3], data[4] };
Accel.InterpretData(accel);
ir = new byte[10];
for (int x = 0; x < 10; x++)
{
ir[x] = data[x + 5];
}
Ir.InterpretData(ir);
ext = new byte[6];
for (int x = 0; x < 6; x++)
{
ext[x] = data[x + 15];
}
if (_Extension != null)
{
_Extension.InterpretData(ext);
}
break;
case InputDataType.REPORT_EXT21: // done.
ext = new byte[21];
for (int x = 0; x < ext.Length; x++)
{
ext[x] = data[x];
}
if (_Extension != null)
{
_Extension.InterpretData(ext);
}
break;
case InputDataType.REPORT_INTERLEAVED:
if (!ExpectingSecondInterleavedPacket)
{
ExpectingSecondInterleavedPacket = true;
InterleavedDataBuffer = data;
}
else if (WiimoteManager.Debug_Messages)
{
Debug.LogWarning(
"Recieved two REPORT_INTERLEAVED (" + InputDataType.REPORT_INTERLEAVED.ToString("x") + ") reports in a row! "
+ "Expected REPORT_INTERLEAVED_ALT (" + InputDataType.REPORT_INTERLEAVED_ALT.ToString("x") + "). Ignoring!"
);
}
break;
case InputDataType.REPORT_INTERLEAVED_ALT:
if (ExpectingSecondInterleavedPacket)
{
ExpectingSecondInterleavedPacket = false;
buttons = new byte[] { data[0], data[1] };
Button.InterpretData(buttons);
byte[] ir1 = new byte[18];
byte[] ir2 = new byte[18];
for (int x = 0; x < 18; x++)
{
ir1[x] = InterleavedDataBuffer[x + 3];
ir2[x] = data[x + 3];
}
Ir.InterpretDataInterleaved(ir1, ir2);
Accel.InterpretDataInterleaved(InterleavedDataBuffer, data);
}
else if (WiimoteManager.Debug_Messages)
{
Debug.LogWarning(
"Recieved two REPORT_INTERLEAVED_ALT (" + InputDataType.REPORT_INTERLEAVED_ALT.ToString("x") + ") reports in a row! "
+ "Expected REPORT_INTERLEAVED (" + InputDataType.REPORT_INTERLEAVED.ToString("x") + "). Ignoring!"
);
}
break;
}
if (ext == null)
{
_RawExtension = null;
}
else
{
_RawExtension = new ReadOnlyArray <byte>(ext);
}
return(status);
}
public CartesianPoint(Pose pose, Twist velocity, Accel acceleration)
{
this.pose = pose;
this.velocity = velocity;
this.acceleration = acceleration;
}
public CartesianPoint()
{
this.pose = new Pose();
this.velocity = new Twist();
this.acceleration = new Accel();
}
public AccelWithCovariance(Accel accel, double[] covariance)
{
this.accel = accel;
this.covariance = covariance;
}
/// <summary>
/// Read the accelerator.
/// </summary>
/// <param name="lpRes">Address in memory.</param>
internal IntPtr Read(IntPtr lpRes) {
_accel = (Accel)Marshal.PtrToStructure(lpRes, typeof (Accel));
return new IntPtr(lpRes.ToInt32() + Marshal.SizeOf(_accel));
}
/// <summary>
/// Read the accelerator.
/// </summary>
/// <param name="lpRes">Address in memory.</param>
internal IntPtr Read(IntPtr lpRes)
{
_accel = (Accel)Marshal.PtrToStructure(lpRes, typeof(Accel));
return(new IntPtr(lpRes.ToInt32() + Marshal.SizeOf(_accel)));
}
void registerAccel (Accel accel)
{
accel_ = new AccelerationHandle ( this, accel );
}
private void Initialize()
{
pos = new Position( 0.0f, 0.0f, 0.0f );
dir = new Direction( 0.0f, 0.0f, 0.0f );
vel = new Velocity( 0.0f, 0.0f, 0.0f );
acl = new Accel( 0.0f, 0.0f, 0.0f );
}
/// <summary>
/// Requests a reading from the sensor and updates its data properties with the gathered values.
/// </summary>
/// <returns>An awaitable Task</returns>
/// <remarks>
/// Note that when #AutoUpdateWhenPropertyRead is `true` (which it is, by default), this method is implicitly
/// called when any sensor data property is read from --- there's no need to call this method unless you set
/// AutoUpdateWhenPropertyRead to `false`.
///
/// Unless otherwise noted, this method updates all sensor data simultaneously, which can often lead to more efficient
/// bus usage (as well as reducing USB chattiness).
/// </remarks>
public async Task UpdateAsync()
{
await Accel.UpdateAsync().ConfigureAwait(false);
await Mag.UpdateAsync().ConfigureAwait(false);
}
