Community-Projekte • FtDuino Rampensteuerung für Encodermotoren

CODE:

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