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