|
trojka
Rookie
Joined: Wed Jun 27, 2007 8:11 am Posts: 2
|
here is my code: i have to say that i know robotc since yesterday, so some or the other thing might be a bit funny...
i also realized that i can use floats (that's why my code is in floats), but that isn't precize enough, because i need a precision of 9 floating-points...
thanks
//*!!Sensor, S1, SensorAlpha, sensorRotation, , !!*//
//*!!Sensor, S2, SensorBeta, sensorReflection, , !!*//
//*!!Sensor, S3, SensorDelta, sensorReflection, , !!*//
//*!!Motor, motorA, Lenk_M, tmotorNormal, !!*//
//*!!Motor, motorC, Antrieb_M, tmotorNormal, !!*//
//*!! !!*//
//*!!Start automatically generated configuration code. !!*//
const tSensors SensorAlpha = (tSensors) S1; //sensorRotation //*!!!!*//
const tSensors SensorBeta = (tSensors) S2; //sensorReflection //*!!!!*//
const tSensors SensorDelta = (tSensors) S3; //sensorReflection //*!!!!*//
const tMotor Lenk_M = (tMotor) motorA; //tmotorNormal //*!!!!*//
const tMotor Antrieb_M = (tMotor) motorC; //tmotorNormal //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration. !!*//
const tMotor Lenk_M = (tMotor) motorA; //tmotorNormal //*!!!!*//
const tMotor Antrieb_M = (tMotor) motorC; //tmotorNormal //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration. !!*//
// miso.c
/*
Quellcode: Lastwagen_mit_Anhaenger: Zustandsregler mit Beobachter
*/
#define A1 -4.85156250857220
static float A[]={1.0, -4.851562509, 9.413555800, -9.131162582, 4.427908335, -0.858739044};
static float Ba[]={0.033597968, -0.098422856, 0.062496805, 0.067153013, -0.096094773, 0.031269842};
static float Bb[]={-0.166866095, 0.489370149, -0.311510536, -0.333499204, 0.478376625, -0.155870951};
static float Bd[]={1.147554847, -3.373648778, 2.158129599, 2.294058448, -3.305684446, 1.07959033};
task main() {
int betaIst, betaSoll;
int deltaIst, deltaSoll;
int lenkIst;
int betaAbweichung = 0;
int deltaAbweichung = 0;
float alpha, beta, delta;
// Abweichung des Fahrzeugs am Ende des Anhängers (= Eingang des Reglers)
float u = 0; // Ausgang des Reglers
float x[] = {0, 0, 0, 0, 0}; // Anfangszustände = 0; (5. Ordnung)
// Sensorwert auslesen, merken und anzeigen
nxtDisplayTextLine (1, "Challo");
wait10Msec(200);
nxtDisplayClearTextLine(1);
deltaSoll = SensorValue[SensorDelta]; // SollWert für hinteren Lichtsensor
betaSoll = SensorValue[SensorBeta];
nxtDisplayTextLine (3, "S3 %3d",SensorValue[SensorDelta]); // ausgeben
wait10Msec(200);
// Solange bis Programm gestoppt
while(true)
{
// Sensoren auslesen
lenkIst = SensorValue[SensorAlpha]; // momentanen Wert des LenkSensors einlesen
betaIst = SensorValue[SensorBeta];
deltaIst = SensorValue[SensorDelta]; // momentanen Wert des hinteren Sensors einlesen
// Abweichung von Trennungslinie ermitteln in RCX-Wert:
deltaAbweichung = deltaSoll - deltaIst;
betaAbweichung = betaSoll - betaIst;
// alpha
alpha = (PI/180)*lenkIst; // da 1/16 Umdrehung etwa 1° entspricht
// beta
//beta = atan(5*(deltaAbweichung - 1)) +695;
beta = (PI/180*20/37)*betaAbweichung;
// delta in [m] ermitteln:
//delta = atan(5*(deltaAbweichung - 1)) +695;
delta = (0.01/5)*deltaAbweichung;
// Zustandsregler in Beobachternormalform (diskret):
/*u = x[4] + alpha*Ba[0] + beta*Bb[0] + delta*Bd[0]; // Output
x[4] = x[3] + alpha*Ba[1] + beta*Bb[1] + delta*Bd[1] - u*A[1]; // Update
x[3] = x[2] + alpha*Ba[2] + beta*Bb[2] + delta*Bd[2] - u*A[2];
x[2] = x[1] + alpha*Ba[3] + beta*Bb[3] + delta*Bd[3] - u*A[3];
x[1] = x[0] + alpha*Ba[4] + beta*Bb[4] + delta*Bd[4] - u*A[4];
x[0] = alpha*Ba[5] + beta*Bb[5] + delta*Bd[5] - u*A[5];
*/
u = x[4] + alpha*Ba[0] + beta*Bb[0] + delta*Bd[0]; // Output
x[4] = x[3] + alpha*Ba[1] + beta*Bb[1] + delta*Bd[1] - u*A[1]; // Update
x[3] = x[2] + alpha*Ba[2] + beta*Bb[2] + delta*Bd[2] - u*A[2];
x[2] = x[1] + alpha*Ba[3] + beta*Bb[3] + delta*Bd[3] - u*A[3];
x[1] = x[0] + alpha*Ba[4] + beta*Bb[4] + delta*Bd[4] - u*A[4];
x[0] = alpha*Ba[5] + beta*Bb[5] + delta*Bd[5] - u*A[5];
motor[Antrieb_M] = 100;
if (u > 100) u = 100;
if (u < -100) u = -100;
if (u > 0)
{
motor[Lenk_M] = u;
if (lenkIst > 30)
//if (alpha < -PI/180*38)
{
motor[Lenk_M] = 0;
}
}
if (u < 0)
{
motor[Lenk_M] = u;
if (lenkIst < -30)
//if (alpha > PI/180*38)
{
motor[Lenk_M] = 0;
}
}
nxtDisplayTextLine (1, "Output %3d",u);
nxtDisplayTextLine (2, "S1 %3d",lenkIst);
nxtDisplayTextLine (3, "S2 %3d",betaIst);
nxtDisplayTextLine (4, "S3 %3d",deltaIst);
nxtDisplayTextLine (5, "Abweich. %3d",deltaAbweichung);
wait1Msec(5);
}
}
|