Stikked @ k4be.pl

Bez tytułu

z Putrid Crow, 6 lata temu, napisane w Plain Text, wyświetlone 641 razy.
URL https://pastebin.k4be.pl/view/d51da07d Udostępnij
Pobierz wklejkę lub Pokaż surowy tekst
  1. void TIM1_CC_IRQHandler(void) {
  2.     float max_dv, tiny_dp, vel_req;
  3.     int32_t pos_err;
  4.  
  5.     TIMER_LED_ON;
  6.  
  7.         TIM1->SR &= ~TIM_SR_CC1IF;      // clear capture/compare 1 event flag
  8.  
  9.         m1.active = 0;
  10.         if (m1.current_period == 0) {
  11.                 // initiate movement
  12.                 // Calculate initial period needed for the calculation of acceleration
  13.                 m1.current_period = stepper_setf(1.0f / sqrtf(2.0f / (float) STEPPER_MAX_A));
  14.                 // No movement in this iteration
  15.                 m1.dir = 0;
  16.                 if(m1.pos_cmd - m1.current_pos > 0) {
  17. //                      m1.dir = 1;
  18.                         STEPPER_SET_DIR_POS;
  19.                 } else if(m1.pos_cmd - m1.current_pos < 0){
  20. //                      m1.dir = -1;
  21.                         STEPPER_SET_DIR_NEG;
  22.                 }
  23.  
  24.                 // Set the counter to CCR1 value, so the first timer loop will not cause compare match
  25.                 TIM1->CNT = TIM1->CCR1;
  26.                 TIM1->EGR = TIM_EGR_UG;         // Generate update event (reload registers)
  27.                 TIM1->SR &= ~TIM_SR_UIF;        // clear update event flag
  28.                 TIM1->CCER |= TIM_CCER_CC1E;    // Enable PWM output
  29.                 TIM1->CR1 |= TIM_CR1_CEN;       // Count up, Enable timer
  30.         }
  31.     m1.current_pos += m1.dir;
  32.         /* compute max change in velocity per servo period */
  33.         max_dv = STEPPER_MAX_A * (float) m1.current_period / (float) CPU_CLK;
  34.         /* calculate desired velocity */
  35.         if (m1.enabled) {
  36.                 /* planner enabled, request a velocity that tends to drive
  37.                    pos_err to zero, but allows for stopping without position
  38.                    overshoot */
  39.                 pos_err = m1.pos_cmd - m1.current_pos;
  40.  
  41.                 /* positive and negative errors require some sign flipping to
  42.                    avoid sqrt(negative) */
  43.                 if (pos_err > 0) {
  44.                         vel_req = -max_dv + sqrtf(2.0f * (float) STEPPER_MAX_A * (float) pos_err + max_dv * max_dv);
  45.                         /* mark planner as active */
  46.                         m1.active = 1;
  47.                 } else if (pos_err < 0) {
  48.                         vel_req = max_dv - sqrtf(-2.0f * (float) STEPPER_MAX_A * (float) pos_err + max_dv * max_dv);
  49.                         /* mark planner as active */
  50.                         m1.active = 1;
  51.                 } else {
  52.                         /* within 'tiny_dp' of desired pos, no need to move */
  53.                         vel_req = 0.0;
  54.                 }
  55.         } else {
  56.                 /* planner disabled, request zero velocity */
  57.                 vel_req = 0.0;
  58.                 /* and set command to present position to avoid movement when
  59.                    next enabled */
  60.                 m1.pos_cmd = m1.current_pos;
  61.         }
  62.         /* limit velocity request */
  63.         if (vel_req > m1.vmax) {
  64.                 vel_req = m1.vmax;
  65.         } else if (vel_req < -m1.vmax) {
  66.                 vel_req = -m1.vmax;
  67.         }
  68.         /* ramp velocity toward request at accel limit */
  69.         if (vel_req > m1.current_vel + max_dv) {
  70.                 m1.current_vel += max_dv;
  71.         } else if (vel_req < m1.current_vel - max_dv) {
  72.                 m1.current_vel -= max_dv;
  73.         } else {
  74.                 m1.current_vel = vel_req;
  75.         }
  76.  
  77.     // TODO poprawić - gubi pierwszy(?) krok
  78.     if(m1.current_vel > 0) {
  79.         m1.dir = 1;
  80.         STEPPER_SET_DIR_POS;
  81.     } else if(m1.current_vel < 0) {
  82.         m1.dir = -1;
  83.         STEPPER_SET_DIR_NEG;
  84.     }
  85.     /* check for still moving */
  86.     if(m1.current_vel != 0.0) {
  87.                 /* yes, mark planner active */
  88.                 m1.active = 1;
  89.     }
  90.     /* integrate velocity to get new position */
  91.     //m1.current_pos += m1.current_vel * m1.current_period;
  92.     if(m1.active == 0) {
  93.         stepper_stop();
  94.     } else {
  95.         m1.current_period = stepper_setf(fabsf(m1.current_vel));
  96.     }
  97.     TIMER_LED_OFF;
  98.  
  99. }

odpowiedź "Bez tytułu"

Tutaj możesz odpowiedzieć na wklejkę z góry

captcha
Wspierane przez Stikked