public static void setAngle(int roll, int pitch, int yaw, ref ControlCommandStructure cCmd)
{
byte[] byteRead = new byte[100];
RealtimeDataStructure rData = rtD;
cCmd.setMode(MODE_ANGLE);
cCmd.setAnglePitch(pitch);
cCmd.setAngleRoll(roll);
cCmd.setAngleYaw(yaw);
//default 30
cCmd.setSpeedPitch(30);
cCmd.setSpeedRoll(30);
cCmd.setSpeedYaw(30);
if (sendCommand(CMD_CONTROL, cCmd.getControlStructure()))
{
Console.WriteLine("Done!");
//호출을 한번 더 해줘야 함. 이유: 나중에 각도 읽어올때 쓰레기값 보냄
if (sendCommand(CMD_REALTIME_DATA))
{
System.Threading.Thread.Sleep(30);
port.Read(byteRead, 0, 100);
rData = parseRealTimeData(byteRead);
float[] angle = rData.getAngle();
}
}
else
{
Console.WriteLine("Can't send command a message");
return;
}
}
static void Main(string[] args)
{
SerialProtocol p = new SerialProtocol();
ControlCommandStructure cCmd = new ControlCommandStructure();
RealtimeDataStructure rData = rtD;
String select;
byte[] byteRead = new byte[100];
int roll, pitch, yaw;
while (true)
{
Console.WriteLine("Input what you want");
Console.WriteLine("1. Check the gimbal angle");
Console.WriteLine("2. Set the gimbal angle");
Console.WriteLine("3. Exit");
switch (select = Console.ReadLine())
{
case "1":
getAngle();
break;
case "2":
Console.WriteLine("Give the angle value.");
roll = Convert.ToInt32(Console.ReadLine());
pitch = Convert.ToInt32(Console.ReadLine());
yaw = Convert.ToInt32(Console.ReadLine());
setAngle(roll, pitch, yaw, ref cCmd);
break;
case "3":
return;
break;
}
}
}
public static float[] getAngle()
{
byte[] byteRead = new byte[100];
RealtimeDataStructure rData = rtD;
if (sendCommand(CMD_REALTIME_DATA))
{
System.Threading.Thread.Sleep(30);
port.Read(byteRead, 0, 100);
rData = parseRealTimeData(byteRead);
float[] angle = rData.getAngle();
Console.WriteLine("Roll: " + rData.getRoll());
Console.WriteLine("Pitch: " + rData.getPitch());
Console.WriteLine("Yaw: " + rData.getYaw());
return(angle);
}
else
{
return(new float[0]);
}
}
public static void setCurrentRealtimeData(RealtimeDataStructure curRealtimeData)
{
realtimeData = curRealtimeData;
}
public static void setCurrentRealtimeData(RealtimeDataStructure curRealtimeData)
{
realtimeData = curRealtimeData;
}