Code:
/*******************************************************************************
*
* Description: Asuro PI-Regler 0.01
*
*****************************************************************************/
/* Bestandteile der Geschwindigkeitssteuerung der Motoren: */
/* Steuerkette zur Pulsbreiten-Modulation */
/* Geschwindikeitsregelkreis */
/* >>>>>>>>> Steuerkette zur Pulsbreiten-Modulation <<<<<<<<<< */
int DDRD = 0x1009; /* Port D Datenrichtung */
int OC1M = 0x100c; /* Ausgabevergleich 1 Maske */
int OC1D = 0x100d; /* Ausgabevergleich 1 Daten */
int TOC1 = 0x1016; /* Ausgabevergleich-Zeitgeber 1*/
int TOC2 = 0x1018; /* Ausgabevergleich-Zeitgeber 2 (inker Motor) */
int TOC3 = 0x101a; /* Ausgabevergleich-Zeitgeber 3 (rechter Motor) */
int TCTL1= 0x1020; /* Zeitgeber-Steuerung 1, 8Bit Register */
/* motor_index: 0 => Linker Motor, 1 => Rechter Motor */
int sign[2] = {1,1}; /* Vorzeichen fuer Motorlaufrichtung */
/* Hilfsfunktionen */
float abs(float arg) /* Absolutwet */
{
if (arg < 0.0) return -arg;
else return arg;
}
int get_sign(float val) /* Vorzeichen des arguments finden */
{
if (val > 0.0) return 1;
else return -1;
}
/* Einschrenkung des wertebereichs von val */
float limit_range(float val, float low, float high)
{
if (val < low) return low;
else if (val > high) return high;
else return val;
}
void init_pwm() /* initia. der Pulsbreiten-Modulation */
{
poke(DDRD,0b00110010); /* Port D; ausgabe 5,4,3,1; eingabe 0 */
poke(OC1M,0b01100000); /* Ausgabevergleich aendert PA5 und PA6 */
poke(OC1D,0b01100000); /* OC1-Vergleich schaltet PA5 und PA6 */
bit_set(TCTL1,0b10100000); /* OC3 schaltet PA5 aus, OC2 schaltet PA6 aus */
pokeword(TOC1,0); /* wenn zeitgeber (TCNT) = 0, OC1 erfolgreich */
pokeword(TOC2,1); /* Mindestzeit fuer OC2 */
pokeword(TOC3,1); /* Mindestzeit fuer OC3 */
}
/* das Vorzeichen wird gesondert behandelt
da wir nur einen Kanal-Encoder haben */
float pwm_motor(float vel, int motor_index)
{
float vel_1;
if (sign[motor_index] > 0) /* wahl der laufrichtung */
bit_set(portd,dir_mask[motor_index]);
else
bit_clear(portd,dir_mask[motor_index]);
vel_1 = limit_range(vel,1.0,99.0);
/* 1 < Pulsbreiten-Modulation - Tastverhaeltnis 100 */
pokeword(TOCx[motor_index], (int) (655.36 * vel_1));
return vel_1;
}
/* Pulsbreiten_Modulationsbefehl hoechster Prioritaet */
void move(float l_vel,float r_vel) /* R,L vel: [-100.0. 100.0] */
{
sign[0] = get_sign(l_vel); /* Laufrichtung */
sign[1] = get_sign(r_vel);
pwm_motor(abs(l_vel),0); /* Pulsbreitenmod. setzen */
pwm_motor(abs(r_vel),1);
}
/* >>>>>>>>>>>>>>>>>>>>>>>>> Regelkreis <<<<<<<<<<<<<<<<<<<<<<<< */
float control_interval = 0.25; /* Regelkreis so oft durchlaufen */
float des_vel_clicks = 0.0; /* Geschw.-Sollwert in Imp. per Zeitinterw. */
float des_bias_clicks = 0.0; /* Gewuenschte Vorspannung in Imp. per Zeitinterw. */
float power[2] = {0.0,0.0}; /* Einschaltbefehl zum Motor */
float integral = 0.0; /* Integral der Geschw.differenz */
float k_integral = 0.10; /* Integraler Fehlerzuwachs */
float k_pro = 1.0; /* Proportionaler Zuwachs */
/* Setzen, Speichern von power */
void alter_power(float error,int motor_index)
{
power[motor_index] = limit_range(power[motor_index] + error, 0.0, 100.0);
pwm_motor(power[motor_index], motor_index);
}
float integrate( float left_vel, float right_vel, float bias)
{
float integral = limit_range((integral + left_vel + bias - right_vel), -1000.0,1000.0);
return integral;
}
void speed_control()
{
int left_vel, right_vel, lweg=0,rweg=0;
float integral_error, left_error, right_error;
while(1)
{
left_vel = get_left_clicks();
right_vel = get_right_clicks();
rweg += right_vel;
lweg += left_vel;
integral_error = k_integral * integrate((float)left_vel, (float)right_vel, des_bias_clicks);
left_error = k_pro * (des_vel_clicks - (float)left_vel - integral_error);
right_error = k_pro * (des_vel_clicks - (float)right_vel + integral_error);
alter_power(left_error, 0);
alter_power(right_error, 1);
printf("vel=%d:%d",right_vel,left_vel);
printf(" weg=%d:%d\n",rweg,lweg);
/* printf(" err=%d:%d\n",righterror,left_error); */
sleep(control_interval);
}
}
float k_clicks = 8.0 / 100.0;
void set_velocity(float vel, float bias)
{
des_vel_clicks = k_clicks * vel;
des_bias_clicks = k_clicks * bias;
sign[0] = get_sign(vel -bias);
sign[1] = get_sign(vel + bias);
}
void start_speed_control()
{
init_pwm();
init_motors();
init_velocity();
get_right_clicks();
get_left_clicks();
start_process(speed_control());
}
Zitieren
Zitat von stochri
Lesezeichen