//Writer : Junsuk Park
//Function : setAngle()
//Set the speed and angle and send the command to the gimbal.
//desired angle and speed values are decorated by 'ControlCommandStructure'
public static void setAngle(int roll, int pitch, int yaw, ref ControlCommandStructure cCmd)
{
RealtimeDataStructure rData = rtD;
cCmd.setMode(MODE_ANGLE);
cCmd.setAnglePitch(pitch);
cCmd.setAngleRoll(roll);
cCmd.setAngleYaw(yaw);
//setting the speed. Roll, Pitch, Yaw
cCmd.setSpeedPitch(sPitch);
cCmd.setSpeedRoll(sRoll);
cCmd.setSpeedYaw(sYaw);
if (sendCommand(CMD_CONTROL, cCmd.getControlStructure()))
{
//// 호출을 한번 더 해줘야 함.이유: 나중에 각도 읽어올때 쓰레기값 보냄
//if (sendCommand(CMD_REALTIME_DATA))
//{
// Thread.Sleep(20);
// if (port.BytesToRead >= 0)
// {
// byteRead = new byte[port.ReadBufferSize + 1];
// int count = port.Read(byteRead, 0, port.ReadBufferSize);
// rData = parseRealTimeData(byteRead);
// }
//}
}
else
{
Console.WriteLine("Can't send command a message");
return;
}
}
//Writer : Junsuk Park
//Function : getAngle()
//Read the realtime values from the gimbal. Data Structure is 'RealtimeDataStructure'
//If you want to get the angle from the gimbal, Just call rData.getAngle(). Other values is also available. Plz check the class functions.
//
public static float[] getAngle()
{
if (timer != null)
{
timer.Enabled = false;
}
byte[] byteRead;
string[] lines = new string[3];
RealtimeDataStructure rData = rtD;
if (sendCommand(CMD_REALTIME_DATA))
{
Thread.Sleep(50);
if (port.BytesToRead >= 0)
{
byteRead = new byte[port.ReadBufferSize + 1];
int count = port.Read(byteRead, 0, port.ReadBufferSize);
rData = parseRealTimeData(byteRead);
}
float[] angle = rData.getAngle();
float[] frameAngle = rData.getRc_angle();
lines[0] = rData.getRoll().ToString();
lines[1] = rData.getPitch().ToString();
lines[2] = rData.getYaw().ToString();
// String Roll = "("+(1000/herz)*count+","+lines[0]+")"+",";
// String Pitch = "(" + (1000 / herz) * count + "," + lines[1] + ")" + ",";
// String Yaw = "(" + (1000 / herz) * count + "," + lines[2] + ")" + ",";
String RPY = ((1000 / herz) * count).ToString() + "\t" + lines[0].ToString() + "\t" + lines[1].ToString() + "\t" + lines[2].ToString() + "\r\n";
count++;
System.IO.File.AppendAllText(@"C:\Users\Junsuk_Park\Roll.txt", RPY, Encoding.Default);
// System.IO.File.AppendAllText(@"C:\Users\Junsuk_Park\Roll.txt", Roll, Encoding.Default);
// System.IO.File.AppendAllText(@"C:\Users\Junsuk_Park\Pitch.txt", Pitch, Encoding.Default);
// System.IO.File.AppendAllText(@"C:\Users\Junsuk_Park\Yaw.txt", Yaw, Encoding.Default);
if (timer != null)
{
timer.Enabled = true;
}
return(angle);
}
else
{
return(new float[0]);
}
}
public static void setCurrentRealtimeData(RealtimeDataStructure curRealtimeData)
{
realtimeData = curRealtimeData;
}
