Управляем шаговым мотором.Цель -синус. Ищу библиотеки.

achest
achest аватар
Offline
Зарегистрирован: 01.10.2012

Привет всем!

имеется шаговый мотор + драйвер Toshiba  TB6560  в китайском варианте. 3 входа: Шаг, Dir, Enable.

Цель: Заставить двигаться мотор по определенной траектории, близкой к синусу, но не синус (параметры) !  т.е. 300 шагов налево, 300 шагов на право, плавно стартутую, плавно торможу. Скорость: 1000 шагов в секунду или больше.

Ищу библиотеку, в которой реализовано подобное. в  библиотеке от http://www.airspayce.com ничего подобного нет, там только X шагов с разгоном/плавным торможением.

Что я сделал: Функция Motor5 запускается из Loop

void motorstep (int dir, int timeMseq)
{
  if (dir == 1)  {
    digitalWrite(DIR_PIN, LOW);
  } else {
    digitalWrite(DIR_PIN, HIGH);
  }
  digitalWrite(STEP_PIN, LOW);
  delayMicroseconds(timeMseq);
  digitalWrite(STEP_PIN, HIGH);
  delayMicroseconds(timeMseq);
}


void motor5(int maxsteps)
{
  int steps = maxsteps+1;
  double pos= 1;
  double time=0;
  double lasttime=0;
  int dir = 1;
  long starttime = millis();
  
  int lenght = (maxsteps-1)*4;
  for (int i=0; i< lenght  ; i++ )
  {
    
       if (i < maxsteps*2-1) {
          pos = (double)pos-(double)1/steps;
          dir = 1;
        } else { 
        pos = (double)pos+(double)1/steps;
        dir = -1;
        }

    time = acos(pos);
    
    long velocity = 1000*(double)(time-lasttime);
    motorstep (dir,velocity);
    lasttime = time;
    
    if (debug== 1)  {
      Serial.print("\n i = "); 
     ....
    }
    
  } 

}



Принцип:  Шаг нам известен. Подсчитаем сколько времени должно пройти до следующего шага и скажем ему, что бы он выдал сигнал на это время (делал шаг столько времени).

Все работает, но только с синусом. для моей функции обратную функцию считать проблематично.

Попробовал второй вариант, когда мотор как бы следит за требуемой позицией... Ничего не получается.

Если не писать в Debug в Serial порт то еще  работает, а если писать, то просто финиш.. Нужно как-то сократить время, процессора не хватает...

 


int pos = hub*sin((double) time/10000);    
    
    if (abs(motorpos-pos) >1 ) {
         Serial.print("\n to slow ");
         
    }
    if (motorpos!= pos) {
    if (motorpos < pos ) {
         motorstep (1,10);
         motorpos++;
      } else {
         motorstep (-1,10);
         motorpos--;
      
      }
    }

 

Michal
Michal аватар
Offline
Зарегистрирован: 26.04.2013

зачем вычисляете синус. Протабулируйте с приемлемым шагом

achest
achest аватар
Offline
Зарегистрирован: 01.10.2012
зачем вычисляете синус. Протабулируйте с приемлемым шагом

Понимаешь, у меня не синус. У меня там нелинейность+ гармоники + частотная модуляция.

Все варианты заранее подсчитать не возможно.

sum = sum+ sin (i);

double sum = 0;
  for (int i =0; i < 10000 ; i++)
  {
     sum = sum+ sin (i);
  }
  int endtime = millis();

1299! т.е. на синус+ 2 умножение+сложение порядка 100 микросекунд...

 

Michal
Michal аватар
Offline
Зарегистрирован: 26.04.2013

ну вы хотите с меги выжать на вычислениях с точкой быстродействие, тогда уж лучше проц сменить. Вон Due возьмите, все таки АРМ.

achest
achest аватар
Offline
Зарегистрирован: 01.10.2012

Написал я 2 скетч... считает синус по 360 точкам... 260 микросекунд. т.е. ускорение в 4 раза! Спасибо за идею,как я понимаю надо полностью избавиться от плавающей точки, тогда все будет значительно быстрее... Пошел ка я спать..

Я все еще ищу библиотеку, как это делать. не хочу делать велосипед.

 

Michal
Michal аватар
Offline
Зарегистрирован: 26.04.2013

еще как вариант, посмотреть чо там компилятор выдает и может основные части на асме написать... ну это когда ваще надо мкс выдавливать с контроллера

achest
achest аватар
Offline
Зарегистрирован: 01.10.2012

Я тут всю ночь думал и пришел к выводу, что можно на 100% отказаться от плавающей точки, расчета синуса и тп. Кроме того мне надо написать 2 функции : одна вперед, другая назад. В этом случае я избегаю еще 3 проверки и постоянное дегание за выход, отвечающий за направление.

Другими словами: Ускорение минимум в 5 раз только за счет синуса. Еще в 2 раза за счет только одного  delayMicroseconds(timeMseq);.

 

Если я откажусь от фазовой модуляции, то всегда могу подсчитать обратную функцию. другими словами даже с актуальным кодом(без оптимизации) скорости процессора хватает...

Что делать с концевыми датчиками? Он там выдает сигнал 1 раз в 20мс....  проверять только в крайних точках? С клавиатурой? опрос еще больше времени займет...