Вариант программируемого робота на робото-шасси "Робот-шпион"

/*                                                                                    */
/*                           (с) YrOM1k    ver. 0.5  29.03.2012                       */
/*                                              0.6  15.04.2012                       */

// подключаем нужные библиотеки
#include <Servo.h>         // управление серво-приводами, так-же нужна и для моторов. От неё зависит <LMotorShield.h>
#include <LMotorShield.h>  // управление моторами

/* используемые порты ->
        DIGIT_PORT_0    //  Rx
        DIGIT_PORT_1    //  Tx
        DIGIT_PORT_2    //  Brk2 антивыбег мотора 2
        DIGIT_PORT_3    //  PWM2 скорость мотора  2
        DIGIT_PORT_4    //  Brk1 антивыбег мотора 1
        DIGIT_PORT_5    //  Sig4 servo signal
        DIGIT_PORT_6    //  Sig3 servo signal
        DIGIT_PORT_7    //  Dir2 направление движения
        DIGIT_PORT_8    //  Dir1 направление движения
        DIGIT_PORT_9    //  Sig2 servo signal
        DIGIT_PORT_10   //  Sig1 servo signal
        DIGIT_PORT_11   //  PWM1 скорость мотора 1
        DIGIT_PORT_12   //  Led red
        DIGIT_PORT_13   //  Led green
        ANALOG_PORT_0   //  DIGIT_PORT_14 - ввод с клавиатуры
        ANALOG_PORT_1   //  none -
        ANALOG_PORT_2   //  none  | единственно, что осталось свободно,
        ANALOG_PORT_3   //  none  | если нужно подключить LCD-индикатор
        ANALOG_PORT_4   //  none -
        ANALOG_PORT_5   //  DIGIT_PORT_19 - переключатель режимов "RUN_MODE/PROGRAMM_MODE"
*/

// определяем значения
#define DIGIT_PORT_A         13  // светодиод-индикатор "RUN MODE" - "РЕЖИМ ИСПОЛНЕНИЯ"
#define DIGIT_PORT_B         12  // светодиод-индикатор "PROGRAMM MODE" - "РЕЖИМ ПРОГРАММИРОВАНИЯ"
#define PWM_1                11  // скорость первого ЭД
#define DIR_1                8   // напракление вращения ЭД №1
#define DIR_2                7   // напракление вращения ЭД №2
#define BRK_1                4   // антивыбег первого ЭД
#define PWM_2                3   // скорость второго ЭД
#define BRK_2                2   // антивыбег первого ЭД
#define ANALOG_PORT          0   // Аналоговый порт 0 - используем для клавиатуры
#define RUN_MODE             19  // используем как цифровой вход, для переключателя "RUN MODE-PROGRAMM MODE"
#define MAX_STEP_PROGRAMM    64  // Максимальое кол-во шагов в программе
#define COMMAND_MOVE_FORWARD 1   // Задаём команду "ДВИЖЕНИЕ ВПЕРЁД"
#define COMMAND_MOVE_BACK    2   // Задаём команду "ДВИЖЕНИЕ НАЗАД"
#define COMMAND_TURN_RIGHT   3   // Задаём команду "ПОВОРОТ ВПРАВО"
#define COMMAND_TURN_LEFT    4   // Задаём команду "ПОВОРОТ ВЛЕВО"
#define COMMAND_PAUSE_05S    5   // Задаём команду "ПАУЗА 0.5 сек"
#define COMMAND_PAUSE_10S    6   // Задаём команду "ПАУЗА 1.0 сек"
#define COMMAND_STOP         7   // Задаём команду "СТОП"
#define COMMAND_NONE         0   // нет команды
#define MOTOR_SPEED          254 // скорость моторов
//#define SIG_1                10 // сервопривод №1 - на будущее
//#define SIG_2                9  // сервопривод №2 - на будущее
//#define SIG_3                6  // сервопривод №3 - на будущее
//#define SIG_4                5  // сервопривод №4 - на будущее

// определяем глобальные переменные
int stick, last_stick; // переключатель RUN_MODE/PROGRAMM_MODE
int step_programm;// текущий шаг программы
int matrix_programm[MAX_STEP_PROGRAMM];// массив в котором будем храниться наша программа
char* text_RM = " -- RUN MODE --";
char* text_PM= " -- PROGRAMM MODE --";
char* text_S = " !!!    STOP    !!!";
char* text_P10 = " PAUSED 1.0 sec ";
char* text_P05 = " PAUSED 0.5 sec ";
char* text_L = " TURN LEFT ";
char* text_R = " TURN RIGHT ";
char* text_B = " MOVE BACK ";
char* text_F = " MOVE FORWARD ";
char* text_Y = " I like to move, move it.  --- (c)YrOM1k --- ";
LMotorShield lms; // определяем переменную, которой будем управлять мотороми м сервами через конструктор

// Предоопределяем используемые функции
void Turn_off_led();
void Blink_led(int);
int  Read_key();
int  Run_mode();
int  Programm_mode();
void Clear_matrix();

// Первичная инициализация программируемого робота
void setup(){
  pinMode( RUN_MODE,     INPUT);  // аналоговый порт-5 определяем как цифровой порт-19, на вход
  pinMode( DIGIT_PORT_A, OUTPUT); // порт к которому бодключен зелёный светодиод
  pinMode( DIGIT_PORT_B, OUTPUT); // порт к которому подключен красный светодиод
  Turn_off_led();                 
  Clear_matrix();
  step_programm = 0;
  last_stick = LOW;
  Serial.begin(9600);
  lms.begin(LMS_MOTORS | LMS_SERVOS);
  Serial.println();
  Serial.print(text_Y);   // говорим хеллоу
  Serial.println();
}

// Основная программаы
void loop(){
 
  stick = digitalRead(RUN_MODE);// считываем показание переключателя
  if (last_stick != stick ){    // определяем было-ли перключение режима
    last_stick = stick;
    step_programm = 0;
  }
  if ( stick == HIGH ){
    // если RUN_MODE - лог '1', то включен режим "исполнения"
    if ((step_programm == MAX_STEP_PROGRAMM) || (matrix_programm[0] == COMMAND_NONE)){
      // если кол-во шагов в программе достигло максимума, или после включения робота то ->
      Blink_led(DIGIT_PORT_A);   // если робот
      delay(100);                // выполнил всю
      Turn_off_led();            // программу то
      Blink_led(DIGIT_PORT_B);   // мигаем поочерёдно
      delay(100);                // зелёным а затем
      Turn_off_led();            // красным светодиодом
    }
    else{                        // иначе выполняем введённую программу
      lms.multipleMotorSpeed( LMS_MOTORS, MOTOR_SPEED ); // включаем оба мотора "ПОЛНЫЙ ВПЕРЁД"
      step_programm = Run_mode();
    }
  }
  else{
    // если RUN_MODE - лог '0', то включен режим "программирования"
    if (step_programm == MAX_STEP_PROGRAMM){  // если кол-во шагов в программе при вводе достигло максимума то ->
      Blink_led(DIGIT_PORT_B);                // мигаем красным светодиодом
      Turn_off_led();
      delay(100);
    }
    else{                                     // иначе вводим программу с предварительной очисткой памяти.
      Turn_off_led();
      Clear_matrix();
      step_programm = Programm_mode();
    }
  } 
}

// очистка матрицы, подготовка перед оечередным вводом программы
void Clear_matrix(){
  for (int index = 0; index < MAX_STEP_PROGRAMM; index++){
    matrix_programm[index] = 0;
  } 
}

// принудительно выключить светодиоды
void Turn_off_led(){
  digitalWrite( DIGIT_PORT_A, LOW );
  digitalWrite( DIGIT_PORT_B, LOW );
}

/* функция моргания светодиодом
        |-  DIGIT_PORT_A
        |   
       --- 
       \ / 
       -v-
        |
        |-  DIGIT_PORT_B
       
---------------------------------------
| DIGIT_PORT_A | DIGIT_PORT_B | color |
---------------------------------------
|     HIGH     |    LOW       | green |
---------------------------------------
|     LOW      |    HIGH      |  red  |
---------------------------------------
*/
void Blink_led(int port_AB){
  // включаем светодиод
  digitalWrite(port_AB, HIGH);
  delay(25);
}

// функция считывает с аналогового входа ANALOG_PORT
// значение и определяет какая кнопка была нажата
//
// кнопка 1 - MOVE FORWARD
// кнопка 2 - MOVE BACK
// кнопка 3 - TURN RIGHT
// кнопка 4 - TURN LEFT
// кнопка 5 - PAUSE 0.5 sec
// кнопка 6 - PAUSE 1.0 sec
// кнопка 7 - STOP
//
// ненажата - NONE
int Read_key(){
  int digit;// определяем локальную переменную
  int value = analogRead(ANALOG_PORT);
  digit = COMMAND_NONE; // принудительно устанавливаем "НЕТ КОМАНДЫ"
  if (value > 140 && value < 150 ){
    digit = COMMAND_STOP;
  }
  else{
    if (value > 165 && value < 175 ){
      digit = COMMAND_PAUSE_10S;
    }
    else{
      if (value > 200 && value < 210 ){
        digit = COMMAND_PAUSE_05S;
      }
      else{
        if (value > 250 && value < 260 ){
          digit = COMMAND_TURN_LEFT;
        }
        else{
          if (value > 340 && value < 350 ){
            digit = COMMAND_TURN_RIGHT;
          }
          else{
            if (value > 502 && value < 512 ){
              digit = COMMAND_MOVE_BACK;
            }
            else{
              if (value > 1015 ){
                digit = COMMAND_MOVE_FORWARD;
              }
            }
          }
        }
      }
    }
  }
  delay(250);
  return digit;
}

// останавливаем  ЭД - конец программы
void Stop(){
  Serial.println(text_S);
  lms.multipleMotorStop( LMS_MOTORS );
}
// пауза на одну секунду
void Pause10sec(){
  Serial.println(text_P10);
  lms.multipleMotorStop( LMS_MOTORS );
  delay(1000); // ***
  lms.multipleMotorRun( LMS_MOTORS );
}
// пауза на пол-секунды
void Pause05sec(){
  Serial.println(text_P05);
  lms.multipleMotorStop( LMS_MOTORS );
  delay(500); // ***
  lms.multipleMotorRun( LMS_MOTORS );
}
// поворот в лево
void Left(){
  Serial.println(text_L);
  lms.motorForward( 1 );
  lms.motorBackward( 2 );
  lms.multipleMotorRun( LMS_MOTORS );
  delay(1000); // ***
}
// поворот в право
void Right(){
  Serial.println(text_R);
  lms.motorForward( 2 );
  lms.motorBackward( 1 );
  lms.multipleMotorRun( LMS_MOTORS );
  delay(1000); // ***
}
// движение назад
void Back(){
  Serial.println(text_B);
  lms.multipleMotorBackward( LMS_MOTORS );
  lms.multipleMotorRun( LMS_MOTORS );
  delay(5000); // ***
}
// движение вперёд
void Forward(){
  Serial.println(text_F);
  lms.multipleMotorForward( LMS_MOTORS );
  lms.multipleMotorRun( LMS_MOTORS );
  delay(5000); // ***
}

// функция исполнения введённой программы
// мигаем зелёным светодиодом DIGIT_PORT_A
int Run_mode(){
  int command;
  Serial.println();
  Serial.print(text_RM);
  Serial.println();
  int  index = 0;
  do{
    command = matrix_programm[index];
    Blink_led(DIGIT_PORT_A);              // во время выполнения, каждую команду подтверждаем миганием зелёным светодиодом
    Turn_off_led();
    Serial.print("command --> ");
    if ( command == COMMAND_MOVE_FORWARD ){
      Forward();
    }
    if ( command == COMMAND_MOVE_BACK ){
      Back();
    }
    if ( command == COMMAND_TURN_RIGHT ){
      Right();
    }
    if ( command == COMMAND_TURN_LEFT ){
      Left();
    }
    if ( command == COMMAND_PAUSE_05S ){
      Pause05sec();
    }
    if ( command == COMMAND_PAUSE_10S ){
      Pause10sec();
    }
    if ( command == COMMAND_STOP){
      Stop();
      index = MAX_STEP_PROGRAMM - 1;
    }
    index++;
  }while ( index < MAX_STEP_PROGRAMM );
  return index;
}

// функция программирования робота
// мигаем красным светодиодом DIGIT_PORT_B
int Programm_mode(){
  Serial.println();
  Serial.print(text_PM);
  Serial.println();
  int  button;
  int  index = 0;
  do{            // основной цикл ввода команд
    Turn_off_led();
    do{          // пока не нажата коммандная кнопка
      button = Read_key();
    }while(button == COMMAND_NONE);
    Blink_led(DIGIT_PORT_B);          // после ввода очередной команды мигнём красным светодиодом
    matrix_programm[index] = button;
   
    // ЛОВУШКА !!!
    Serial.print(" matrix_programm[");
    Serial.print(index);
    Serial.print("] = ");
    Serial.println(matrix_programm[index]);
    // ЛОВУШКА !!!
   
    if (button == COMMAND_STOP){   // если введена команда "СТОП"
      index = MAX_STEP_PROGRAMM - 1;
    }
    index++;
  }while( index < MAX_STEP_PROGRAMM );
  return index;
}