/// <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);
}
}
private void SendToArduino2(ToArduino toArduino1, ToArduino toArduino2)
{
lock (arduinoComm.outputQueue)
{
arduinoComm.outputQueue.Enqueue(toArduino1);
arduinoComm.outputQueue.Enqueue(toArduino2);
}
}
private void SendToArduino(ToArduino toArduino)
{
lock (arduinoComm.outputQueue)
{
Debug.WriteLine("SendToArduino: " + toArduino);
arduinoComm.outputQueue.Enqueue(toArduino);
}
}
public void setDefaultAnimations()
{
Debug.WriteLine("setDefaultAnimations()");
ToArduino toArduino = new ToArduino {
channel = (int)AnimationChannels.ANIMATIONS, command = (int)AnimationCommands.ANIMATIONS_DEFAULT
};
SendToArduino(toArduino);
}
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), // but 7 will be interpreted by Arduino side
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 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 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 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;
}
}
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;
}
}
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;
}
}
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 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 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;
}
}
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 SendToArduino2(ToArduino toArduino1, ToArduino toArduino2)
{
Console.WriteLine("*** SendToArduino2(): " + toArduino1 + " : " + toArduino2);
_serialPort.WriteLine(toArduino1.ToString());
_serialPort.WriteLine(toArduino2.ToString());
}
private void SendToArduino2(ToArduino toArduino1, ToArduino toArduino2)
{
arduinoComm.SendToArduino2(toArduino1, toArduino2);
}
public void SendToArduino(ToArduino toArduino)
{
Console.WriteLine("*** SendToArduino(): " + toArduino);
_serialPort.WriteLine(toArduino.ToString());
}
private void SendToArduino(ToArduino toArduino)
{
arduinoComm.SendToArduino(toArduino);
}
