//********************************************************************
//  Filename:  ROBO_SWG_1.ppas  Date:  16.09.2008                    *
//  File Version:  0.01                                              *
//  Author:      S.W.G.   vsivkov@online.kz                          *
//********************************************************************
// * Project name:
//    ROBO_SWG_1
// * Copyright: (c) Сивков В.Г., 2008
// * Revision History:  20080916:
// * Description:
//
// * Test configuration:
//     MCU:             PIC16F873A
//     Dev.Board:       ROBO_1.PCB
//     Oscillator:      HS, 4.000 MHz
//     Ext. Modules:    None
//     SW:              mikroPascal v8.1
// * NOTES:


program ROBO_SWG_1;
var  //  Начало блока переменных.
  SSP_1  : byte; //Слово состояния программы. Исходное - "0"
//   Побитно:
//SSP_1.0 - "1"- Работа ДВ_ЛЕВ  двигателя не закончена.(Прошло врем.< заданного)
//SSP_1.1 - "1"- Работа ДВ_ПРАВ двигателя не закончена.(Прошло врем.< заданного)
//SSP_1.2 - "1"- Обнаружено препятствие.
//SSP_1.3 - "1"- Отрабатывается обьезд препятствия.
//SSP_1.4 - "1"- Стартовая полярность ("0") на линии приема.
//SSP_1.5 - "1"- Осуществляется прием команды.
//SSP_1.6 - "1"- Осуществляется передача команды.
//SSP_1.7 - "1"- Аварийный останов с подачей сигнала "Помощь".

  SSP_2  : byte; // Слово состояния программы дополнительное. Исходное - "0"
//SSP_2.0 - "1"- Состояние "Покой". Ожидание новых команд.
//SSP_2.1 - "1"- Состояние "Аварийный останов". Ожидание новых команд.
//SSP_2.2 - "1"- Состояние "Исполнение".Выполнение уже имеющихся команд.
//SSP_2.3 - "1"- Прием команды с радиоканала.
//SSP_2.4 - "1"- Прием команды с I2C.
//SSP_2.7 - "1"- Дешифрация команды.
//SSP_2.8 - "1"- Резерв.
//-----------------------------------------------------------
// Переменные для ДВ_ЛЕВ:
//  DWL_1 : byte; // - Время работы с последней команды движения.
//  DWL_2 : byte; // - Время работы заданное.(Может исп. как декр.счетчик ост.вр)
  DWL_3 : byte; // - Коэффициент ШИМ (0-255).
//  DWL_4 : byte; // - Режим (Стоп, Движ.в теч.задан.врем, Непр.движ, Впер-назад)
//  DWL_5 : byte; // - Пройденный путь относительное с посл.команды движ.(1 байт)
//  DWL_6 : byte; // - Пройденный путь полный (1 байт из 2?).
//  DWL_7 : byte; // - Пройденный путь полный (2 байт из 2?).
//  DWL_8 : byte; // - Резерв.
//-----------------------------------------------------------
// Переменные для ДВ_ПРАВ:
//  DWP_1 : byte; // - Время работы с последней команды движения.
//  DWP_2 : byte; // - Время работы заданное.(Может исп. как декр.счетчик ост.вр)
  DWP_3 : byte; // - Коэффициент ШИМ (0-255).
//  DWP_4 : byte; // - Режим (Стоп, Движ.в теч.задан.врем, Непр.движ, Впер-назад)
//  DWP_5 : byte; // - Пройденный путь относительное с посл.команды движ.(1 байт)
//  DWP_6 : byte; // - Пройденный путь полный (1 байт из 2?).
//  DWP_7 : byte; // - Пройденный путь полный (2 байт из 2?).
//  DWP_8 : byte; // - Резерв.
//===========================================================

//  temp_res : word;
  current_duty_1, old_duty_1 : byte;
  current_duty_2, old_duty_2 : byte;
  
procedure InitMain();
begin
  USART_Init(9600);  // initalize USART (19200 baud rate, 1 stop bit, no parity)
 // ANSEL  := $04;   // configure AN2 pin as analog input
 // ANSELH := 0;     // configure other AN pins as digital I/O
  ADCON1.1 := 1;
  ADCON1.2 := 1;
  ADCON1.3 := 0;
  PORTA := 0;
  TRISA := $F0;      // configure PORTA 4-5 pins as input, 3-0 pins as output
  PORTB := 0;        // set PORTB to 0
  TRISB := $FF;      // designate PORTB pins as input
  OPTION_REG.7 := 0; // Подтягивающие резисторы порта B вкл!
  PORTC := 0;        // set PORTC to 0
  TRISC := $B8;      // designate PORTC 0,1,2,6 pins as out, 3,4,5,7,-as input
  
  PWM1_Init(500);    // initialize PWM1 module at 0,5 KHz
  PWM2_Init(500);    // initialize PWM1 module at 0,5 KHz
end;

begin
  initMain();
  current_duty_1 := 100;  // initial value for current_duty (0-255)
  old_duty_1     := 0;  // old_duty will keep the 'old current_duty' value
  current_duty_2 := 100;  // initial value for current_duty (0-255)
  old_duty_2     := 0;  // old_duty will keep the 'old current_duty' value
  PWM1_Start();
  PWM2_Start();
//=============== Основной цикл! ============================
  while TRUE do
    begin    // endless loop
      SSP_1.5 := 0;          // НЕТ команды.
      Delay_ms(1);                    // Притормозить движки?
      if (Button(PORTB, 4,1,0)) then  // button on RB4 pressed = "ЕДЕМ ВПЕРЕД!"
        begin // ЕДЕМ ВПЕРЕД!
          SSP_1.5 := 1;               // Осуществляется прием команды.
          PORTA.0 := 1;
          PORTA.1 := 0;
          PORTA.2 := 1;
          PORTA.3 := 0;
        end;
      if (Button(PORTB, 5,1,0)) then  // button on RB5 pressed = "ЕДЕМ НАЗАД!"
        begin // ЕДЕМ НАЗАД!
          SSP_1.5 := 1;               // Осуществляется прием команды.
          PORTA.0 := 0;
          PORTA.1 := 1;
          PORTA.2 := 0;
          PORTA.3 := 1;
        end;
      if (Button(PORTB, 6,1,0)) then  // button on RB6 pressed = "ВЛЕВО!"
        begin //  ВЛЕВО НА МЕСТЕ!
          SSP_1.5 := 1;               // Осуществляется прием команды.
          PORTA.0 := 0;
          PORTA.1 := 1;
          PORTA.2 := 1;
          PORTA.3 := 0;
        end;
      if (Button(PORTB, 7,1,0)) then  // button on RB7 pressed = "ВПРАВО!"
        begin //  ВПРАВО НА МЕСТЕ!
          SSP_1.5 := 1;               // Осуществляется прием команды.
          PORTA.0 := 1;
          PORTA.1 := 0;
          PORTA.2 := 0;
          PORTA.3 := 1;
        end;
      if SSP_1.5 = 0 then              // НЕТ команды.
        begin  // СТОИМ!
          PORTA.0 := 0;
          PORTA.1 := 0;
          PORTA.2 := 0;
          PORTA.3 := 0;
          SSP_1.0 := 0;   //Работа ДВ_ЛЕВ  закончена.(Прошло врем.> задан.)
          SSP_1.1 := 0;   //Работа ДВ_ПРАВ закончена.(Прошло врем.> задан.)
       end
     else
       begin
          SSP_1.0 := 1;   //Работа ДВ_ЛЕВ  НЕ закончена.(Прошло врем.< задан.)
          SSP_1.1 := 1;   //Работа ДВ_ПРАВ НЕ закончена.(Прошло врем.< задан.)
       end;

// Тест ШИМ.
      if (Button(PORTB, 0,1,0)) then       // button on RB0 pressed
        begin
          if current_duty_1 < 255 then
             Inc(current_duty_1);          // increment current_duty_1
          if current_duty_2 < 255 then
             Inc(current_duty_2);          // increment current_duty_2
        end;
      if (Button(PORTB, 1,1,0)) then       // button on RB1 pressed
        begin
          if current_duty_1 > 0 then
             Dec(current_duty_1);           // decrement current_duty_1
          if current_duty_2 > 0 then
             Dec(current_duty_2);           // decrement current_duty_2
        end;
      if ((old_duty_1 <> current_duty_1) or (old_duty_2 <> current_duty_2)) then
        begin                                            	 // if change in duty cycle requested
          PWM1_Change_Duty(current_duty_1); // set new duty ratio,
          old_duty_1 := current_duty_1;     // memorize it
          DWL_3 := current_duty_1;          // Коэффициент ШИМ ДВ_ЛЕВ (0-255)
          PWM2_Change_Duty(current_duty_2); // set new duty ratio,
          old_duty_2 := current_duty_2;     // memorize it
          DWP_3 := current_duty_2;          // Коэффициент ШИМ ДВ_ПРАВ (0-255)
          USART_Write('#');                 // send '#' as byte
          USART_Write(current_duty_1);      // send current_duty_1 as byte
          USART_Write(current_duty_2);      // send current_duty_2 as byte
          USART_Write(09);                  // send 'CR' as byte
        end;
      Delay_ms(50);                         // slow down change pace a little
    end;      // endless loop
//=================== Конец - Основной цикл! ==========================
end.