<?php <?php
// Werte initalisieren
$y_old = 0;
$y_sum = 0;
$y_e_sum = 0;
$y_rc_mittelwert = 0;
$x_old = 0;
$x_sum = 0;
$x_e_sum = 0;
$x_rc_mittelwert = 0;
$gier_old = 0;
// Neutralwert errechen (Normalerweise 100 Durchläufe, hier nur 10)
function adc_neutral($a, $b) {
$wert = 0;
$i = 10;
while($i--) {
$wert += rand($a, $b);
}
return (int)$wert / 10;
}
// Nochmal das gleiche für einen aktuellen Wert... (etwas doppelt gemoppelt)
function get_adc($neutral, $a, $b) {
$wert = 0;
$i = 10;
while($i--) {
$wert += $neutral - rand($a, $b);
}
return array((int)$wert, 10);
}
$y_rc_mittelwert = adc_neutral(100, 250);
// Drei Testdurchläufe
$i = 3;
while ($i--) {
// Initalisieren neuer Werte
list($adc0_value_mux["ADC_GIER"], $adc_count["ADC_GIER"]) = get_adc(adc_neutral(507, 567), 507, 567);
list($adc0_value_mux["ADC_ACCY"], $adc_count["ADC_ACCY"]) = get_adc(adc_neutral(420, 480), 420, 480);
list($adc0_value_mux["ADC_ACCX"], $adc_count["ADC_ACCX"]) = get_adc(adc_neutral(220, 280), 220, 280);
list($adc0_value_mux["ADC_NICK"], $adc_count["ADC_NICK"]) = get_adc(adc_neutral(276, 326), 276, 326);
list($adc0_value_mux["ADC_ROLL"], $adc_count["ADC_ROLL"]) = get_adc(adc_neutral(376, 416), 376, 416);
$rc_ch["RC_NICK"] = adc_neutral(100, 250);
$rc_ch["RC_GAS"] = adc_neutral(100, 250);
$rc_ch["RC_GIER"] = adc_neutral(100, 250);
// ----------- Ab hier Starte die Code Interpretation fc.c - Copyright UlrichRadig.de !!! -------------
$gier = -($adc0_value_mux["ADC_GIER"] / $adc_count["ADC_GIER"]);
$y_out = 0;
$y_gyro_error = 0;
$y_b = $adc0_value_mux["ADC_ACCY"] / $adc_count["ADC_ACCY"];
$y_g = $adc0_value_mux["ADC_NICK"] / $adc_count["ADC_NICK"];
$x_out = 0;
$x_gyro_error = 0;
$x_b = $adc0_value_mux["ADC_ACCX"] / $adc_count["ADC_ACCX"];
$x_g = $adc0_value_mux["ADC_ROLL"] / $adc_count["ADC_ROLL"];
$e = $w = $rc_error = 0;
$motor_value = 10;
$tmp;
$adc_count["ADC_NICK"] = 0;
$adc_count["ADC_ACCY"] = 0;
$adc_count["ADC_ACCX"] = 0;
$adc_count["ADC_ROLL"] = 0;
$adc_count["ADC_GIER"] = 0;
$adc0_value_mux["ADC_NICK"] = 0;
$adc0_value_mux["ADC_ACCY"] = 0;
$adc0_value_mux["ADC_ACCX"] = 0;
$adc0_value_mux["ADC_ROLL"] = 0;
$adc0_value_mux["ADC_GIER"] = 0;
//---------------------------------------------------------------------------
print('$gier = '. $gier . ', $y_b = ' . $y_b . ', $y_g = ' . $y_g .
', $x_b = ' . $x_b . ', $x_g = ' . $x_g . '<br>');
//---------------------------------------------------------------------------
//PD Gyro (GIER)
//e = w - x (Sollwert = 0)
$e = ($gier * 0.5) - ($rc_ch["RC_GIER"] * 0.4);
//---------------------------------------------------------------------------
print(' $e = ' . $e );
//---------------------------------------------------------------------------
//P-Regelpart Kp * e
$gier = 4 * $e;
//---------------------------------------------------------------------------
print(' $gier = ' . $gier );
//---------------------------------------------------------------------------
//D-Regelpart Kd/ta * e (Zeit ta = 0,01s)
$gier += 10 * ($e - $gier_old);
//---------------------------------------------------------------------------
print(' $gier = ' . $gier . "<br>");
//---------------------------------------------------------------------------
$gier_old = $e;
//---------------------------------------------------------------------------
//Gyro Signal integrieren und an AAC abgleichen
$rc_error = $rc_ch["RC_NICK"] - $y_rc_mittelwert;
if (abs($rc_error) > 20) {
if ($rc_ch["RC_NICK"] > $y_rc_mittelwert) $y_rc_mittelwert += 3;
if ($rc_ch["RC_NICK"] < $y_rc_mittelwert) $y_rc_mittelwert -= 3;
} else {
if ($rc_ch["RC_NICK"] > $y_rc_mittelwert) $y_rc_mittelwert++;
if ($rc_ch["RC_NICK"] < $y_rc_mittelwert) $y_rc_mittelwert--;
}
//---------------------------------------------------------------------------
print(' $y_rc_mittelwert = ' . $y_rc_mittelwert);
//---------------------------------------------------------------------------
$y_sum += $y_g;
//---------------------------------------------------------------------------
print(' $y_sum = ' . $y_sum);
//---------------------------------------------------------------------------
$y_gyro_error = $y_sum - ($y_b * 70);
//---------------------------------------------------------------------------
print(' $y_gyro_error = ' . $y_gyro_error );
//---------------------------------------------------------------------------
if($y_gyro_error > 3000) $y_gyro_error = 3000;
if($y_gyro_error < -3000) $y_gyro_error =-3000;
$y_sum -= $y_gyro_error * 0.005;
//---------------------------------------------------------------------------
print(' $y_sum = ' . $y_sum . "<br>");
//---------------------------------------------------------------------------
//----------------------------------------
//PI - Regler
//e = w - x (Sollwert = 0)
$w = ($y_g * 0.3) + ($y_sum * 0.003);
//---------------------------------------------------------------------------
print(' $w = ' . $w);
//---------------------------------------------------------------------------
$e = $w - ($y_rc_mittelwert * 0.2);
//---------------------------------------------------------------------------
print(' $e = ' . $e);
//---------------------------------------------------------------------------
//Integral bilden und begrenzen (I-Anteil)
$y_e_sum += $e;
if ($y_e_sum > 3000) $y_e_sum = 3000;
if ($y_e_sum < -3000) $y_e_sum = -3000;
//---------------------------------------------------------------------------
print(' $y_e_sum = ' . $y_e_sum);
//---------------------------------------------------------------------------
//P-Regler Kp * e + Ki + e_sum_nick1 (P + I-Anteil)
$y_out = 3 * $e + ($y_e_sum * 0.025);
//---------------------------------------------------------------------------
print(' $y_out = ' . $y_out . "<hr>");
//---------------------------------------------------------------------------
//D-Regelpart Kd/ta * e (Zeit ta = 0,01s) (+D Anteil)
//$y_out += 2 * ($e - $y_old);
$y_old = $e;
//----------------------------------------
//Werte mischen
$motor_value = $rc_ch["RC_GAS"] + 70 + $y_out + $gier;
if ($motor_value < 5) $motor_value = 5;
if ($motor_value > 255) $motor_value = 255;
$PWM_Motor[0]= $motor_value;
$motor_value = $rc_ch["RC_GAS"] + 70 - $y_out + $gier;
if ($motor_value < 5) $motor_value = 5;
if ($motor_value > 255) $motor_value = 255;
$PWM_Motor[1]= $motor_value;
//---------------------------------------------------------------------------
print('$rc_ch["RC_GAS"] + 70 = ' . ($rc_ch["RC_GAS"] + 70) .
' $y_out = ' . $y_out .
' $gier = ' . $gier . "<br>");
print(' $PWM_Motor[1] = ' . $PWM_Motor[1] . ' $PWM_Motor[0] = ' . $PWM_Motor[0] . "<hr>");
//---------------------------------------------------------------------------
} ?>
|
Riesen Dank für den Link. 
(
