예제 #1
0
        //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;
            }
        }
예제 #2
0
        //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]);
            }
        }
예제 #3
0
 public static void setCurrentRealtimeData(RealtimeDataStructure curRealtimeData)
 {
     realtimeData = curRealtimeData;
 }