public static void GetInputPort()
{
if (UP.isLink)
{
IsRecv = false;
UP.SendDataPacket(UartProtocol.PacketCmd.GetInputPort, 1, data);
}
}
public static void MotorRun(double[] angle)
{
if (!Enable)
{
return;
}
ByteStream b = new ByteStream(30);
int[] p = new int[6];
for (int i = 0; i < 6; i++)
{
int plus = (int)((angle[i] + offsetAngle[i]) / scaleAngle[i]) + plsoffset[i];
b.WriteDWord(plus);
}
byte[] data = b.toBytes();
UP.SendDataPacket(UartProtocol.PacketCmd.LoadPluse, 1, data, 0, data.Length);
IsReady = false;
}
public static void Init(UartProtocol up)
{
SpeedTime = 200;
StartSpeed = 500;
//PlusToDeg = 100000;
Frequency = 10000;
UP = up;
Enable = false;
up.RegisterCmdEvent(UartProtocol.PacketCmd.LoadPluseList, (UartProtocol.PacketStats stats, byte[] buff) => {
switch (stats)
{
case UartProtocol.PacketStats.RecvOK:
ByteStream s = new ByteStream(buff);
switch (s.ReadByte())
{
case 0:
BuffLen = s.ReadWord();
BuffPos = 0;
goto case 1;
case 1:
if (Count <= 0)
{
UP.SendCmdPacket(UartProtocol.PacketCmd.LoadPluseList, (byte)2);
return;
}
s = new ByteStream(53);
s.WriteByte(1);
if (Count >= 2 && BuffLen >= 48)
{
s.WriteWord(BuffPos);
s.WriteByte(48);
for (int i = 0; i < 2; i++)
{
int[] ag = pluslist[Index + i];
for (int j = 0; j < 6; j++)
{
s.WriteDWord(ag[j]);
}
}
Index += 2;
Count -= 2;
BuffPos += 48;
BuffLen -= 48;
UP.SendDataPacket(UartProtocol.PacketCmd.LoadPluseList, 1, s.toBytes());
}
else if (BuffLen >= 24)
{
s.WriteWord(BuffPos);
s.WriteByte(24);
int[] ag = pluslist[Index];
for (int j = 0; j < 6; j++)
{
s.WriteDWord(ag[j]);
}
Index++;
Count--;
BuffPos += 24;
BuffLen -= 24;
UP.SendDataPacket(UartProtocol.PacketCmd.LoadPluseList, 1, s.toBytes());
}
else
{
return;
}
break;
case 2:
break;
case 3:
break;
case 4:
IsReady = true;
break;
}
break;
case UartProtocol.PacketStats.RecvError:
break;
case UartProtocol.PacketStats.RecvTimeOut:
break;
}
});
UartProtocol.UartFunction act = (UartProtocol.PacketStats stats, byte[] buff) => {
switch (stats)
{
case UartProtocol.PacketStats.RecvOK:
if (buff[0] == 0xaa)
{
//System.Threading.Volatile.Write(ref isReady, true);
IsReady = true;
}
break;
case UartProtocol.PacketStats.RecvError:
break;
case UartProtocol.PacketStats.RecvTimeOut:
break;
}
};
up.RegisterCmdEvent(UartProtocol.PacketCmd.LoadPluse, act);
up.RegisterCmdEvent(UartProtocol.PacketCmd.SetupPWMOption, (UartProtocol.PacketStats stats, byte[] buff) => {
switch (stats)
{
case UartProtocol.PacketStats.RecvOK:
IsReady = true;
break;
case UartProtocol.PacketStats.RecvError:
break;
case UartProtocol.PacketStats.RecvTimeOut:
break;
}
});
}
