Winterzeit ist Programmierzeit. Also habe ich mich mal an die schon lange schlummernde Idee für eine Rampensteuerung bei Encodermotoren für den FtDuino gemacht. Herausgekommen ist dabei eine Erweiterung der FtDuino Bibliothek um die Funktion motor_counter_ramp(...). Die ursprüngliche motor_counter(...) Funktion bleibt unverändert.
Die neue Funktion hat folgende Parameter:
port = Motor M1..M4
mode = LEFT, RIGHT
pwmmin = Anfangsdrehzahl 0..MAX
pwmmax = max. Drehzahl 0..MAX
pwmintervall = nach wieviel Impulsen die Drehzahl jeweils um +/- 1 verändert wird
counter = Zielzähler
Die Beschleunigungs- und Abbremsgeschwindigkeit ist nicht linear sondern erhöht bzw. vermindert sich im Laufe der Rampen.
Die zusätzlichen Variablen sind in der FtDuino.h definiert.
Hier der Code für die Funktion und der geänderte Interrupthandler:
Code: Alles auswählen
volatile uint8_t counter4ramp; // Flags für M1..M4,Bit 0..3 Motorrampe: inaktiv=0/aktiv=1
volatile uint16_t counter_start[4]; // Startwert von C1..C4
volatile uint8_t mx_mode[4]; // Laufrichtung M1..M4
volatile uint8_t mx_pwm[4]; // Start PWM M1..M4
volatile uint8_t mx_status[4]; // Verarbeitungsstatus für Rampe
volatile uint8_t pwm_int[4]; // Counterintervall für nächste PWM Änderung
volatile uint16_t ramp_steps[4]; // Anzahl Steps der Rampen
void Ftduino::motor_counter_ramp(uint8_t port, uint8_t mode, uint8_t pwmmin, uint8_t pwmmax, uint8_t pwmintervall, uint16_t counter) {
// enable counter
counter_set_mode(port - Ftduino::M1 + Ftduino::C1, Ftduino::C_EDGE_RISING);
// load counter so that it passes 0 after "counter" events
counter_val[port - Ftduino::M1] = 0 - counter;
// Counter zu Beginn zum Vergleich
counter_start[port -Ftduino::M1] = 0 - counter;
//Laufrichtung M1..M4
mx_mode[port - Ftduino::M1] = mode;
// Start PWM M1..M4
mx_pwm[port - Ftduino::M1] = pwmmin;
// Counterintervall für nächste PWM Änderung
pwm_int[port - Ftduino::M1] = pwmintervall;
// Status der Rampenverarbeitung initalisieren
mx_status[port - Ftduino::M1] = RAMPUP;
// set flag indicating that the counter is being used for a motor
counter4motor |= (1<< (port - Ftduino::M1));
// Rampenfunktion einschalten
counter4ramp |= (1<< (port - Ftduino::M1));
ramp_steps[port - Ftduino::M1] = (pwmmax - pwmmin) * pwmintervall; // wieviele Steps für Beschleunigungs-/Bremsphase
if (ramp_steps[port - Ftduino::M1] > counter / 2) { // sind die Phasen länger als die Fahrstrecke
ramp_steps[port - Ftduino::M1] = counter/ 2; // Phasen verkürzen
}
// start motor using the given values
motor_set(port, mode, pwmmin);
}
void Ftduino::counter_check_pending(uint8_t counter) {
// check if the current counter is being used for a motor
if(counter4motor & (1 << counter)) {
// get current port state
bool state = counter_get_pin_state(counter);
// Something has happened. Check if it's a rising edge
if(state && !(counter_in_state & (1<<counter))) {
// check if this event (once processed after all debouncing) will
// make the counter roll over to 0
if(counter_val[counter] == 0xffff ) {
// ok, counter will reach zero and a motor is active. Stop it!
motor_set(counter + Ftduino::M1,
(counter4motor & (0x10 << counter))?Ftduino::BRAKE:Ftduino::OFF,
Ftduino::MAX);
// unlink motor and counter
counter4motor &= ~(1 << counter);
// Rampenverarbeitung beendet
counter4ramp &= ~(1 << counter);
}
}
}
// check if there is a "unprocessed event for this counter
// damit wird auch der Nachlauf NACH erreichen des Ziels gezählt
if(counter_event_time[counter]) {
// check if it's longer than the timeout time
if((micros() - counter_event_time[counter]) > 4*COUNTER_FILTER)
counter_timer_exceeded(counter);
}
}
void Ftduino::counter_timer_exceeded(uint8_t c) {
// get current port state
bool state = counter_get_pin_state(c);
// save last accepted state and only count if
// the current state differs from the accepted one.
// otherwise very short spikes may e.g. counted
// check if state of counter input was high and has fallen
if(counter_in_state & (1<<c)) {
// pin state as not changed: Do nothing
if(state) return;
// pin state bas changed: Save new state
else counter_in_state &= ~(1<<c);
}
// check if state of counter input was low and has risen
else {
// pin state as not changed: Do nothing
if(!state) return;
// pin state bas changed: Save new state
else counter_in_state |= (1<<c);
}
// determine mode of current counter port
uint8_t mode = (counter_modes >> (2*c)) & 3;
// count event if it has the desired edge
if((mode == Ftduino::C_EDGE_ANY) ||
(!state && (mode == Ftduino::C_EDGE_FALLING)) ||
( state && (mode == Ftduino::C_EDGE_RISING)) ){
++counter_val[c];
if (counter4ramp & (1 << c)) { // Rampenverarbeitung
if (mx_status[c] == RAMPUP) { //Beschleunigungsphase
if (counter_val[c] < counter_start[c] + ramp_steps[c]) { // Beschleunigungsphase noch aktiv
if (counter_val[c] % pwm_int[c] == 0) { // nächstes Geschwindigkeitsintervall erreicht
++mx_pwm[c]; // Geschwindigkeit erhöhen
motor_set(c + Ftduino::M1, mx_mode[c], mx_pwm[c]); // neue Geschwindigkeit einstellen
};
}
else { // Beschleunigungsphase beenden
mx_status[c] = CONSTSPEED;
};
}
else if (mx_status[c] == CONSTSPEED) { // Konstantdrehzahl
if (ramp_steps[c] >= 65535 - counter_val[c]) { // Konstantdrehzahl beenden
mx_status[c] = RAMPDOWN;
};
}
else if (mx_status[c] == RAMPDOWN) { //Abbremsen
if (counter_val[c] > 65535 - ramp_steps[c]) { // Bremsphase aktiv
if ((counter_val[c] % pwm_int[c]) == 0) { // nächster Schaltpunkt erreicht
--mx_pwm[c]; // Geschwindigkeit verringern
motor_set(c+Ftduino::M1, mx_mode[c], mx_pwm[c]); // neue Geschwindigkeit einstellen
};
};
};
};
};
// this counter timer has been processed
counter_event_time[c] = 0;
}