public void clearAnimations()
{
Debug.WriteLine("clearAnimations()");
ToArduino toArduino = new ToArduino { channel = (int)AnimationChannels.ANIMATIONS, command = (int)AnimationCommands.ANIMATIONS_CLEAR };
SendToArduino(toArduino);
}
public ToArduino ToArduino(double scale = 1.0d, bool doRepeat = false)
{
int nFrames = _commandValues.Length / 3;
ToArduino ret = new ToArduino
{
channel = (int)_channel,
command = (int)AnimationCommands.SET_FRAMES + (nFrames << 8) + (doRepeat ? 0x80 : 0),
commandValues = scaleValues(scale)
};
return ret;
}
public void setPinkyMks(int pinkyMks)
{
if (pinkyMks != pinkyMksLast)
{
Debug.WriteLine(string.Format("setPinkyMks: Pinky: {0,4:0} -> {1,4:0} mks", pinkyMksLast, pinkyMks));
ToArduino toArduino = new ToArduino { channel = (int)HandChannels.PINKY, command = (int)HandCommands.SET_VALUE, commandValues = new int[] { pinkyMks } };
SendToArduino(toArduino);
pinkyMksLast = pinkyMks;
}
}
public void setMiddleFingerMks(int middleFingerMks)
{
if (middleFingerMks != middleFingerMksLast)
{
Debug.WriteLine(string.Format("setMiddleFingerMks: MiddleFinger: {0,4:0} -> {1,4:0} mks", middleFingerMksLast, middleFingerMks));
ToArduino toArduino = new ToArduino { channel = (int)HandChannels.MIDDLE_FINGER, command = (int)HandCommands.SET_VALUE, commandValues = new int[] { middleFingerMks } };
SendToArduino(toArduino);
middleFingerMksLast = middleFingerMks;
}
}
public void setIndexFingerMks(int indexFingerMks)
{
if (indexFingerMks != indexFingerMksLast)
{
Debug.WriteLine(string.Format("setIndexFingerMks: IndexFinger: {0,4:0} -> {1,4:0} mks", indexFingerMksLast, indexFingerMks));
ToArduino toArduino = new ToArduino { channel = (int)HandChannels.INDEX_FINGER, command = (int)HandCommands.SET_VALUE, commandValues = new int[] { indexFingerMks } };
SendToArduino(toArduino);
indexFingerMksLast = indexFingerMks;
}
}
public void setElbowAngleMks(int elbowAngleMks)
{
if (elbowAngleMks != elbowAngleMksLast)
{
Debug.WriteLine(string.Format("setElbowAngleMks: Elbow: {0,4:0} -> {1,4:0} mks", elbowAngleMksLast, elbowAngleMks));
ToArduino toArduino = new ToArduino { channel = (int)HandChannels.ELBOW_ANGLE, command = (int)HandCommands.SET_VALUE, commandValues = new int[] { elbowAngleMks } };
SendToArduino(toArduino);
elbowAngleMksLast = elbowAngleMks;
}
}
private void SendToArduino2(ToArduino toArduino1, ToArduino toArduino2)
{
lock (arduinoComm.outputQueue)
{
arduinoComm.outputQueue.Enqueue(toArduino1);
arduinoComm.outputQueue.Enqueue(toArduino2);
}
}
public void setWristTurnMks(int wristTurnMks)
{
if (wristTurnMks != wristTurnMksLast)
{
Debug.WriteLine(string.Format("setWristTurnMks: Wrist: {0,4:0} -> {1,4:0} mks", wristTurnMksLast, wristTurnMks));
ToArduino toArduino = new ToArduino { channel = (int)HandChannels.WRIST_TURN, command = (int)HandCommands.SET_VALUE, commandValues = new int[] { wristTurnMks } };
SendToArduino(toArduino);
wristTurnMksLast = wristTurnMks;
}
}
private void SendToArduino(ToArduino toArduino)
{
arduinoComm.SendToArduino(toArduino);
}
public void setTiltMks(int tiltMks)
{
if (tiltMks != tiltMksLast)
{
Debug.WriteLine(string.Format("setTiltMks: Tilt: {0,4:0} -> {1,4:0} mks", tiltMksLast, tiltMks));
ToArduino toArduino = new ToArduino { channel = (int)AnimationChannels.TILT, command = (int)AnimationCommands.SET_VALUE, commandValues = new int[] { tiltMks } };
SendToArduino(toArduino);
tiltMksLast = tiltMks;
}
}
/// <summary>
/// preferred method for Head pan/tilt control
/// </summary>
/// <param name="panDegreesFromCenter"></param>
/// <param name="tiltDegreesFromCenter"></param>
public void setPanTilt(double panDegreesFromCenter, double tiltDegreesFromCenter)
{
currentPan = panDegreesFromCenter;
double mksPan = PanTiltAlignment.getInstance().mksPan(panDegreesFromCenter);
int panMks = (int)mksPan;
currentTilt = tiltDegreesFromCenter;
double mksTilt = PanTiltAlignment.getInstance().mksTilt(tiltDegreesFromCenter);
int tiltMks = (int)mksTilt;
if (panMks != panMksLast || tiltMks != tiltMksLast)
{
panMksLast = panMks;
tiltMksLast = tiltMks;
Debug.WriteLine("setPanTilt: panMks=" + panMks + " tiltMks=" + tiltMks);
ToArduino toArduino1 = new ToArduino { channel = (int)AnimationChannels.PAN, command = (int)AnimationCommands.SET_VALUE, commandValues = new int[] { panMks } };
ToArduino toArduino2 = new ToArduino { channel = (int)AnimationChannels.TILT, command = (int)AnimationCommands.SET_VALUE, commandValues = new int[] { tiltMks } };
SendToArduino2(toArduino1, toArduino2);
}
}
public void setPanMks(int panMks)
{
if (panMks != panMksLast)
{
Debug.WriteLine(string.Format("setPanMks: Pan: {0,4:0} -> {1,4:0} mks", panMksLast, panMks));
ToArduino toArduino = new ToArduino { channel = (int)AnimationChannels.PAN, command = (int)AnimationCommands.SET_VALUE, commandValues = new int[] { panMks } };
SendToArduino(toArduino);
panMksLast = panMks;
}
}
public void setJawMks(int jawMks)
{
if (jawMks != jawMksLast)
{
Debug.WriteLine(string.Format("setJawMks: Jaw: {0,4:0} -> {1,4:0} mks", jawMksLast, jawMks));
ToArduino toArduino = new ToArduino { channel = (int)AnimationChannels.JAW, command = (int)AnimationCommands.SET_VALUE, commandValues = new int[] { jawMks } };
SendToArduino(toArduino);
jawMksLast = jawMks;
}
}
public void setDefaultAnimations()
{
Debug.WriteLine("setDefaultAnimations()");
ToArduino toArduino = new ToArduino { channel = (int)AnimationChannels.ANIMATIONS, command = (int)AnimationCommands.ANIMATIONS_DEFAULT };
SendToArduino(toArduino);
}
public void setShoulderTurnMks(int turnMks)
{
if (turnMks != shoulderTurnMksLast)
{
Debug.WriteLine(string.Format("setShoulderTurnMks: Turn: {0,4:0} -> {1,4:0} mks", shoulderTurnMksLast, turnMks));
ToArduino toArduino = new ToArduino { channel = (int)HandChannels.SHOULDER_TURN, command = (int)HandCommands.SET_VALUE, commandValues = new int[] { turnMks } };
SendToArduino(toArduino);
shoulderTurnMksLast = turnMks;
}
}
public void setThumbMks(int thumbMks)
{
if (thumbMks != thumbMksLast)
{
Debug.WriteLine(string.Format("setThumbMks: Thumb: {0,4:0} -> {1,4:0} mks", thumbMksLast, thumbMks));
ToArduino toArduino = new ToArduino { channel = (int)HandChannels.THUMB, command = (int)HandCommands.SET_VALUE, commandValues = new int[] { thumbMks } };
SendToArduino(toArduino);
thumbMksLast = thumbMks;
}
}
private void SendToArduino2(ToArduino toArduino1, ToArduino toArduino2)
{
arduinoComm.SendToArduino2(toArduino1, toArduino2);
}
private void SendToArduino(ToArduino toArduino)
{
lock (arduinoComm.outputQueue)
{
Debug.WriteLine("SendToArduino: " + toArduino);
arduinoComm.outputQueue.Enqueue(toArduino);
}
}
public void Open(string comPort, int baudRate = 57600)
{
ComPortName = comPort;
ComBaudRate = baudRate;
//lock (outputQueue)
//{
// outputQueue.Clear();
//}
// create our background worker and support cancellation
worker = new BackgroundWorker();
worker.WorkerSupportsCancellation = true;
worker.DoWork += delegate(object s, DoWorkEventArgs args)
{
Debug.WriteLine("IP: ArduinoComm RunWorker started");
isWorkerRunning = true;
lastLineReceived = DateTime.Now;
int i = 0;
while (!worker.CancellationPending)
{
Debug.WriteLine("********* ArduinoComm Try: " + (++i).ToString());
using (_serialPort = new SerialPort(ComPortName, ComBaudRate, Parity.None, 8, StopBits.One))
{
_serialPort.Handshake = Handshake.RequestToSendXOnXOff; //.None;
_serialPort.Encoding = Encoding.ASCII; // that's only for text read, not binary
_serialPort.NewLine = "\r\n";
_serialPort.ReadTimeout = 1100;
_serialPort.WriteTimeout = 10000;
_serialPort.DtrEnable = false;
_serialPort.RtsEnable = false;
//p.ParityReplace = 0;
try
{
_serialPort.Open();
_serialPort.DataReceived += new SerialDataReceivedEventHandler(serialPort_DataReceived);
Debug.WriteLine("OK: ArduinoComm Open() success!");
while (!worker.CancellationPending)
{
bool needToSleep = true;
lock (outputQueue)
{
while (outputQueue.Count > 0)
{
ToArduino toArduino = outputQueue.Dequeue();
string toWrite = toArduino.ToString();
Debug.WriteLine("========> " + toWrite);
_serialPort.WriteLine(toWrite);
Thread.Sleep(1);
needToSleep = false;
}
}
if (needToSleep)
{
Thread.Sleep(2);
}
}
Debug.WriteLine("IP: ArduinoComm RunWorker Cancellation Pending");
_serialPort.Close();
}
catch (Exception e)
{
Debug.WriteLine(e.GetType().Name + ": " + e.Message);
}
}
Debug.WriteLine("********* ArduinoComm finished try " + i);
}
args.Cancel = true;
Debug.WriteLine("OK: ArduinoComm RunWorker Cancellation sequence completed");
isWorkerRunning = false;
};
worker.RunWorkerCompleted += delegate(object s, RunWorkerCompletedEventArgs args)
{
Debug.WriteLine("OK: ArduinoComm RunWorker Completed");
};
//run the process:
worker.RunWorkerAsync();
}
