/// <summary>
/// Main Method of the UAV Control Logic
/// Will be called when the System is fully loaded and running
/// If this Mehthod is completed the Program will exit :-(
/// </summary>
public override void run()
{
initialised = true; // Wir sind fertig mit dem Laden und Starten unser eigentliches Programm daher erlauben wir auch den Empfang von Daten der Bodenstation
servo1.ConnectHardware();
//// public ParameterPID (UAVParameter pG, UAVParameter iG, UAVParameter dG, UAVParameter pv, UAVParameter diff, UAVParameter output,UAVParameter sp)
//PID_Höhe = new PID(kp_Höhe, ki_Höhe, kd_Höhe, lagesensor["theta"], lagesensor ["gyroalphastrich"],PID_Out_Höhe, sp_Höhe);
//PID_Quer = new PID(kp_Quer, ki_Quer, kd_Quer, lagesensor["phi"], lagesensor ["gyrogammastrich"],PID_Out_Quer, sp_Quer);
////starte 100Hz Schleife //keine Ahnung wie das geht
// lagesensor.DataRecieved+=new UAVCommons.UAVStructure.ValueChangedHandler(Ausgaberoutine);
// servo1.DoubleValue = -100;
counter = 0;
do // läuft immer durch damit run nicht beendet und somit das Programm beendet wird
{
Thread.Sleep(10);
// Console.WriteLine("Counter: " + servo1.DoubleValue);
counter++;
// servo1.DoubleValue++;
servo1.DoubleValue = counter;
servo2.DoubleValue = counter;
servo3.DoubleValue = counter;
servo4.DoubleValue = counter;
servo5.DoubleValue = counter;
PWM.UpdateServos();
Console.WriteLine("throttle" + this.kd_Höhe.MaxDoubleValue);
// servo1.DoubleValue = counter;
// servo1.DoubleValue = counter;
if (counter == 99)
{
counter = 0;
}
} while (running == true);
}
public void Ausgaberoutine(UAVSingleParameter param, bool isremote) //100Hz Schleife //keine Ahnung wie das geht ,...was ist die max aktualisierungsrate von Maestro und PWM-switch ???
{
System.Diagnostics.Stopwatch w2 = new System.Diagnostics.Stopwatch();
w2.Start();
try{
// Console.WriteLine(watch.ElapsedMilliseconds+" ms Phi "+lagesensor["phi"].DoubleValue+" Theta"+lagesensor["theta"].DoubleValue+" psi"+lagesensor["psi"].DoubleValue);
//counter ++;
// sp_Quer.DoubleValue = empfängerusb ["Axis0"].DoubleValue/10;//bin mir nicht sicher welche Achse
// sp_Höhe.DoubleValue = empfängerusb ["Axis1"].DoubleValue/10; //bin mir nicht sicher welche Achse
sp_Quer.DoubleValue = phi_rolerate.DoubleValue; //(empfängerusb ["Axis0"].DoubleValue+18)/2;
sp_Höhe.DoubleValue = theta_rolerate.DoubleValue; //(empfängerusb ["Axis1"].DoubleValue+03)/3;
NewSeitePV.DoubleValue = ComputePV((UAVParameter)lagesensor["psi"], sp_Seite);
sp_Seite.DoubleValue = 0; // sp_Seite.DoubleValue + psi_rolerate.DoubleValue/100;//(empfängerusb ["Axis2"].DoubleValue+18)/20;
if (sp_Seite.DoubleValue < 0)
{
sp_Seite.DoubleValue = sp_Seite.DoubleValue + 360;
}
if (sp_Seite.DoubleValue > 360)
{
sp_Seite.DoubleValue = sp_Seite.DoubleValue - 360;
}
PIDS_Höhe.PIDS_Calc();
PIDS_Quer.PIDS_Calc();
PIDS_Seite.PIDS_Calc();
// Wie gehe ich sicher dass die PIDS fertig mit der Berechnung sind bevor ich die output-Werte in Folge zusammenzähle?????
// (möchte keine alten Werte zusammenzählen)
servo1.DoubleValue = Tools.Limit((output_Quer.DoubleValue - output_Höhe.DoubleValue), -80, 80);
servo2.DoubleValue = Tools.Limit((output_Quer.DoubleValue + output_Höhe.DoubleValue), -80, 80);
servo3.DoubleValue = Tools.Limit((SteuerMotorLeistung.DoubleValue) * 1.8 - 80 + output_Seite.DoubleValue, -100, 100); //mal 1.1 //(empfängerusb ["Axis2"].DoubleValue+18)/2
servo4.DoubleValue = Tools.Limit((SteuerMotorLeistung.DoubleValue) * 1.8 - 80 - output_Seite.DoubleValue, -100, 100);
//((empfängerusb ["Axis4"].DoubleValue)+90)*-0.8+110
// HauptMotorLeistung.Value = (int)Tools.ScaleValue(throttle.DoubleValue,-100,100,-80,100);
// Console.WriteLine(HauptMotorLeistung.DoubleValue);
servo5.DoubleValue = HauptMotorLeistung.DoubleValue - HauptMotorDiff.DoubleValue;
servo6.DoubleValue = HauptMotorLeistung.DoubleValue + HauptMotorDiff.DoubleValue;
PWM.UpdateServos();
// Console.WriteLine("HauptMotor Left "+(HauptMotorLeistung.DoubleValue - HauptMotorDiff.DoubleValue)+" Right"+(HauptMotorLeistung.DoubleValue + HauptMotorDiff.DoubleValue)+"Differenz: "+HauptMotorDiff.DoubleValue);
//servo5.DoubleValue = (empfängerusb ["Axis4"].DoubleValue)*-1.8-80;
// servo6.DoubleValue = (empfängerusb ["Axis4"].DoubleValue)*-1.8-80;
}catch (Exception ex) {
Console.WriteLine(ex.Message + " " + ex.StackTrace.ToString());
}finally{
if (w2.ElapsedMilliseconds > 10)
{
Console.WriteLine("too long" + w2.ElapsedMilliseconds);
}
}
// servo5.DoubleValue = Tools.Limit((((empfängerusb ["Axis4"].DoubleValue)+90)*-1+100)*1 , 100, -100);
//- (empfängerusb ["Axis2"].DoubleValue+30)/2
// + (empfängerusb ["Axis2"].DoubleValue+30)/2
//+ lagesensor ["gyrobetastrich"].DoubleValue
//- lagesensor ["gyrobetastrich"].DoubleValue
// Warum sollte ich Servo-Werte limitieren ?? -> sind doch bereits limitiert !! -> Limits kommen weg
// Wie gehe ich sicher dass die Umrechnung in PWM genau nach dem Setzen der Servo-Werte passiert????????
//Console.WriteLine("Servo3=" + servo3.DoubleValue +" Servo4=" + servo4.DoubleValue);
//Console.WriteLine("Wert Servo1:"+Tools.Limit ((output_Quer.DoubleValue - output_Höhe.DoubleValue) , 80, -80));
//
// servo3.DoubleValue =(empfängerusb ["Axis4"].DoubleValue +90)*-0.8+110 - output_Quer.DoubleValue ;
// servo4.DoubleValue =(empfängerusb ["Axis4"].DoubleValue +90)*-0.8+110 + output_Quer.DoubleValue ;
}
