Mega 3 датчика, 2 потенциометра и 1 шаговый двигатель

Ищу исполнителя
1

Здравствуйте, на Mega 2650 требуется вот такой функционал:1) при срабатывании 1го датчика, через паузу (регулируемую потенциометром) двигатель должен начинать крутиться, до момента срабатывания второго датчика.2) Скорость двигателя должна регулироваться потенциометром в настраиваемом диапазоне.3) При срабатывании датчика 3 ардуина должна отправлять сообщение “+1” 4) при получении сообщения “brak” на 46 пин надо подать высокий уровень, время подачи сигнала должно настраиваться при отладке. sistema164@mail.ru

#include "AccelStepper.h" // #include <AccelStepper.h>

// раскомментировать для вывода отладочных сообщений
#define DEBUG

#ifndef MY_BUILD
  
  /////////////////////////////////////////////////////////
  // НАСТРОЙКИ НАЧАЛО

  // MOTORS
  const uint8_t   STEPPER_INTERFACE   = AccelStepper::FULL2WIRE;
  
  const uint8_t   PIN_MOTOR1_STEP     = 4;
  const uint8_t   PIN_MOTOR1_DIR      = 5;
  const uint8_t   PIN_MOTOR1_EN       = -1;

  const uint8_t   PIN_MOTOR2_STEP     = 2;
  const uint8_t   PIN_MOTOR2_DIR      = 3;
  const uint8_t   PIN_MOTOR2_EN       = -1;
  
  // SPEED MOTOR1  
  const float     M1_SPEED_MIN        = 5000;
  const float     M1_SPEED_MAX        = 12000;  
  const uint32_t  M1_PHASE1_STEPS     = 70;
    
  // SPEED MOTOR2  
  const float     M2_SPEED_MIN        = 250;
  const float     M2_SPEED_MAX        = 3200;  
  const uint32_t  M2_PHASE1_STEPS     = 650;

  // SENSORS MOTOR1
  const uint8_t   PIN_M1_S1           = 14;
  const bool      M1_S1_TRIG_LVL      = LOW;
  const uint32_t  M1_S1_BOUNCE_MS     = 20;

  const uint8_t   PIN_M1_S2           = 42;
  const bool      M1_S2_TRIG_LVL      = LOW;
  const uint32_t  M1_S2_BOUNCE_MS     = 10;

  // SENSORS MOTOR2
  const uint8_t   PIN_M2_S1           = 32;
  const bool      M2_S1_TRIG_LVL      = LOW;
  const uint32_t  M2_S1_BOUNCE_MS     = 50;
  
  const uint8_t   PIN_M2_S2           = 33;
  const bool      M2_S2_TRIG_LVL      = HIGH;
  const uint32_t  M2_S2_BOUNCE_MS     = 50;


  // ANALOG INPUTS
  const uint8_t   PIN_R1_DELAY        = A0;
  const uint8_t   PIN_R1_SPEED        = A1;
  const uint8_t   PIN_R2_DELAY        = A4;
  const uint8_t   PIN_R2_SPEED        = A3;

  // BRAK  
  const uint8_t   PIN_BRAK_OUT        = 45;
  const uint32_t  BRAK_DELAY_MS       = 500;
  const uint32_t  BRAK_ACT_TIME_MS    = 500;  
  const uint8_t   BRAK_ACTIVE_LEVEL   = HIGH;

  // SENSOR 5
  const uint8_t   PIN_S_AUX           = 43;
  const uint8_t   S_AUX_TRG           = LOW;
  const uint32_t  S_AUX_BOUNCE_MS     = 50;
  
  // НАСТРОЙКИ КОНЕЦ
  /////////////////////////////////////////////////////////  
  
#else

  /////////////////////////////////////////////////////////
  // НИЖЕ НАСТРОЙКИ НЕ МЕНЯТЬ !!! ЭТО ДЛЯ ОТЛАДКИ

  // MOTORS
  const uint8_t   STEPPER_INTERFACE   = AccelStepper::DRIVER;
  
  const uint8_t   PIN_MOTOR1_STEP     = 26;
  const uint8_t   PIN_MOTOR1_DIR      = 28;
  const uint8_t   PIN_MOTOR1_EN       = 24;

  const uint8_t   PIN_MOTOR2_STEP     = 36;
  const uint8_t   PIN_MOTOR2_DIR      = 34;
  const uint8_t   PIN_MOTOR2_EN       = 30;

  // SPEED MOTOR1  
  const float     M1_SPEED_MIN        = 150;
  const float     M1_SPEED_MAX        = 1000;  
  const uint32_t  M1_PHASE1_STEPS     = 1000;
    
  // SPEED MOTOR2  
  const float     M2_SPEED_MIN        = 300;
  const float     M2_SPEED_MAX        = 1200;  
  const uint32_t  M2_PHASE1_STEPS     = 1000;

  // SENSORS MOTOR1
  const uint8_t   PIN_M1_S1           = 32;
  const bool      M1_S1_TRIG_LVL      = LOW;
  const uint32_t  M1_S1_BOUNCE_MS     = 20;

  const uint8_t   PIN_M1_S2           = 47;
  const bool      M1_S2_TRIG_LVL      = LOW;
  const uint32_t  M1_S2_BOUNCE_MS     = 20;

  // SENSORS MOTOR2
  const uint8_t   PIN_M2_S1           = 45;
  const bool      M2_S1_TRIG_LVL      = LOW;
  const uint32_t  M2_S1_BOUNCE_MS     = 20;
  
  const uint8_t   PIN_M2_S2           = 43;
  const bool      M2_S2_TRIG_LVL      = LOW;
  const uint32_t  M2_S2_BOUNCE_MS     = 20;


  // ANALOG INPUTS
  const uint8_t   PIN_R1_DELAY        = A3;
  const uint8_t   PIN_R1_SPEED        = A4;
  const uint8_t   PIN_R2_DELAY        = A11;
  const uint8_t   PIN_R2_SPEED        = A12;

  // BRAK  
  const uint8_t   PIN_BRAK_OUT        = LED_BUILTIN;
  const uint32_t  BRAK_DELAY_MS       = 100;
  const uint32_t  BRAK_ACT_TIME_MS    = 700;  
  const uint8_t   BRAK_ACTIVE_LEVEL   = HIGH;

  // SENSOR 5
  const uint8_t   PIN_S_AUX           = 41;
  const uint8_t   S_AUX_TRG           = LOW;
  const uint32_t  S_AUX_BOUNCE_MS     = 10;
  
#endif

///////////////////////////////////////////////////////////////////////////////

#ifdef DEBUG
  #define DBG(x)      { Serial.print(x); }  
  #define DBGFS(s)     { Serial.print(F(s)); }    
#else
  #define DBG(x)
  #define DBGFS(s)
#endif

///////////////////////////////////////////////////////////////////////////////

AccelStepper m1(STEPPER_INTERFACE, PIN_MOTOR1_STEP, PIN_MOTOR1_DIR); // (STEP, DIR)
AccelStepper m2(STEPPER_INTERFACE, PIN_MOTOR2_STEP, PIN_MOTOR2_DIR);

#define INVERSE_PIN_LEVEL(LEVEL) ((LEVEL==LOW)?HIGH:LOW)

const uint8_t BRAK_INACTIVE_LEVEL = INVERSE_PIN_LEVEL(BRAK_ACTIVE_LEVEL);


class AvrPinChangeDetector
{
public:
  
  inline bool READ_PIN(void) const { return ((*_inreg) & _bit); }
  
  void init(uint8_t pin, uint8_t mode = INPUT_PULLUP, uint32_t debounceTimeMs = 20, int debounce = 10)
  {
    _count = 0;
    _debounce = (debounce < 3) ? 3 : debounce;   
    _debounceIntervalUs = debounceTimeMs * 1000 / _debounce;
    
    pinMode(pin, mode);       
        
    _bit = digitalPinToBitMask(pin);
    _inreg = portInputRegister(digitalPinToPort(pin));

    _state = READ_PIN();    
  }
    
  bool state() const { return _state; }
  
  bool updateOnTime()
  {
    if (READ_PIN() != _state)
    {
      if ((++_count) >= _debounce)
      {
        _state = !_state;         
        _count = 0;
                  
        return true;
      }
    }
    else
    {
      _count = 0;
    }         
    
    return false;
  }

  bool updateCasuallyInGroup(uint32_t curMicros)
  {      
    if ((curMicros - _last) >= _debounceIntervalUs)
    {
      _last = curMicros;
      
      return updateOnTime();
    }
    return false;
  }

  bool updateCasually()
  { 
    return updateCasuallyInGroup(micros());
  }
  
private:
  int _debounce;
  uint32_t _debounceIntervalUs;
  
  uint8_t _bit;
  volatile uint8_t * _inreg;
    
  volatile bool _state;
  volatile int _count;  

  volatile unsigned long _last = 0;
};


AvrPinChangeDetector s1m1;
AvrPinChangeDetector s2m1;

AvrPinChangeDetector s1m2;
AvrPinChangeDetector s2m2;

AvrPinChangeDetector saux;


template<int N, class T = int> class CInsertionSortArray
{
public:
  void insert(T e)
  {    
    int i;
    T save = e;
    for (i = 0; i < _n; i++)
    {
      if (_a[i] > e)
      {
        save = _a[i];
        _a[i++] = e;
        break;
      }
    }

    for (; i < _n; i++)
    {
      e = _a[i];
      _a[i] = save;
      save = e;     
    }

    _a[_n++] = save;
  }

  void reset() { _n = 0; }

  T _a[N];
  int _n;
};

constexpr int ANALOG_FLT_SIZE = 10;

CInsertionSortArray<ANALOG_FLT_SIZE> afm1;
CInsertionSortArray<ANALOG_FLT_SIZE> afm2;

void setup()
{  
  // Serial  
  Serial.begin(9600);  
  DBGFS("START\n");
  
  // MOTORS
  m1.setPinsInverted(false, false, true);
  m1.setEnablePin(PIN_MOTOR1_EN);  
  
  m2.setPinsInverted(false, false, true);
  m2.setEnablePin(PIN_MOTOR2_EN);
  
  // SENSORS
   
  s1m1.init(PIN_M1_S1, INPUT_PULLUP, M1_S1_BOUNCE_MS);
  s2m1.init(PIN_M1_S2, INPUT_PULLUP, M1_S2_BOUNCE_MS);

  s1m2.init(PIN_M2_S1, INPUT_PULLUP, M2_S1_BOUNCE_MS);
  s2m2.init(PIN_M2_S2, INPUT_PULLUP, M2_S2_BOUNCE_MS);

  saux.init(PIN_S_AUX, INPUT_PULLUP, S_AUX_BOUNCE_MS);
  
  // BRAK
  pinMode(PIN_BRAK_OUT, OUTPUT);
  digitalWrite(PIN_BRAK_OUT, BRAK_INACTIVE_LEVEL); 
}


bool gotBrakCmd()
{
  static const char * brak = "brak";
  static const int brakLen = strlen(brak);
  
  static int idx = 0;

  bool bRet = false;
  
  if (Serial.available())
  {
    int c = Serial.read();

    if (c == '\n')
    {                     
      if (idx == brakLen) bRet = true;
      idx = 0;
    }
    else if (idx < brakLen)
    {
      if (c != brak[idx++]) idx = 0;       
    }
  }

  return bRet;
}

void loop()
{  
  
  static uint32_t lastSensPollTime;

  static enum BRAK_STATE { BS_INACT, BS_DELAY, BS_ACTIV } bs = BS_INACT;

  static enum MOTOR_STATE { MS_STOPPED, MS_FLTDELAY, MS_DELAY, MS_FLTSPEED, MS_MOVEPHASE_STARTACCEL, MS_MOVEPHASE_CONSTSPEED } ms1 = MS_STOPPED, ms2 = MS_STOPPED;

  /// SENSORS //////////////////////////////////////////////////

  uint32_t curMicros = micros();
  bool bS1M1 = s1m1.updateCasuallyInGroup(curMicros);
  bool bS2M1 = s2m1.updateCasuallyInGroup(curMicros);
  bool bS1M2 = s1m2.updateCasuallyInGroup(curMicros);
  bool bS2M2 = s2m2.updateCasuallyInGroup(curMicros);

  if (saux.updateCasuallyInGroup(curMicros) && (saux.state() == S_AUX_TRG)) Serial.println("+1");
  
  /// MOTOR1 ///////////////////////////////////////////////////

  static uint32_t m1StartTime, m1DelayTime, m1SpeedMax;

  switch(ms1)
  {
    case MS_STOPPED:
      if (bS1M1 && (s1m1.state() == M1_S1_TRIG_LVL))
      {         
        m1StartTime = millis();
        afm1.reset();
        ms1 = MS_FLTDELAY;        
        DBGFS("M1 S1 ON\n");
      }
    break;

    case MS_FLTDELAY:
      if (afm1._n < ANALOG_FLT_SIZE)
      {
        afm1.insert(analogRead(PIN_R1_DELAY));
      }
      else
      {        
        m1DelayTime = afm1._a[ANALOG_FLT_SIZE/2];
        ms1 = MS_DELAY;               
        DBGFS("M1 Pauz ");DBG(m1DelayTime);DBGFS("\n");
      }
    break;

    case MS_DELAY:
      if ((millis() - m1StartTime) >= m1DelayTime)
      {
        afm1.reset();
        ms1 = MS_FLTSPEED;                      
      }
    break;

    case MS_FLTSPEED:
      if (afm1._n < ANALOG_FLT_SIZE)
      {
        afm1.insert(analogRead(PIN_R1_SPEED)/2);
      }
      else
      {        
        m1SpeedMax = map(afm1._a[ANALOG_FLT_SIZE/2], 0, 1023, M1_SPEED_MAX, M1_SPEED_MIN);
                
        m1.setCurrentPosition(0);
        m1.move(M1_PHASE1_STEPS*2);
        m1.setAcceleration(m1SpeedMax);
        m1.setMaxSpeed(m1SpeedMax);
        
        ms1 = MS_MOVEPHASE_STARTACCEL;

        DBGFS("M1 max Sk ");DBG(m1SpeedMax);DBGFS(", M1 GO\n");
      }
    break;

    case MS_MOVEPHASE_STARTACCEL:
      if (m1.distanceToGo() <= M1_PHASE1_STEPS)
      {        
        ms1 = MS_MOVEPHASE_CONSTSPEED;        
        DBGFS("M1 Speed ");DBG(m1.speed());DBGFS(" shagov ");DBG(m1.distanceToGo());DBGFS("\n");        
      }
    break;
   
    case MS_MOVEPHASE_CONSTSPEED:
      if (bS2M1 && (s2m1.state() == M1_S2_TRIG_LVL))
      {
        m1.setSpeed(0);
        ms1 = MS_STOPPED;
        DBGFS("M1 S2 ON, M1 stop\n");
      }
    break;
  }

  if (ms1 != MS_MOVEPHASE_CONSTSPEED) m1.run(); else m1.runSpeed();  

  /// MOTOR2 ///////////////////////////////////////////////////

  static uint32_t m2StartTime, m2DelayTime, m2SpeedMax;

  switch(ms2)
  {
    case MS_STOPPED:
      if (bS1M2 && (s1m2.state() == M2_S1_TRIG_LVL))
      {         
        m2StartTime = millis();
        afm2.reset();
        ms2 = MS_FLTDELAY;        
        DBGFS("M2 S1 ON\n");
      }
    break;

    case MS_FLTDELAY:
      if (afm2._n < ANALOG_FLT_SIZE)
      {
        afm2.insert(100);
      }
      else
      {        
        m2DelayTime = afm2._a[ANALOG_FLT_SIZE/2];
        ms2 = MS_DELAY;               
        DBGFS("M2 Pauz ");DBG(m2DelayTime);DBGFS("\n");
      }
    break;

    case MS_DELAY:
      if ((millis() - m2StartTime) >= m2DelayTime)
      {
        afm2.reset();
        ms2 = MS_FLTSPEED;                      
      }
    break;

    case MS_FLTSPEED:
      if (afm2._n < ANALOG_FLT_SIZE)
      {
        afm2.insert(analogRead(PIN_R2_SPEED));
      }
      else
      {        
        m2SpeedMax = map(afm2._a[ANALOG_FLT_SIZE/2], 0, 1023, M2_SPEED_MAX, M2_SPEED_MIN);
                
        m2.setCurrentPosition(0);
        m2.move(M2_PHASE1_STEPS*2);
        m2.setAcceleration(m2SpeedMax*2/3);
        m2.setMaxSpeed(m2SpeedMax);
        
        ms2 = MS_MOVEPHASE_STARTACCEL;

        DBGFS("M2 max Sk ");DBG(m2SpeedMax);DBGFS(", M2 GO\n");
      }
    break;

    case MS_MOVEPHASE_STARTACCEL:
      if (m2.distanceToGo() <= M2_PHASE1_STEPS)
      {        
        ms2 = MS_MOVEPHASE_CONSTSPEED;        
        DBGFS("M2 Sk ");DBG(m2.speed());DBGFS(" shag ");DBG(m2.distanceToGo());DBGFS("\n");        
      }
    break;
   
    case MS_MOVEPHASE_CONSTSPEED:
      if (bS2M2 && (s2m2.state() == M2_S2_TRIG_LVL))
      {
        m2.setSpeed(0);
        ms2 = MS_STOPPED;
        DBGFS("M2 S2 ON, M2 stop\n");
      }
    break;
  }

  if (ms2 != MS_MOVEPHASE_CONSTSPEED) m2.run(); else m2.runSpeed();  

  /// BRAK /////////////////////////////////////////////////////
  static uint32_t bsTime;

  bool bGotBrak = gotBrakCmd(); // read port anyway, whatever state is
  
  switch(bs)
  {
    case BS_INACT:
      if (bGotBrak)
      {                
        bsTime = millis();
        bs = BS_DELAY;
      }
    break;  

    case BS_DELAY:
    {
      uint32_t curTime = millis();
      if ((curTime - bsTime) >= BRAK_DELAY_MS)
      {
        bsTime = curTime;
        digitalWrite(PIN_BRAK_OUT, BRAK_ACTIVE_LEVEL);                 
        bs = BS_ACTIV;
      }
    }
    break;

    case BS_ACTIV:
      if ((millis() - bsTime) >= BRAK_ACT_TIME_MS)
      {
        digitalWrite(PIN_BRAK_OUT, BRAK_INACTIVE_LEVEL);         
        bs = BS_INACT;        
      }
    break;  
  }
   
  ///////////////////////////////
        
}
2

Написал на почту

3

Желательно работу разбить на 2 этапа: 1. внести корректировки в существующий код для увеличения скорости двигателя, пусть не с потенциометра а в жесткую прописать, я на месте подберу нужную. 2. написать новую программу с необходимыми регулировками.

4

Какая скорость нужна? Почему в коде 2 двигателя а в задаче 1?

5

скорость 8000 имп/с, изначально планировал 2 двигателя сейчас хотя бы с одним разобраться

6

Для меги максимум 4000 шагов, без сторонней нагрузки.

7

честно говоря не знаю в каких попугаях у меня указано в программе, но текущая программа выводит не то 1000 не то 1300 и этого недостаточно, хотя я конечно понимаю что 1000 импульсов в секунду должно обеспечивать 5 оборотов в секунду для двигателя с шагом 1,8гр, и по идее мне этого должно хватать, но в другой программе мне выдает 8000 и скорости достаточно, но там датчики как то слабовато обрабатывает. мне надо 3 раза в секунду поворачиваться примерно на 60гр, то есть у меня даже оборота в секунду нет

8

Давай еще раз повторю, если не дошло: на меге максимум 4000 шагов. 8000 там быть не может никак.

Откуда тогда взялось 8000, когда надо всего 100?

Еще раз, сначала, полностью и внятно, опиши задачу. Если хочешь, чтобы ее кто-то сделал. Сидеть разбираться в этой каше, которая сама себе противоречит, никому не надо.

1 лайк