«Драгстер» и «Драгстер: Расширенный набор»: исходный код проектов

«Драгстер»

Обрати внимание!

Мы регулярно обновляем наши наборы. Проверь, какая версия досталась тебе! Для этого посмотри на обратную сторону обложки буклета. Если на ней нет значка с текстом «а2» как на картинке снизу, то тебе сюда.

Исходники

1. Запуск

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
}
 
void loop() {
  digitalWrite(LED_BUILTIN, HIGH);
  delay(1000);
  digitalWrite(LED_BUILTIN, LOW);
  delay(1000);
}

2. Индикация

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
}
 
void loop() {
  digitalWrite(LED_BUILTIN, HIGH);
  delay(1000);
  digitalWrite(LED_BUILTIN, LOW);
  delay(1000);
}

3. Ключ на старт

void setup() {
  pinMode(10, INPUT);
  pinMode(LED_BUILTIN, OUTPUT);
}
 
void loop() {
  if (digitalRead(10) == LOW) {
    digitalWrite(LED_BUILTIN, HIGH);
  } else {
    digitalWrite(LED_BUILTIN, LOW);
  }
}

4. Прогрев двигателей

void setup() {
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
 
  digitalWrite(4, HIGH);
  digitalWrite(7, HIGH);
}
 
void loop() {
  analogWrite(5, 64);
  analogWrite(6, 64);
  delay(1000);
 
  analogWrite(5, 0);
  analogWrite(6, 0);
  delay(1000);
}

5. Змейка

#define DIR1 4
#define DIR2 7
#define EN1 5
#define EN2 6
 
void setup() {
  pinMode(DIR1, OUTPUT);
  pinMode(EN1, OUTPUT);
  pinMode(EN2, OUTPUT);
  pinMode(DIR2, OUTPUT);
 
  digitalWrite(DIR1, HIGH);
  digitalWrite(DIR2, HIGH);
 
  analogWrite(EN1, 45);
  analogWrite(EN2, 90);
}
 
void loop() {
 
}

6. Тюнинг. Библиотеки

Библиотека Dragster

  1. Нажми на ссылку библиотеки — браузер скачает zip-архив.
  2. Открой в Arduino IDE и перейди в меню Скетч Подключить библиотеку Добавить .ZIP библиотеку…
  3. В окне выбора файла открой скачанный zip-архив и нажми Open.
  4. Библиотека готова к использованию.
#include "Dragster.h"
 
Dragster robot(MMAX_16_OHM);
 
void setup() {
  robot.begin();
  // robot.begin(SWAP_RIGHT);
  // robot.begin(SWAP_LEFT);
  // robot.begin(SWAP_BOTH);
}
 
void loop() {
  robot.drive(110, 50);
  delay(1000);
  robot.drive(50, 110);
  delay(1000);
}

7. Стоп-сигнал

Библиотека TroykaLedMatrix

Подробное описание LED-матрицы.

#include "Dragster.h"
#include "TroykaLedMatrix.h"
 
Dragster robot(MMAX_16_OHM);
TroykaLedMatrix matrix;
 
uint8_t stop[] {
  0b11111111,
  0b11111111,
  0b11111111,
  0b11111111,
  0b11111111,
  0b11111111,
  0b11111111,
  0b11111111,
};
 
void setup() {
  robot.begin();
  matrix.begin();
}
 
void loop() {
  matrix.clear();
  robot.drive(120, 120);
  delay(1000);
 
  matrix.drawBitmap(stop);
  robot.drive(0, 0);
  delay(1000);
}

8. Поворотники

Редактор изображений

#include "Dragster.h"
#include "TroykaLedMatrix.h"
 
Dragster robot(MMAX_16_OHM);
TroykaLedMatrix matrix;
 
const byte IMAGES[][8] = {
  {0x18, 0x3c, 0x7e, 0xdb, 0x99, 0x18, 0x18, 0x18},
  {0x18, 0x18, 0x18, 0x99, 0xdb, 0x7e, 0x3c, 0x18}
};
 
const int IMAGES_LEN = sizeof(IMAGES) / 8;
 
void setup() {
  robot.begin();
  matrix.begin();
}
 
void loop() {
  matrix.drawBitmap(IMAGES[0]);
  robot.drive(110, 60);
  delay(1000);
 
  matrix.drawBitmap(IMAGES[1]);
  robot.drive(60, 110);
  delay(1000);
}

9. Реклама спонсоров

#include "TroykaLedMatrix.h"
 
TroykaLedMatrix matrix;
 
const byte IMAGES[][8] = {
  {0x30, 0x78, 0xcc, 0xcc, 0xfc, 0xcc, 0xcc, 0x00},
  {0xc6, 0xee, 0xfe, 0xfe, 0xd6, 0xc6, 0xc6, 0x00},
  {0xfc, 0x66, 0x66, 0x7c, 0x60, 0x60, 0xf0, 0x00},
  {0xfe, 0x62, 0x68, 0x78, 0x68, 0x62, 0xfe, 0x00},
  {0xfc, 0x66, 0x66, 0x7c, 0x6c, 0x66, 0xe6, 0x00},
  {0xe6, 0x66, 0x6c, 0x78, 0x6c, 0x66, 0xe6, 0x00},
  {0x30, 0x78, 0xcc, 0xcc, 0xfc, 0xcc, 0xcc, 0x00},
  {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
};
const int IMAGES_LEN = sizeof(IMAGES) / 8;
 
byte shift = 0;
 
void setup() {
  matrix.begin();
}
 
void loop() {
  delay(70);
  matrix.marquee(IMAGES, IMAGES_LEN, shift++);
  if (shift == IMAGES_LEN * 8) {
    shift = 0;
  }
}

10. Уровень заряда

#include "TroykaLedMatrix.h"
 
TroykaLedMatrix matrix;
 
byte valume[8] =
  {0x01, 0x03, 0x07, 0x0f, 0x1f, 0x3f, 0x7f, 0xff};
byte diagram[8] = {0, 0, 0, 0, 0, 0, 0, 0};
float R1 = 10000;
float R2 = 36000;
 
void setup() {
  matrix.begin();
  pinMode(A1, INPUT);
}
 
void loop() {
  delay(200);
  int adcValue = analogRead(A1);
  float V = adcValue * 5.0 / 1023.0;
  float Vbat = V * (R1 + R2) / R2;
 
  int matrixValue = (Vbat - 3.2) * 8;
  for (int i = 0; i < 8; i++) {
    if (i < matrixValue) {
      diagram[i] = valume[i];
    } else {
      diagram[i] = 0x00;
    }
  }
  matrix.drawBitmap(diagram);
}

11. Энкодер

#include "Dragster.h"
 
#define ENC1 0
 
Dragster robot(MMAX_16_OHM);
 
void setup() {
  pinMode(ENC1, INPUT);
 
  Serial.begin(115200);
}
 
void loop() {
  Serial.println(digitalRead(ENC1));
  delay(100);
}
#include "Dragster.h"
 
#define ENC1 0
#define ENC2 1
 
Dragster robot;
 
void setup() {
  Serial.begin(115200);
  pinMode(ENC1, INPUT);
  pinMode(ENC2, INPUT);
  robot.begin();
  robot.drive(120, 120);
}
 
void loop() {
  Serial.print(digitalRead(ENC1));
  Serial.print(" ");
  Serial.println(digitalRead(ENC2));
  delay(100);
}

12. Умные энкодеры

#include "Dragster.h"
 
Dragster robot(MMAX_16_OHM);
int counter = 0;
 
void leftEncoder() {
  counter++;
}
 
void rightEncoder() {
 
}
 
void setup() {
  robot.begin();
  robot.encodersBegin(leftEncoder, rightEncoder);
  robot.drive(60, 0);
 
  Serial.begin(115200);
}
 
void loop() {
  Serial.println(counter);
 
  delay(200);
}

13. Одометр

#include "Dragster.h"
 
Dragster robot(MMAX_16_OHM);
float left = 0;
float right = 0;
 
void leftEncoder() {
  left = left + 9.42;
}
 
void rightEncoder() {
  right = right + 9.42;
}
 
void setup() {
  robot.begin();
  robot.encodersBegin(leftEncoder, rightEncoder);
  robot.drive(100, 120);
  Serial.begin(115200);
}
 
void loop() {
  Serial.print(left);
  Serial.print(" ");
  Serial.print(right);
  Serial.println();
  delay(20);
}

14. Спидометр

#include "Dragster.h"
#include "TroykaLedMatrix.h"
 
Dragster robot(MMAX_16_OHM);
TroykaLedMatrix matrix;
byte diagram[8];
int speed = 0;
 
void handler() {
  speed++;
}
 
void setup() {
  robot.begin();
  robot.encodersBegin(handler, handler);
  robot.drive(40, 80);
  matrix.begin();
}
 
void loop() {
  for (int i = 0; i < 8; i++) {
    if (speed > i) {
      diagram[i] = matrix.map(8 - (i + 1), 0, 8);
    } else {
      diagram[i] = 0;
    }
  }
  matrix.drawBitmap(diagram);
  speed = 0;
  delay(150);
}

16. Сканер дорожного полотна

Библиотека Octoliner

Подробное описание датчика линии.

#include "TroykaLedMatrix.h"
#include "Octoliner.h"
 
Octoliner octoliner;
TroykaLedMatrix matrix;
 
byte diagram[8];
 
void setup() {
  matrix.begin();
  octoliner.begin();
  octoliner.setSensitivity(50);
}
 
void loop() {
  for (int i = 0; i < 8; i++) {
    int analogValue = octoliner.analogRead(i);
    diagram[i] = matrix.map(analogValue, 0, 1023);
  }
  matrix.drawBitmap(diagram);
}

Автоматическая калибровка Октолайнера

#include "Octoliner.h"
#include "TroykaLedMatrix.h"
 
const byte images[][8] = {
  {0x38, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x38},
  {0x10, 0x30, 0x10, 0x10, 0x10, 0x10, 0x10, 0x38},
  {0x38, 0x44, 0x04, 0x04, 0x08, 0x10, 0x20, 0x7c},
  {0x38, 0x44, 0x04, 0x18, 0x04, 0x04, 0x44, 0x38},
  {0x04, 0x0c, 0x14, 0x24, 0x44, 0x7c, 0x04, 0x04},
  {0x7c, 0x40, 0x40, 0x78, 0x04, 0x04, 0x44, 0x38},
  {0x38, 0x44, 0x40, 0x78, 0x44, 0x44, 0x44, 0x38},
  {0x7c, 0x04, 0x04, 0x08, 0x10, 0x20, 0x20, 0x20},
  {0x38, 0x44, 0x44, 0x38, 0x44, 0x44, 0x44, 0x38},
  {0x38, 0x44, 0x44, 0x44, 0x3c, 0x04, 0x44, 0x38},
  {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
  {0x38, 0x44, 0x44, 0x08, 0x10, 0x00, 0x10, 0x10} 
};
 
byte result[5][8];
const int IMAGES_LEN = sizeof(images) / 8;
const int RESULT_LEN = sizeof(result) / 8;
TroykaLedMatrix matrix;
Octoliner octoliner;
byte shift = 0;
byte getX__(byte c) { return c / 100;}
byte get_X_(byte c) { return (c - floor(c / 100) * 100) / 10; }
byte get__X(byte c) { return (c - floor(c / 10) * 10); }
 
void copyPattern(byte position, byte value) {
  for(byte i = 0; i < 8; i++) {
    result[position][i] = images[value][i];
  }
}
 
void setup() {
    byte optimal;
    Serial.begin(115200);
    matrix.begin();
    octoliner.begin();
    delay(1000);
    while( digitalRead(10) != 0);
    matrix.drawBitmap(images[11]);
    octoliner.optimizeSensitivityOnBlack();
    optimal = octoliner.getSensitivity();    
    Serial.print(optimal);   
    copyPattern(0,10); copyPattern(1,10);    
    copyPattern(2,getX__(optimal));
    copyPattern(3,get_X_(optimal));
    copyPattern(4,get__X(optimal));
}
 
void loop() {
  delay(70);
  matrix.marquee(result, RESULT_LEN, shift++);
  if(shift == RESULT_LEN * 8) { shift = 0; }
}

17. Драг-рейсинг

#include "Dragster.h"
#include "TroykaLedMatrix.h"
#include "Octoliner.h"
 
Dragster robot(MMAX_16_OHM);
TroykaLedMatrix matrix;
Octoliner octoliner;
 
byte diagram[8];
 
void setup() {
  robot.begin();
  matrix.begin();
  octoliner.begin();
  octoliner.setSensitivity(208);
}
 
void loop() {
  for (int i = 0; i < 8; i++) {
    int adcValue = octoliner.analogRead(i);
    diagram[i] = matrix.map(adcValue, 0, 1023);
    if (diagram[i] < 4) {
      robot.drive(0, 0);
    }
  }
  matrix.drawBitmap(diagram);
 
  if (robot.readButton() == 0) {
    delay(1000);
    robot.drive(100, 100);
  }
}

18. Следование по линии

#include "Dragster.h"
#include "TroykaLedMatrix.h"
#include "Octoliner.h"
 
Dragster robot(MMAX_16_OHM);
TroykaLedMatrix matrix;
Octoliner octoliner;
 
byte diagram[8];
float weight[] =
  { -1, -0.75, -0.5, -0.25, 0.25, 0.5, 0.75, 1};
float Kp = 2;
 
void setup() {
  robot.begin();
  matrix.begin();
  octoliner.begin();
  octoliner.setSensitivity(208);
}
 
void loop() {
  float error = 0;
  float sum = 0;
  float sumWeighted = 0;
 
  for (int i = 0; i < 8; i++) {
    int adcValue = octoliner.analogRead(i);
    diagram[i] = matrix.map(adcValue, 0, 1023);
    sum += adcValue;
    sumWeighted += adcValue * weight[i];
  }
  if (sum != 0.0) {
    error = sumWeighted / sum;
  }
  matrix.drawBitmap(diagram);
 
  robot.driveF(0.35 * (1 - error * Kp), 0.35 * (1 + error * Kp));
}

19. Тюнинг. ПИД-регулятор

Библиотека PID_dragster

#include "Dragster.h"
#include "Octoliner.h"
#include "PID_dragster.h"
 
Dragster robot(MMAX_16_OHM);
Octoliner octoliner;
 
int lineData[8];
 
float speed = 0.4;
float KP = 1;
 
double output;
PID regulator(&output, KP, 0, 0);
 
void setup() {
  robot.begin();
  octoliner.begin();
  octoliner.setSensitivity(208);
}
 
void loop() {
  for (int i = 0; i < 8; i++) {
    lineData[i] = octoliner.analogRead(i);
  }
  double error = octoliner.trackLine(lineData);
  regulator.compute(error);
  robot.driveF(speed * (1 - output), speed * (1 + output));
}

20. Предсказания

#include "Dragster.h"
#include "Octoliner.h"
#include "PID_dragster.h"
 
Dragster robot(MMAX_16_OHM);
Octoliner octoliner;
 
int lineData[8];
 
float speed = 0.5;
float KP = 1;
float KD = 1;
 
double output;
PID regulator(&output, KP, 0, KD);
 
void setup() {
  robot.begin();
  octoliner.begin();
  octoliner.setSensitivity(208);
}
 
void loop() {
  for (int i = 0; i < 8; i++) {
    lineData[i] = octoliner.analogRead(i);
  }
  double error = octoliner.trackLine(lineData);
  regulator.compute(error);
  robot.driveF(speed * (1 - output), speed * (1 + output));
}

21. Накопление

#include "Dragster.h"
#include "Octoliner.h"
#include "PID_dragster.h"
 
Dragster robot(MMAX_16_OHM);
Octoliner octoliner;
 
int lineData[8];
 
float speed = 0.55;
float KP = 0.5;
float KD = 0.5;
float KI = 0.0;
 
double output;
PID regulator(&output, KP, KI, KD);
 
void setup() {
  robot.begin();
  octoliner.begin();
  octoliner.setSensitivity(208);
}
 
void loop() {
  for (int i = 0; i < 8; i++) {
    lineData[i] = octoliner.analogRead(i);
  }
  double error = octoliner.mapLine(lineData);
  regulator.compute(error);
  robot.driveF(speed * (1 - output), speed * (1 + output));
}

«Драгстер. Расширенный набор»

Исходники

1

#include "Dragster.h"
 
Dragster robot(MMAX_16_OHM);
 
void setup() {
  robot.begin();
}
 
void loop() {
  float speed = analogRead(A11) / 1023.0;
  robot.driveF(speed, speed);
}

2

#include "Dragster.h"
#include "Octoliner.h"
#include "PID_dragster.h"
 
Dragster robot(MMAX_16_OHM);
 
Octoliner octoliner;
int lineData[8];
 
double output;
PID regulator(&output, 0.0, 0.0, 0.0);
 
void setup() {
robot.begin();
octoliner.begin();
octoliner.setSensitivity(208);
}
 
void loop() {
  float kp = analogRead(A4) / 1023.0 * 1.0;
  float kd = analogRead(A5) / 1023.0 * 0.2;
  float speed = analogRead(A11) / 1023.0 * 1.0;
 
  regulator.SetTunings(kp, 0, kd);
 
  for (int i = 0; i < 8; i++) {
    lineData[i] = octoliner.analogRead(i);
  }
  float difference = octoliner.mapLine(lineData);
 
  regulator.compute(difference);
 
  robot.driveF(speed - output, speed + output);
}

3

#include "Dragster.h"
#include "Octoliner.h"
#include "PID_dragster.h"
#include "TroykaLedMatrix.h"
 
Dragster robot(MMAX_16_OHM);
 
Octoliner octoliner;
int lineData[8];
TroykaLedMatrix matrix;
byte diagram[8];
byte sensitivity;
float speed = 0.2;
double output;
PID regulator(&output, 0.5, 0, 0.02);
 
void setup() {
  robot.begin();
  octoliner.begin();
  octoliner.setSensitivity(208);
  matrix.begin();
  Serial.begin(115200);
}
 
void loop() {
  octoliner.setSensitivity(sensitivity = analogRead(A4) / 4);
  for (int i = 0; i < 8; i++) {
    lineData[i] = 4095 - octoliner.analogRead(i);
    diagram[i] = matrix.map(lineData[i], 3072, 4095);
  }
 
  matrix.drawBitmap(diagram);
  regulator.compute(octoliner.mapLine(lineData));
  robot.driveF(speed - output, speed + output);
  Serial.println(sensitivity);
}

4

#include "Dragster.h"
#include "Octoliner.h"
#include "PID_dragster.h"
#include "TroykaLedMatrix.h"
 
Dragster robot(MMAX_16_OHM);
 
Octoliner octoliner;
int lineData[8];
 
TroykaLedMatrix matrix;
byte diagram[8];
float speed = 0.2;
double output;
PID regulator(&output, 0.3, 0, 0.01);
 
void setup() {
  robot.begin();
  octoliner.begin();
  octoliner.setSensitivity(208);
  matrix.begin();
}
 
void loop() {
  int sum = 0;
  for (int i = 0; i < 8; i++) {
    lineData[i] = octoliner.analogRead(i);
    sum += lineData[i];
    diagram[i] = matrix.map(lineData[i], 0, 1023);
  }
  matrix.drawBitmap(diagram);
 
  if (sum > 2047 * 8) {
    for (int i = 0; i < 8; i++) {
      lineData[i] = 4095 - lineData[i];
    }
  }
 
  float error = octoliner.mapLine(lineData);
  regulator.compute(error);
  robot.driveF(speed - output, speed + output);
}

5

#include "Dragster.h"
#include "Octoliner.h"
#include "PID_dragster.h"
 
Dragster robot(MMAX_16_OHM);
 
Octoliner octoliner;
int lineData[8];
float speed = 0.35;
float kp = 0.35;
float kd = 0.05;
double output;
PID regulator(&output, kp, 0, kd);
 
void setup() {
  robot.begin();
  octoliner.begin();
  octoliner.setSensitivity(208);
  while (digitalRead(11) == 1) {
    delay(10);
  }
}
 
void loop() {
  for (int i = 0; i < 8; i++) {
    lineData[i] = octoliner.analogRead(i);
  }
  double difference = octoliner.mapLine(lineData);
  regulator.compute(difference);
  robot.driveF(speed - output, speed + output);
}

6

#define TRIGPIN 8
#define ECHOPIN 9
 
void setup() {
  pinMode(TRIGPIN, OUTPUT);
  pinMode(ECHOPIN, INPUT);
  Serial.begin(115200);
}
 
void loop() {
  digitalWrite(TRIGPIN, HIGH);
  delayMicroseconds(5);
  digitalWrite(TRIGPIN, LOW);
 
  unsigned long duration = pulseIn(ECHOPIN, HIGH);
  float distance = 0.345 * duration / 2;
 
  Serial.print(distance);
  Serial.println("mm");
 
  delay(100);
}

7

#include "Dragster.h"
#include "PID_dragster.h"
 
#define TRIGPIN 8
#define ECHOPIN 9
 
Dragster robot(MMAX_16_OHM);
 
double output;
PID regulator(&output, 0.5, 0.1, 0.02);
 
void setup() {
  pinMode(TRIGPIN, OUTPUT);
  pinMode(ECHOPIN, INPUT);
  robot.begin();
  regulator.setOutputLimits(-0.2, 0.2);
}
 
void loop() {
  digitalWrite(TRIGPIN, HIGH);
  delayMicroseconds(5);
  digitalWrite(TRIGPIN, LOW);
 
  long duration = pulseIn(ECHOPIN, HIGH, 1800);
 
  if (duration == 0) {
    robot.driveF(0, 0);
  } else {
    double distance = 0.34 * duration / 2.0;
    double difference = (100 - distance) / 100.0;
    regulator.compute(difference);
    robot.driveF(output, output);
  }
}

8

Библиотека TroykaOLED

#include "TroykaOLED.h"
TroykaOLED display;
 
void setup() {
  display.begin();
}
 
void loop() {
  display.setFont(digits24x40);
    for (int i = 5; i > 0; i--) {
      display.clearDisplay();
      display.print(i, OLED_CENTER, OLED_CENTER);
      delay(1000);
  }
 
  display.clearDisplay();
  display.setFont(alfabet12x16);
  display.print("Hi, racer!", OLED_CENTER, OLED_CENTER);
  delay(2000);
}

9

#include "TroykaOLED.h" 
TroykaOLED display;
 
char text[] = "Amperka racing";
int length = strlen(text);
int symbolWidth = 12;
int width = symbolWidth * length;
 
void setup() {
  display.begin();
  display.setFont(alfabet12x16);
  display.autoUpdate(false);
}
 
void loop() {
  for (int x = 127; x > -width; x--) {
    display.clearDisplay();
    display.print(text, x, OLED_CENTER);
    display.update();
  }
}

10

#include "TroykaOLED.h"
 
TroykaOLED display;
 
const byte PROGMEM logo[] = { 
  11, 16,
  0x88, 0x50, 0x20, 0x8e, 0x51, 0x51, 0x51, 0x8e, 0x20, 0x50, 0x88,
  0x0f, 0x10, 0x27, 0x48, 0x50, 0x50, 0x50, 0x48, 0x27, 0x10, 0x0f
};
 
int position = 60;
int direction = 1;
 
void setup() {
  display.begin();
  display.autoUpdate(false);
}
 
void loop() {
  display.clearDisplay();
  display.drawImage(logo, position, OLED_CENTER);
  display.update();
 
  position += direction;
 
  if ((position > 115) || (position < 3)) {// 7
    direction *= -1; // 
  }
}

11

Библиотека TroykaColorSensor

#include "TroykaColorSensor.h"
 
TroykaColorSensor colorSensor;
 
void setup() {
  colorSensor.begin();
  Serial.begin(115200);
}
 
void loop() {
  analogWrite(13, analogRead(A5) / 4);
 
  RGB color = colorSensor.colorRead();
 
  Serial.print(color.red);
  Serial.print('\t');
  Serial.print(color.green);
  Serial.print('\t');
  Serial.println(color.blue);
 
  delay(100);
}

12

#include "TroykaColorSensor.h"
 
TroykaColorSensor colorSensor;
 
void printColor(RGB c) {
  if ((c.red > c.green) && (c.red > c.blue))
    Serial.println("RED");
  if ((c.green > c.red) && (c.green > c.blue))
    Serial.println("GREEN");
  if ((c.blue > c.green) && (c.blue > c.red))
    Serial.println("BLUE");
}
 
void setup() {
  colorSensor.begin();
  Serial.begin(115200);
}
 
void loop() {
  analogWrite(13, analogRead(A5) / 4);
  RGB color = colorSensor.colorRead();
  printColor(color);
  delay(100);
}

13

#include "TroykaColorSensor.h"
 
typedef enum {
  RED = 0, GREEN = 1, BLUE = 2, OTHER = 3
} ColorComponent;
 
  TroykaColorSensor colorSensor;
  int colors[4] = { 0, 0, 0, 0 };
  int totalCount = 0;
  int colorIs(RGB c) {
    if ((c.red > c.green) && (c.red > c.blue)) return RED;
    else if ((c.green > c.red) && (c.green > c.blue)) return GREEN;
    else if ((c.blue > c.green) && (c.blue > c.red)) return BLUE;
    else return OTHER;
}
 
void setup() {
colorSensor.begin();
Serial.begin(115200);
}
 
void loop() {
  analogWrite(13, analogRead(A5) / 4);
  if (digitalRead(10) == 0) {
    totalCount++;
    ColorComponent answer = colorIs(colorSensor.colorRead());
    colors[answer] += 1;
    Serial.println(totalCount);
    delay(500);
  }
  if (totalCount == 10) {
    Serial.print("Red: ");
    Serial.println(colors[RED]);
    Serial.print("Green: ");
    Serial.println(colors[GREEN]);
    Serial.print("Blue: ");
    Serial.println(colors[BLUE]);
    Serial.print("Other: ");
    Serial.println(colors[OTHER]);
    totalCount = 0;
    colors[0] = colors[1] = colors[2] = colors[3] = 0;
  }
}

14

#include "TroykaOLED.h"
#include "TroykaColorSensor.h"
 
typedef enum {
  RED = 0, GREEN = 1, BLUE = 2, OTHER = 3
} ColorComponent;
 
TroykaColorSensor colorSensor;
int colors[4] = { 0, 0, 0, 0 };
int totalCount = 0;
TroykaOLED display;
 
int colorIs(RGB c) {
  if ((c.red > c.green) && (c.red > c.blue)) return RED;
  else if ((c.green > c.red) && (c.green > c.blue)) return GREEN;
  else if ((c.blue > c.green) && (c.blue > c.red)) return BLUE;
  else return OTHER;
}
 
void setup() {
  colorSensor.begin();
  display.begin();
  display.clearDisplay();
}
 
void loop() {
  analogWrite(13, analogRead(A5) / 4);
  if (digitalRead(10) == 0) {        
    totalCount++;
    ColorComponent answer = colorIs(colorSensor.colorRead());
    colors[answer] += 1;
    display.setFont(alfabet12x16);
    display.clearDisplay();
    display.print(totalCount, OLED_CENTER, OLED_CENTER);
    delay(500);
    }
  if (totalCount == 10) {
    display.clearDisplay();
    display.setFont(alfabet6x8);
    display.print("Red: ", 0, 0); display.print(colors[RED],40, 0);
    display.print("Green: ", 0, 10); display.print(colors[GREEN], 40, 10);
    display.print("Blue: ", 0, 20);	display.print(colors[BLUE], 40, 20);
    display.print("Other: ", 0, 30); display.print(colors[OTHER], 40, 30);
    totalCount = 0;
    colors[0] = colors[1] = colors[2] = colors[3] = 0;
    delay(5000);
  }
}

15

#include "TroykaLedMatrix.h"
 
int left, right;
TroykaLedMatrix matrix;
 
byte diagram[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
 
void setup() {
  matrix.begin();
  Serial.begin(115200);
}
 
void loop() {
  right = analogRead(A0);
  left = analogRead(A3);
  diagram[0] = matrix.map(right, 0, 1023);
  diagram[7] = matrix.map(left, 0, 1023);
 
  Serial.print(right);
  Serial.print("\t");
  Serial.println(left);
 
  matrix.drawBitmap(diagram);
  delay(200);
}

16

#include "Dragster.h"
#include "PID_dragster.h"
#include "TroykaLedMatrix.h"
 
int left, right;
 
Dragster robot(MMAX_16_OHM);
 
TroykaLedMatrix matrix;
byte diagram[8];
float kp = 0.3;
float kd = 0.06;
float speed = 0.25;
double output;
PID regulator(&output, kp, 0.02, kd);
 
void setup() {
  robot.begin();
  matrix.begin();
}
 
void loop() {
  right = analogRead(A0);
  diagram[0] = matrix.map(right, 0, 1023);
  left = analogRead(A3);
  diagram[7] = matrix.map(left, 0, 1023);
  matrix.drawBitmap(diagram);
  float difference = (right - left) / 1023.0;
  regulator.compute(difference);
  robot.driveF(speed - output, speed + output);
}

17

#include "Servo.h"
 
Servo servo;
 
void setup() {
  servo.attach(9);
}
 
void loop() {
  int value = analogRead(A5);
  int angle = map(value, 0, 1023, 0, 180);
  servo.write(angle);
  delay(15);
}

21

#include "Servo.h"
 
#define BUTTONPIN 9
#define SERVOPIN 6
 
#define OPENED 160
#define CLOSED 100
 
Servo barrier;
int state = OPENED;
 
void setup() {
  barrier.attach(SERVOPIN);
  barrier.write(state);
}
 
void loop() {
  if (digitalRead(BUTTONPIN) == 0) {
    delay(100);
    if (digitalRead(BUTTONPIN) == 0) {
      for (int i = state;
        (state == OPENED) ? i >= CLOSED : i <= OPENED;
        (state == OPENED) ? i-- : i++) {
        barrier.write(i);
        delay(15);
      }
      state = (state == OPENED) ? CLOSED : OPENED;
    }
  }
}

22

Библиотека TroykaProximity

#include "TroykaProximity.h"
 
TroykaProximity sensor;
 
void setup() {
  sensor.begin();
  Serial.begin(115200);
}
 
void loop() {
  int range = sensor.readRange();
  if (range == 255) {
    Serial.println("Out of range");
  }
  else {
    Serial.println(range);
  }
  delay(200);
}

23

Библиотека QuadDisplay2

#include "QuadDisplay2.h"
#include "TroykaProximity.h"
 
#define DISTANCE 130
 
TroykaProximity proximity;
QuadDisplay quad(6);
long startTime = 0;
 
void setup() {
  quad.begin();
  proximity.begin();
}
 
void loop() {
  if (proximity.readRange() < DISTANCE) {
    startTime = millis();
    delay(2000);
  }
  float lapTime = millis() - startTime;
  quad.displayFloat(lapTime / 1000.0, 2);
}

24

#define INFRAREDPIN 5
#define REDPIN 6
#define YELLOWPIN 7
#define GREENPIN 9
 
void light(byte pin, int ms) {
  digitalWrite(pin, HIGH);
  delay(ms);
  digitalWrite(pin, LOW);
}
 
void light(byte pin1, byte pin2, int ms) {
  digitalWrite(pin1, HIGH);
  digitalWrite(pin2, HIGH);
  delay(ms);
  digitalWrite(pin1, LOW);
  digitalWrite(pin2, LOW);
}
 
void blinked(byte pin, int ms) {
  for (byte i = 0; i < 4; i++) {
    light(pin, ms);
    delay(ms);
  }
}
 
void setup() {
  pinMode(INFRAREDPIN, OUTPUT);
  pinMode(REDPIN, OUTPUT);
  pinMode(YELLOWPIN, OUTPUT);
  pinMode(GREENPIN, OUTPUT);
}
 
void loop() {
  light(REDPIN, 4000);
  light(REDPIN, YELLOWPIN, 2000);
  noTone(INFRAREDPIN);
  light(GREENPIN, 2000);
  blinked(GREENPIN, 250);
  tone(INFRAREDPIN, 38000);
  light(YELLOWPIN, 3000);
}