예제 #1
0
        private static void RunPwm()
        {
            StreamWriter swCal = new StreamWriter("Logs/Calib");
            SerialInput  si    = new SerialInput(path + pwmIn, b57600);
            SerialInput  so    = new SerialInput(path + pwmOut, b57600);

            Thread.Sleep(1000);
            si.WriteNBytes(new byte[] { 1, 1, 1 });
            si.CheckStartOfMessage();
            so.WriteNBytes(new byte[] { 1 });

            byte[] b   = new byte[4];
            byte[] ans = new byte[4];

            PIDManager pid = PIDManager.GetInstance();
            NavManager nav = NavManager.GetInstance();
            GlobalArea ga  = GlobalArea.GetInstance();

            while (true)
            {
                switch (nav.GetCurrentMode())
                {
                case NavManager.Mode.MANUAL:
                    b = si.ReadNBytes(4);
                    so.WriteNBytes(b);
                    Console.WriteLine("entrada: " + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
                    int[] aux = new int[4];
                    aux[0] = (int)Math.Round(b[0] * pid.GetSpanValue(PIDManager.Ctrl.THROTTLE) + pid.GetOffsetValue(PIDManager.Ctrl.THROTTLE));
                    aux[1] = (int)Math.Round(b[1] * pid.GetSpanValue(PIDManager.Ctrl.ROLL) + pid.GetOffsetValue(PIDManager.Ctrl.ROLL));
                    aux[2] = (int)Math.Round(b[2] * pid.GetSpanValue(PIDManager.Ctrl.PITCH) + pid.GetOffsetValue(PIDManager.Ctrl.PITCH));
                    aux[3] = (int)Math.Round(b[3] * pid.GetSpanValue(PIDManager.Ctrl.YAW) + pid.GetOffsetValue(PIDManager.Ctrl.YAW));

                    Console.WriteLine("entrada: " + aux[0] + " " + aux[1] + " " + aux[2] + " " + aux[3]);
                    pid.SetCh(1, b[0]);
                    pid.SetCh(2, b[1]);
                    pid.SetCh(3, b[2]);
                    pid.SetCh(4, b[3]);
                    op.WritePwm(time, b);
                    op.WritePwm(time, b);
                    break;

                case NavManager.Mode.CALIBRATION_THROTTLE:
                    swCal.WriteLine(time + " " + "throt");
                    b = si.ReadNBytes(4);
                    Console.WriteLine("entrada:" + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
                    pid.SetCh(1, b[0]);
                    pid.SetCh(2, b[1]);
                    pid.SetCh(3, b[2]);
                    pid.SetCh(4, b[3]);
                    ans[0] = ThrottleCalib(pid, ga.Adc);
                    ans[1] = b[1];
                    ans[2] = b[2];
                    ans[3] = b[3];
                    so.WriteNBytes(ans);
                    Console.WriteLine("sortida:" + ans[0] + " " + ans[1] + " " + ans[2] + " " + ans[3]);
                    op.WritePwm(time, b);
                    op.WritePwm(time, ans);
                    break;

                case NavManager.Mode.CALIBRATION_ROLL:
                    swCal.WriteLine(time + " " + "roll");
                    b = si.ReadNBytes(4);
                    Console.WriteLine("entrada:" + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
                    pid.SetCh(1, b[0]);
                    pid.SetCh(2, b[1]);
                    pid.SetCh(3, b[2]);
                    pid.SetCh(4, b[3]);
                    ans[0] = b[0];
                    ans[1] = RollCalib(pid, ga.Imu);
                    ans[2] = b[2];
                    ans[3] = b[3];
                    so.WriteNBytes(ans);
                    Console.WriteLine("sortida:" + ans[0] + " " + ans[1] + " " + ans[2] + " " + ans[3]);
                    op.WritePwm(time, b);
                    op.WritePwm(time, ans);
                    break;

                case NavManager.Mode.CALIBRATION_PITCH:
                    swCal.WriteLine(time + " " + "pitch");
                    b = si.ReadNBytes(4);
                    Console.WriteLine("entrada:" + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
                    pid.SetCh(1, b[0]);
                    pid.SetCh(2, b[1]);
                    pid.SetCh(3, b[2]);
                    pid.SetCh(4, b[3]);
                    ans[0] = b[0];
                    ans[1] = b[1];
                    ans[2] = PitchCalib(pid, ga.Imu);
                    ans[3] = b[3];
                    so.WriteNBytes(ans);
                    Console.WriteLine("sortida:" + ans[0] + " " + ans[1] + " " + ans[2] + " " + ans[3]);
                    op.WritePwm(time, b);
                    op.WritePwm(time, ans);
                    break;

                case NavManager.Mode.CALIBRATION_YAW:
                    swCal.WriteLine(time + " " + "yaw");
                    b = si.ReadNBytes(4);
                    Console.WriteLine("entrada:" + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
                    pid.SetCh(1, b[0]);
                    pid.SetCh(2, b[1]);
                    pid.SetCh(3, b[2]);
                    pid.SetCh(4, b[3]);
                    ans[0] = b[0];
                    ans[1] = b[1];
                    ans[2] = b[2];
                    ans[3] = YawCalib(pid, ga.Imu);
                    Console.WriteLine("sortida:" + ans[0] + " " + ans[1] + " " + ans[2] + " " + ans[3]);
                    op.WritePwm(time, b);
                    op.WritePwm(time, ans);
                    so.WriteNBytes(ans);
                    break;

                default:
                    b = si.ReadNBytes(4);
                    Console.WriteLine("entrada:" + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
                    pid.SetCh(1, b[0]);
                    pid.SetCh(2, b[1]);
                    pid.SetCh(3, b[2]);
                    pid.SetCh(4, b[3]);
                    Console.WriteLine("sortida:" + ans[0] + " " + ans[1] + " " + ans[2] + " " + ans[3]);
                    Console.WriteLine();
                    ans[0] = pid.GetCh(1);
                    ans[1] = pid.GetCh(2);
                    ans[2] = pid.GetCh(3);
                    ans[3] = pid.GetCh(4);
                    so.WriteNBytes(ans);
                    op.WritePwm(time, b);
                    op.WritePwm(time, ans);
                    break;
                }
            }
        }
예제 #2
0
		private static void RunPwm()
		{
			StreamWriter swCal = new StreamWriter("Logs/Calib");
			SerialInput si = new SerialInput(path + pwmIn, b57600);
			SerialInput so = new SerialInput(path + pwmOut, b57600);
			Thread.Sleep(1000);
			si.WriteNBytes(new byte[]{1,1,1});
			si.CheckStartOfMessage();
			so.WriteNBytes(new byte[]{1});
			
			byte[] b = new byte[4];
			byte[] ans = new byte[4];
			
			PIDManager pid = PIDManager.GetInstance();
			NavManager nav = NavManager.GetInstance();
			GlobalArea ga = GlobalArea.GetInstance();
			
			while(true)
			{
				switch(nav.GetCurrentMode())
				{
				case NavManager.Mode.MANUAL:
					b = si.ReadNBytes(4);
					so.WriteNBytes(b);
					Console.WriteLine("entrada: " + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
					int[] aux = new int[4];
					aux[0] = (int)Math.Round(b[0] * pid.GetSpanValue(PIDManager.Ctrl.THROTTLE) + pid.GetOffsetValue(PIDManager.Ctrl.THROTTLE));
					aux[1] = (int)Math.Round(b[1] * pid.GetSpanValue(PIDManager.Ctrl.ROLL) + pid.GetOffsetValue(PIDManager.Ctrl.ROLL));
					aux[2] = (int)Math.Round(b[2] * pid.GetSpanValue(PIDManager.Ctrl.PITCH) + pid.GetOffsetValue(PIDManager.Ctrl.PITCH));
					aux[3] = (int)Math.Round(b[3] * pid.GetSpanValue(PIDManager.Ctrl.YAW) + pid.GetOffsetValue(PIDManager.Ctrl.YAW));
						
					Console.WriteLine("entrada: " +  aux[0] + " " + aux[1] + " " + aux[2] + " " + aux[3]); 
					pid.SetCh(1, b[0]);
					pid.SetCh(2, b[1]);
					pid.SetCh(3, b[2]);
					pid.SetCh(4, b[3]);
					op.WritePwm(time, b);
					op.WritePwm(time, b);
					break;
				case NavManager.Mode.CALIBRATION_THROTTLE:
					swCal.WriteLine(time + " " + "throt");
					b = si.ReadNBytes(4);
					Console.WriteLine("entrada:" + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
					pid.SetCh(1, b[0]);
					pid.SetCh(2, b[1]);
					pid.SetCh(3, b[2]);
					pid.SetCh(4, b[3]);
					ans[0] = ThrottleCalib(pid, ga.Adc);
					ans[1] = b[1];
					ans[2] = b[2];
					ans[3] = b[3];
					so.WriteNBytes(ans);
					Console.WriteLine("sortida:" + ans[0] + " " +  ans[1] + " " + ans[2] + " " + ans[3]);
					op.WritePwm(time, b);
					op.WritePwm(time, ans);
					break;
				case NavManager.Mode.CALIBRATION_ROLL:
					swCal.WriteLine(time + " " + "roll");
					b = si.ReadNBytes(4);
					Console.WriteLine("entrada:" + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
					pid.SetCh(1, b[0]);
					pid.SetCh(2, b[1]);
					pid.SetCh(3, b[2]);
					pid.SetCh(4, b[3]);
					ans[0] = b[0];
					ans[1] = RollCalib(pid, ga.Imu);
					ans[2] = b[2];
					ans[3] = b[3];
					so.WriteNBytes(ans);
					Console.WriteLine("sortida:" + ans[0] + " " +  ans[1] + " " + ans[2] + " " + ans[3]);
					op.WritePwm(time, b);
					op.WritePwm(time, ans);
					break;
				case NavManager.Mode.CALIBRATION_PITCH:
					swCal.WriteLine(time + " " + "pitch");
					b = si.ReadNBytes(4);
					Console.WriteLine("entrada:" + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
					pid.SetCh(1, b[0]);
					pid.SetCh(2, b[1]);
					pid.SetCh(3, b[2]);
					pid.SetCh(4, b[3]);
					ans[0] = b[0];
					ans[1] = b[1];
					ans[2] = PitchCalib(pid, ga.Imu);
					ans[3] = b[3];
					so.WriteNBytes(ans);
					Console.WriteLine("sortida:" + ans[0] + " " +  ans[1] + " " + ans[2] + " " + ans[3]);
					op.WritePwm(time, b);
					op.WritePwm(time, ans);
					break;
				case NavManager.Mode.CALIBRATION_YAW:
					swCal.WriteLine(time + " " + "yaw");
					b = si.ReadNBytes(4);
					Console.WriteLine("entrada:" + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
					pid.SetCh(1, b[0]);
					pid.SetCh(2, b[1]);
					pid.SetCh(3, b[2]);
					pid.SetCh(4, b[3]);
					ans[0] = b[0];
					ans[1] = b[1];
					ans[2] = b[2];
					ans[3] = YawCalib(pid, ga.Imu);
					Console.WriteLine("sortida:" + ans[0] + " " +  ans[1] + " " + ans[2] + " " + ans[3]);
					op.WritePwm(time, b);
					op.WritePwm(time, ans);
					so.WriteNBytes(ans);
					break;
				default:
					b = si.ReadNBytes(4);
					Console.WriteLine("entrada:" + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
					pid.SetCh(1, b[0]);
					pid.SetCh(2, b[1]);
					pid.SetCh(3, b[2]);
					pid.SetCh(4, b[3]);
					Console.WriteLine("sortida:" + ans[0] + " " +  ans[1] + " " + ans[2] + " " + ans[3]);
					Console.WriteLine();
					ans[0] = pid.GetCh(1);
					ans[1] = pid.GetCh(2);
					ans[2] = pid.GetCh(3);
					ans[3] = pid.GetCh(4);
					so.WriteNBytes(ans);
					op.WritePwm(time, b);
					op.WritePwm(time, ans);
					break;
				}
			}
		}