public bool UpdatePose(int actionId, int poseId, uc.UcServo[] servo, bool allServo, byte headLed,
byte mp3Folder, byte mp3File, byte mp3Vol,
UInt16 servoTime, UInt16 waitTime)
{
data.PoseInfo pi = actionTable.action[actionId].pose[poseId];
if (pi == null)
{
return(false);
}
byte[] servoAngle = new byte[CONST.MAX_SERVO + 1];
byte[] servoLed = new byte[CONST.MAX_SERVO + 1];
for (int i = 1; i <= CONST.MAX_SERVO; i++)
{
if (servo[i].exists && (allServo || servo[i].isSelected))
{
servoAngle[i] = servo[i].angle;
servoLed[i] = servo[i].led;
}
else
{
servoAngle[i] = 0xff;
servoLed[i] = 0x00;
}
}
return(pi.Update(actionId, poseId, servoAngle, servoLed, headLed, mp3Folder, mp3File, mp3Vol, servoTime, waitTime));
}
public void SetPoseSize(UInt16 size)
{
Array.Resize(ref pose, size);
for (UInt16 pId = 0; pId < pose.Length; pId++)
{
pose[pId] = new PoseInfo(actionId, pId);
}
}
public int CopyToEnd(UInt16 poseId)
{
UInt16 size = (UInt16)pose.Length;
int newId = pose.Length;
Array.Resize(ref pose, size + 1);
pose[newId] = new PoseInfo(actionId, poseId);
pose[newId].ReadFromArray(pose[poseId].GetData(), actionId, newId, 0);
pose[newId].poseId = (UInt16)newId;
return(newId);
}
public UInt16 InsertPose(UInt16 poseId)
{
UInt16 size = (UInt16)pose.Length;
Array.Resize(ref pose, size + 1);
for (UInt16 i = size; i > poseId; i--)
{
pose[i] = pose[i - 1];
pose[i].poseId = i;
}
pose[poseId] = new PoseInfo(actionId, poseId);
return(poseId);
}
public bool DownloadActionPose(byte actionId, UInt16 poseId)
{
data.PoseInfo pi = actionTable.action[actionId].pose[poseId];
bool success = false;
byte poseHigh = (byte)((poseId >> 8) & 0xFF);
byte poseLow = (byte)(poseId & 0xFF);
byte[] command = { 0xA9, 0x9A, 0x05, CONST.CMD.GET_ADPOSE, actionId, poseHigh, poseLow, 0, 0xED };
if (V2_SendCommand(command, true, 1000))
{
success = pi.ReadFromArray(receiveBuffer.ToArray(), actionId, poseId);
}
return success;
}
/*
// A9 9A 03 F1 02 F6 ED
public bool ReadFromSPIFFS(byte actionId)
{
byte[] command = { 0xA9, 0x9A, 0x03, CONST.CMD.READSPIFFS, 0, 0xED };
if (!V2_SendCommand(command, true))
{
UpdateInfo(String.Format("Fail reading action {0} from SPIFFS", actionId), Util.InfoType.error);
return false;
}
UpdateInfo(String.Format("Action {0} has been refresh from SPIFFS", actionId));
return true;
}
public bool WriteToSPIFFS()
{
byte[] command = { 0xA9, 0x9A, 0x02, CONST.CMD.WRITESPIFFS, 0, 0xED };
if (!V2_SendCommand(command, true, 2000))
{
UpdateInfo("Fail writing to SPIFFS", Util.InfoType.error);
return false;
}
UpdateInfo("Action data saved to SPIFFS");
return true;
}
*/
public bool SetPose(int actionId, int poseId)
{
UpdateInfo("Please wait......");
data.PoseInfo pi = actionTable.action[actionId].pose[poseId];
V2_SetPose(pi);
/*
* // TODO: how to set delay....
* // There has no need to wait
*/
// int waitTimeMs = 1000 * pi.servoTime / 40 + 10; // Add extra 10ms for safety
int waitTimeMs = pi.servoTime + 10;
Thread.Sleep(waitTimeMs);
UpdateInfo("");
RefreshAngle();
return true;
}
private void btnReadPose_Click(object sender, RoutedEventArgs e)
{
UpdateInfo();
data.PoseInfo pi = ucActionDetail.GetSelectedPose();
if (pi == null)
{
UpdateInfo("没有选定姿势", MyUtil.UTIL.InfoType.alert);
}
else
{
UBT.SetPose(pi.actionId, pi.poseId);
for (int i = 1; i <= CONST.MAX_SERVO; i++)
{
servo[i].SetLED(pi.servoLed[i]);
servo[i].Show();
}
if (activeServo > 0)
{
rbLedNoChange.IsChecked = (pi.servoLed[activeServo] == CONST.LED.NO_CHANGE);
rbLedTurnOn.IsChecked = (pi.servoLed[activeServo] == CONST.LED.TURN_ON);
rbLedTurnOff.IsChecked = (pi.servoLed[activeServo] == CONST.LED.TURN_OFF);
}
rbHeadLedNoChange.IsChecked = (pi.headLed == CONST.LED.NO_CHANGE);
rbHeadLedTurnOn.IsChecked = (pi.headLed == CONST.LED.TURN_ON);
rbHeadLedTurnOff.IsChecked = (pi.headLed == CONST.LED.TURN_OFF);
if (pi.mp3Vol == CONST.AI.STOP_MUSIC_VOL)
{
cbxStopMp3.IsChecked = true;
tbMp3Folder.Text = "";
tbMp3File.Text = "";
tbMp3Vol.Text = "";
}
else
{
cbxStopMp3.IsChecked = false;
tbMp3Folder.Text = (pi.mp3Folder == 0xff ? "" : pi.mp3Folder.ToString());
tbMp3File.Text = (pi.mp3File == 0xff ? "" : pi.mp3File.ToString());
tbMp3Vol.Text = (pi.mp3Vol == 0xff ? "" : pi.mp3Vol.ToString());
}
tbExecTime.Text = pi.servoTime.ToString();
tbWaitTime.Text = pi.waitTime.ToString();
}
}
// A9 9A ?? 23 <id,angle,time>* C8 ED
public void V2_SetPose(data.PoseInfo pi)
{
List <byte> movement = new List <byte>();
int moveCnt = 0;
for (byte id = 1; id <= CONST.MAX_SERVO; id++)
{
if (pi.servoAngle[id] <= CONST.SERVO.MAX_ANGLE)
{
movement.Add(id);
movement.Add(pi.servoAngle[id]);
// movement.Add((byte)(pi.servoTime / CONST.SERVO_TIME_FACTOR));
UInt16 moveTime = pi.servoTime;
movement.Add((byte)(moveTime >> 8));
movement.Add((byte)(moveTime & 0xFF));
moveCnt++;
}
}
if (moveCnt > 0)
{
int moveIdx = 0;
while (moveIdx < moveCnt)
{
int servoCnt = moveCnt - moveIdx;
if (servoCnt > 30)
{
servoCnt = 30;
}
List <byte> command = new List <byte>();
command.Add(0xA9);
command.Add(0x9A);
command.Add(0x00);
command.Add(CONST.CMD.SERVOMOVE);
command.AddRange(movement.GetRange(4 * moveIdx, 4 * servoCnt));
command.Add(0);
command.Add(0xED);
byte[] data = command.ToArray();
data[2] = (byte)((4 * servoCnt) + 2);
// V2_SendCommand(data);
uint expectCnt = (uint)(7 + 4 * servoCnt);
SendRobotCommand(data, CONST.CB.SERVOMOVE, expectCnt);
moveIdx += servoCnt;
}
}
}
// A9 9A ?? 23 <id,angle,time>* C8 ED
public void V2_SetPose(data.PoseInfo pi)
{
List <byte> movement = new List <byte>();
int moveCnt = 0;
for (byte id = 1; id <= CONST.MAX_SERVO; id++)
{
if (pi.servoAngle[id] <= CONST.SERVO.MAX_ANGLE)
{
movement.Add(id);
movement.Add(pi.servoAngle[id]);
movement.Add((byte)(pi.servoTime / CONST.SERVO_TIME_FACTOR));
moveCnt++;
}
}
if (moveCnt > 0)
{
int moveIdx = 0;
while (moveIdx < moveCnt)
{
int servoCnt = moveCnt - moveIdx;
if (servoCnt > 19)
{
servoCnt = 19;
}
List <byte> command = new List <byte>();
command.Add(0xA9);
command.Add(0x9A);
command.Add(0x00);
command.Add(CONST.CMD.SERVOMOVE);
command.AddRange(movement.GetRange(3 * moveIdx, 3 * servoCnt));
command.Add(0);
command.Add(0xED);
byte[] data = command.ToArray();
data[2] = (byte)((3 * servoCnt) + 2);
V2_SendCommand(data);
moveIdx += servoCnt;
}
}
}
private void UpdatePoseInfo(bool allServo)
{
UpdateInfo();
data.PoseInfo pi = ucActionDetail.GetSelectedPose();
if (pi == null)
{
UpdateInfo("没有选定姿势", MyUtil.UTIL.InfoType.alert);
return;
}
bool lastPose = ucActionDetail.LastSelected;
pi.enabled = true;
byte headLed;
if (rbHeadLedTurnOn.IsChecked == true)
{
headLed = CONST.LED.TURN_ON;
}
else if (rbHeadLedTurnOff.IsChecked == true)
{
headLed = CONST.LED.TURN_OFF;
}
else
{
headLed = CONST.LED.NO_CHANGE;
}
byte mp3Folder, mp3File, mp3Vol;
if (cbxStopMp3.IsChecked == true)
{
mp3Folder = 0xFF;
mp3File = 0xFF;
mp3Vol = CONST.AI.STOP_MUSIC_VOL; // Special code for mp3Vol = 0xFE -> Stop play
}
else
{
if (!getMp3Folder(out mp3Folder))
{
return;
}
if (!getMp3File(out mp3File, true))
{
return;
}
if (!getMp3Vol(out mp3Vol))
{
return;
}
}
if (!getActionTime(out UInt16 servoTime, out UInt16 waitTime))
{
return;
}
if (UBT.UpdatePose(pi.actionId, pi.poseId, servo, allServo, headLed, mp3Folder, mp3File, mp3Vol, servoTime, waitTime))
{
UBT.actionTable.action[pi.actionId].CheckPoses();
ucActionList.Refresh();
ucActionDetail.Refresh();
if (lastPose)
{
ActoinList_InsertPose(InsertMode.END);
}
}
}