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;
}
}
}
