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

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

Привет всем!

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

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

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

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

01void motorstep (int dir, int timeMseq)
02{
03  if (dir == 1)  {
04    digitalWrite(DIR_PIN, LOW);
05  } else {
06    digitalWrite(DIR_PIN, HIGH);
07  }
08  digitalWrite(STEP_PIN, LOW);
09  delayMicroseconds(timeMseq);
10  digitalWrite(STEP_PIN, HIGH);
11  delayMicroseconds(timeMseq);
12}
13 
14 
15void motor5(int maxsteps)
16{
17  int steps = maxsteps+1;
18  double pos= 1;
19  double time=0;
20  double lasttime=0;
21  int dir = 1;
22  long starttime = millis();
23   
24  int lenght = (maxsteps-1)*4;
25  for (int i=0; i< lenght  ; i++ )
26  {
27     
28       if (i < maxsteps*2-1) {
29          pos = (double)pos-(double)1/steps;
30          dir = 1;
31        } else {
32        pos = (double)pos+(double)1/steps;
33        dir = -1;
34        }
35 
36    time = acos(pos);
37     
38    long velocity = 1000*(double)(time-lasttime);
39    motorstep (dir,velocity);
40    lasttime = time;
41     
42    if (debug== 1)  {
43      Serial.print("\n i = ");
44     ....
45    }
46     
47  }
48 
49}

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

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

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

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

 

01int pos = hub*sin((double) time/10000);   
02     
03    if (abs(motorpos-pos) >1 ) {
04         Serial.print("\n to slow ");
05          
06    }
07    if (motorpos!= pos) {
08    if (motorpos < pos ) {
09         motorstep (1,10);
10         motorpos++;
11      } else {
12         motorstep (-1,10);
13         motorpos--;
14       
15      }
16    }

 

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

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

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

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

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

sum = sum+ sin (i);

1double sum = 0;
2  for (int i =0; i < 10000 ; i++)
3  {
4     sum = sum+ sin (i);
5  }
6  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мс....  проверять только в крайних точках? С клавиатурой? опрос еще больше времени займет...