Подскажите как работает часть кода для "бесполезной машины".

Здравствуйте, господа ардуинщики! В ардуино и программировании я новичок и поэтому прошу помощи у более опытных. Загорелся сделать "бесполезную машину". Собрал, работает. Мне непонятна небольшая часть кода, но она самая сложная (для меня).
Красным то что непонятно.

Вот код:

Код (Text):

#include <Servo.h>
Servo myservo;

enum PinAssignments {
  encoderPinA = 2,
  encoderPinB = 3,
  clearButton = 1,
  SPEED1  =  5 ,
  SPEED2  =  6 ,
  DIRleft    =  4,
  DIRright    =  7,
  SW1 = A0,
  SW2 = A1,
  SW3 = A2,
  SW4 = A3,
  SW5 = A4,
  SW6 = A5,
  SW7 = 12,
  SW8 = 11,
  SW9 = 10,

  optButton = 8,
};

#define POSTOL    30 // position tolerance
#define MAXPOS 6900 // end of line
/* controller parameters */
#define PID_KP    4
//#define PID_KI  0 // works without integral
#define PID_KD    10
#define VMIN 70      //Min = 0
#define VMAX 200  //Max = 255
/*
* NOTE: to adjust KI and KD for sampling rate:
* KI = KI/rate
* KD = KD*rate
*/
/* delay times for switches, see main */
#define WAIT0    10  //10
#define WAIT1    14  //14
#define WAIT2    30  //30

volatile int encoderPos = 0;
int lastReportedPos = 1;
boolean A_set = false;
boolean B_set = false;
// switch position
//            0    1  2    3    4    5    6    7  8
int SWP[9] = {10, 750, 1650, 2500, 3450, 4280, 5220, 6150, 7080};

void setup() {
  for (int i = 4; i <= 7; ++i)
  {
    pinMode(i, OUTPUT);
  }
  pinMode(encoderPinA, INPUT);
  pinMode(encoderPinB, INPUT);
  pinMode(clearButton, INPUT);
  digitalWrite(clearButton, LOW);
  //attachInterrupt(5, resetZero, RISING);
  // encoder pin on interrupt 0 (pin 2)
  attachInterrupt(0, doEncoderA, CHANGE);
  // encoder pin on interrupt 1 (pin 3)
  attachInterrupt(1, doEncoderB, CHANGE);
  pinMode(SW1, INPUT);
  pinMode(SW2, INPUT);
  pinMode(SW3, INPUT);
  pinMode(SW4, INPUT);
  pinMode(SW5, INPUT);
  pinMode(SW6, INPUT);
  pinMode(SW7, INPUT);
  pinMode(SW8, INPUT);
  pinMode(SW9, INPUT);
}

void loop() {
  int loopcount = 0;
  clearPos(100);
  while (digitalRead(optButton) == LOW) {
    if (digitalRead(SW1) == HIGH) {
      moveTo(SWP[0]);
      //delay(10);
      toggle();
      loopcount++;
    }
    if (digitalRead(SW2) == HIGH) {
      moveTo(SWP[1]);
      //delay(10);
      toggle();
      loopcount++;
    }
    if (digitalRead(SW3) == HIGH) {
      moveTo(SWP[2]);
      //delay(10);
      toggle();
      loopcount++;
    }
    if (digitalRead(SW4) == HIGH) {
      moveTo(SWP[3]);
      //delay(10);
      toggle();
      loopcount++;
    }
    if (digitalRead(SW5) == HIGH) {
      moveTo(SWP[4]);
      //delay(10);
      toggle();
      loopcount++;
    }
    if (digitalRead(SW6) == HIGH) {
      moveTo(SWP[5]);
      //delay(10);
      toggle();
      loopcount++;
    }
    if (digitalRead(SW7) == HIGH) {
      moveTo(SWP[6]);
      //delay(10);
      toggle();
      loopcount++;
    }
    if (digitalRead(SW8) == HIGH) {
      moveTo(SWP[7]);
      //delay(10);
      toggle();
      loopcount++;
    }
    if (digitalRead(SW9) == HIGH) {
      moveTo(SWP[8]);
      //delay(10);
      toggle();
      loopcount++;
    }

    // reset cumulative encoder error
    if (loopcount >= 48) {
      clearPos(130);
      loopcount = 0;
    }
    delay(10);
  }
}

// Interrupt on A changing state
void doEncoderA() {
  // Test transition
  A_set = digitalRead(encoderPinA) == HIGH;
  // and adjust counter + if A leads B
  encoderPos += (A_set != B_set) ? +1 : -1;
  if (digitalRead(clearButton)) encoderPos = 0;
}

// Interrupt on B changing state
void doEncoderB() {
  // Test transition
  B_set = digitalRead(encoderPinB) == HIGH;
  // and adjust counter + if B follows A
  encoderPos += (A_set == B_set) ? +1 : -1;
  if (digitalRead(clearButton)) encoderPos = 0;
}

void moveRight (int moveSpeed) {
  analogWrite(SPEED2, 255);
  digitalWrite(DIRright, moveSpeed);

  analogWrite(SPEED1, 0);
}

void moveLeft (int moveSpeed) {
  analogWrite(SPEED1, 255);
  digitalWrite(DIRleft, moveSpeed);
  analogWrite(SPEED2, 0);
}

void moveStop () {
  analogWrite(SPEED1, 0);
  analogWrite(SPEED2, 0);
}

void clearPos(int moveSpeed) {
  while (digitalRead(clearButton) == LOW) {
    moveLeft (moveSpeed);
    digitalWrite(clearButton, LOW);
    delay(5);
  }
  encoderPos = 0;
  moveStop ();
}
/* basic PID controller */
void set_pid( int setPos ) {
  //static int32_t int_sum = 0;
  static int deltalast = 0;
  int m_output = 0;
  if (digitalRead(clearButton)) encoderPos = 0;
  int delta = setPos - encoderPos ;
  /*int_sum += delta;  //Integrative disabled
  if (int_sum > 20000)
      int_sum = 20000;
  if (int_sum < -20000)
      int_sum = -20000;*/
  m_output = PID_KP * delta + /* PID_KI*((int32_t)int_sum) +*/ (PID_KD * (delta - deltalast));
  deltalast = delta;
  if (m_output > VMIN) {
    if (m_output > VMAX) moveRight(VMAX);
    else moveRight(m_output);
  }
  else if (m_output < -VMIN) {
    if (m_output < -VMAX) moveLeft(VMAX);
    else moveLeft(-1 * m_output);
  }
  else moveStop();
}
/* busy delay loop with PID set */
void moveLoop (int cycle, int setPos) {
  for (; cycle > 0; cycle--) {
    set_pid(setPos);
    delay(2);
  }
}

void moveTo (int setPos) {
  moveLoop(WAIT0, setPos);
  while (!(encoderPos >= (setPos - POSTOL) && encoderPos <= (setPos + POSTOL))) {
    moveLoop(WAIT1, setPos);
  }
  moveLoop(WAIT2, setPos);
  moveStop ();
}

void toggle () {
  myservo.attach(9);
  myservo.write(145);  // устанавливаем сервопривод в серединное положение
  delay(200);
  // myservo.write(139);  // устанавливаем сервопривод в крайнее левое положение
  // delay(700);


  myservo.write(90);  // устанавливаем сервопривод в крайнее левое положение
  delay(20);

}
 

Это небольшое пояснение к коду которое я получил от автора.

clearButton was connected to a end of course switch which is used to set the position reference during power-up since encoder position is relative (it was also used to reset cumulative encoder error caused by a few faded lines in my encoder strip).
servoPinR and servoPinL are the drive signals for the H-Bridge.
potPin was temporarily used to tune the PD controller gains.

optButton was also used for debug purposes: it interrupts the switch polling.

togglePin triggers the solenoid action which deactivate the switches.

lidPin can control an auxiliary motor to open the lid before the solenoid action, if required (e.g. the lid is too heavy) .

 

1. В Arduino IDE нажмите Ctrl+T.
2. При вставке кода на форум пользуйтесь тэгом CODE или кнопкой "<>".
Иначе желающих ломать глаза при чтении кода будет очень мало.

 

Исправил

 

Теперь стало непонятно что именно непонятно. )

Я вот не любитель многократных однотипных операций как вижу здесь с опросом тумблеров. Лучше бы сделать в цикле, а номера пинов закинуть в массив, если они не по порядку идут. Я бы вообще сделал опрос через входной сдвиговый регистр, точнее два, потому что больше 8 тумблеров. Пинов минимум и опрос в пару строк.

Не совсем понял как осуществляется выключение, серва катается по рельсе при помощи моторчика до нужного тумблера?

 

В оригинале тумблеров 8. Это я уже от себя добавил девятый.
Да, серва катается на коретке от струйного принтера и включается когда коретка останавливается на заданном расстоянии. В оригинале в место сервы автор использовал соленоид.

Если не трудно, сделайте код через сдвиговый регистр и массив. Заранее спасибо!

 

Вечером попробую написать код для опроса через регистр и его обработки.

 

Спасибо!

 

Выяснилось что нет в наличии входных регистров у меня, без них трудно написать скеч, вдруг что забуду. Завтра постараюсь их купить и тогда смогу сделать с проверкой.

Если сама суть, то так:
Опрашивается состояние тумблеров через регистр, получается байт в котором биты "1" это включенные тумблеры, а "0" выключенные.
При помощи цикла for и функции bitRead сканируется каждый бит и выполняется moveTo и прочее в случае если бит = 1.

Собственно дальше как обычно, не проверял что там, если работает, то будет работать. Преимущества такого подхода в том, что будет задействовано всего 4 цифровых пина, даже если тумблеров не 8, а гораздо больше. Кроме того, этот кусок кода займет всего несколько строк.

 

Спасибо за помощь! Буду читать что такое входной регистр и массивы.

 

Могу показать код для С++ только другой автор. Может чего интересного там найдете.

 

Вот для С++

 

Вот входной регистр. Работает так же как выходной, только считывает байт а не передает, и примеры тому есть.