Gripper: pick up and place

// Шаблон среднего уровня. Напиши код в разделе «Твоя часть» по целям в комментариях. #include <Servo.h> // Контакт сервопривода захвата: впиши номер из таблицы проводки своего робота.const int SERVO_PIN = -1;int angle = 90; // меняй понемногу, чтобы найти «открыто» и «закрыто» Servo gripper; // ===== Твоя часть: напиши сам ===== void setup() {  if (SERVO_PIN < 0) {    while (true) {    }  }   // Захват поворачивается на угол angle и держит его.} void loop() {}

// Шаблон среднего уровня. Напиши код в разделе «Твоя часть» по целям в комментариях. #include <Servo.h>const int LEFT_SENSOR_PIN = A4;const int RIGHT_SENSOR_PIN = A5;const int LEFT_DIR_PIN = 7;const int LEFT_SPEED_PIN = 6;const int RIGHT_DIR_PIN = 4;const int RIGHT_SPEED_PIN = 5;const int FORWARD = HIGH;const int BACKWARD = (FORWARD == HIGH) ? LOW : HIGH; // Измерения и настройки из журнала (уроки 3.1 и 3.2).const int LEFT_WHITE = 0;const int LEFT_BLACK = 0;const int RIGHT_WHITE = 0;const int RIGHT_BLACK = 0;float kp = 0.5;int baseSpeed = 100;int leftLevel = 100;int rightLevel = 100; // Контакты ультразвукового датчика: впиши номера из таблицы проводки своего робота.const int TRIG_PIN = -1;const int ECHO_PIN = -1;int turnSpeed = 100;int centerMs = 250; // подбираем: проехать от датчиков до центра перекрёсткаint leaveMs = 200;  // время, чтобы датчик ушёл со старой линии // Сервопривод захвата: впиши контакт и свои углы из журнала.const int SERVO_PIN = -1;const int OPEN_ANGLE = 90;const int CLOSED_ANGLE = 40;const int STOP_CM = 5; Servo gripper;const unsigned long CARRY_MS = 1500; // сколько ехать по линии с грузом // ===== Твоя часть: напиши сам ===== void setupGripper() {  // Без контакта сервопривода программа дальше не идёт.  // Захват подключён и открыт; дай ему время раскрыться.} void pickUp() {  // Взять груз: груз оказывается в захвате и не выталкивается.} void putDown() {  // Поставить груз: груз остаётся на месте, когда робот отъезжает назад на 300 мс; в конце колёса стоят.} void setup() {  setupSonar();  setupGripper();  setupRobot();   // Доставка: подъехать к грузу, взять, развернуться на линии, проехать с грузом CARRY_MS, поставить.} // ===== Готовое из уроков 3.2–5.1 — не меняй ===== int level(int value, int white, int black) {  long result = (long)(value - black) * 100 / (white - black);  return constrain(result, 0, 100);} void readSensors() {  leftLevel = level(analogRead(LEFT_SENSOR_PIN), LEFT_WHITE, LEFT_BLACK);  rightLevel = level(analogRead(RIGHT_SENSOR_PIN), RIGHT_WHITE, RIGHT_BLACK);} void drive(int leftDir, int leftSpeed, int rightDir, int rightSpeed) {  digitalWrite(LEFT_DIR_PIN, leftDir);  digitalWrite(RIGHT_DIR_PIN, rightDir);  analogWrite(LEFT_SPEED_PIN, leftSpeed);  analogWrite(RIGHT_SPEED_PIN, rightSpeed);} void stopMotors() {  analogWrite(LEFT_SPEED_PIN, 0);  analogWrite(RIGHT_SPEED_PIN, 0);} void followLineStep() {  int error = leftLevel - rightLevel;  int correction = kp * error;  drive(FORWARD, constrain(baseSpeed + correction, 0, 255),        FORWARD, constrain(baseSpeed - correction, 0, 255));} void setupRobot() {  pinMode(LEFT_DIR_PIN, OUTPUT);  pinMode(LEFT_SPEED_PIN, OUTPUT);  pinMode(RIGHT_DIR_PIN, OUTPUT);  pinMode(RIGHT_SPEED_PIN, OUTPUT);  stopMotors();  // Без измерений робот не поедет.  if (LEFT_WHITE == LEFT_BLACK || RIGHT_WHITE == RIGHT_BLACK) {    while (true) {    }  }  delay(2000);} // Расстояние в сантиметрах или -1, если эхо не вернулось.int readDistanceCm() {  digitalWrite(TRIG_PIN, LOW);  delayMicroseconds(2);  digitalWrite(TRIG_PIN, HIGH);  delayMicroseconds(10);  digitalWrite(TRIG_PIN, LOW);  unsigned long echoUs = pulseIn(ECHO_PIN, HIGH, 25000);  if (echoUs == 0) {    return -1;  }  return echoUs / 58;} void setupSonar() {  // Без контактов датчика программа дальше не пойдёт.  if (TRIG_PIN < 0 || ECHO_PIN < 0) {    while (true) {    }  }  pinMode(TRIG_PIN, OUTPUT);  pinMode(ECHO_PIN, INPUT);} // Выехать на центр перекрёстка: датчики стоят впереди колёс.void toCenter() {  drive(FORWARD, baseSpeed, FORWARD, baseSpeed);  delay(centerMs);  stopMotors();} // Крутиться на месте, пока датчик со стороны поворота не найдёт новую линию.void turnUntilLine(bool left) {  if (left) {    drive(BACKWARD, turnSpeed, FORWARD, turnSpeed);  } else {    drive(FORWARD, turnSpeed, BACKWARD, turnSpeed);  }  delay(leaveMs);  do {    readSensors();  } while ((left ? leftLevel : rightLevel) > 30);  stopMotors();} void turnLeftAtCrossing() {  toCenter();  turnUntilLine(true);} void turnRightAtCrossing() {  toCenter();  turnUntilLine(false);} // Развернуться на линии: крутиться, пока линия снова не окажется под датчиком.void turnAroundOnLine() {  turnUntilLine(true);} // Ехать по линии заданное время.void followLineFor(unsigned long ms) {  unsigned long start = millis();  while (millis() - start < ms) {    readSensors();    followLineStep();  }  stopMotors();} // Ехать по линии и остановиться перед грузом по ультразвуку.void approachCan() {  while (true) {    int distance = readDistanceCm();    if (distance > 0 && distance <= STOP_CM) {      break;    }    readSensors();    followLineStep();  }  stopMotors();} void loop() {}