Форум

#include <motor2.h>
int Adc;
char txt[6];

// Запуск подключения LCD
sbit LCD_RS at RD2_bit;
sbit LCD_EN at RD3_bit;
sbit LCD_D4 at RD4_bit;
sbit LCD_D5 at RD5_bit;
sbit LCD_D6 at RD6_bit;
sbit LCD_D7 at RD7_bit;

sbit LCD_RS_Direction at TRISD2_bit;
sbit LCD_EN_Direction at TRISD3_bit;
sbit LCD_D4_Direction at TRISD4_bit;
sbit LCD_D5_Direction at TRISD5_bit;
sbit LCD_D6_Direction at TRISD6_bit;
sbit LCD_D7_Direction at TRISD7_bit;
// Окончание подключения LCD

void Read_Adc() // Вычисление текущего статуса ИК дальномера
{
ADCON0=0b11001001; // Выбор Analog2 RC_Mode и ADON
ADCON0.GO=1; // Запуск преобразования
while(ADCON0.GO); // Окончание преобразования
Adc=(ADRESH*4)+(ADRESL/64); // Объединение данных
}
void main()
{
Lcd_Init(); // Инициализация LCD
Lcd_Cmd(_LCD_CURSOR_OFF); // Выключение курсора LCD
Lcd_Cmd(_LCD_CLEAR); // Выключение курсора LCD
TRISA.RA4 = 1; // Установка кнопки RA4 на ввод
Lcd_Out(1,1,"Press RA4 "  ;)  ; // Вывод текста первой строки
while (PORTA.RA4 == 1);

MotorA_CurrentSpeed = 255; // Текущая скорость двигателя А
MotorB_CurrentSpeed = 255; // Текущая скорость двигателя В
Delay_ms(1000);

Lcd_Cmd(_LCD_CURSOR_OFF); // Выключение курсора LCD
Lcd_Out(1,1,"Raw Data= "  ;)  ; // Вывод текста первой строки
while(1)
{
Read_Adc(); // Запрос текущего статуса ИК дальномера
WordToStr(Adc,txt); // Преобразование результата запроса в строку
Lcd_Out(1,10,txt); // Отображение результата преобразования на LCD
if (Adc<90) // Если Adc<90, то нет препятствий в зоне видимости
{
Lcd_Out(2,1,"Forward"  ;)  ; // Движение вперед
Forward(MotorA_CurrentSpeed, MotorB_CurrentSpeed);
}
else
{
Adc = (2914/(Adc+5))-1; // Преобразование результата в сантиметры
WordToStr(Adc,txt); // Преобразование результата в строку
Lcd_Out(2,1,"In CM= "  ;)  ; // Отображение результата на LCD
Lcd_Out(2,6,txt);

{
Backward(MotorA_CurrentSpeed, MotorB_CurrentSpeed);
Delay_ms(500); // Движение назад в течение задержки

RotateLeft(MotorA_CurrentSpeed, MotorB_CurrentSpeed);
Delay_ms(500); // Разворот влево в течение задержки

Lcd_Out(2,1,"Forward"  ;)  ; // Движение вперед
Forward(MotorA_CurrentSpeed, MotorB_CurrentSpeed);
}
}
// Delay_ms(1000);
}

}

unsigned short MotorA_Speed = 127; // Ширина импульса ШИМ по умолчанию 50%
unsigned short MotorB_Speed = 127; // для обоих двигателей

// Переменные для работы с двигателями
unsigned short MotorA_Status = 0; // 0 - Выключен, 1 - Вперед, 2 - Назад
unsigned short MotorB_Status = 0;

unsigned short MotorA_CurrentSpeed = 255;
unsigned short MotorB_CurrentSpeed = 255;

unsigned short MotorInitStatus = 0; // Статус функции инициализации

const unsigned long PWMFreq = 5000;// Частота ШИМ

// Подключение моторов:
// *** Motor A *****
// RD0 ====> 1A
// RD1 ====> 1B
// RC2 ====> 1E (PWM1)

// *** Motor B *****
// RB1 ====> 2A
// RB2 ====> 2B
// RC1 ====> 2E (PWM2)

// Инициализация модулей ШИМ и двигателей
void MotorInit (void){
 if (MotorInitStatus == 0) { // Это первый вызов функции?
 MotorInitStatus = 1; // Установка состояния
 ANSELH.F0 = 0; // RB1 - цифровой вход/выход
 ANSELH.F2 = 0; // RB2 - цифровой вход/выход
 TRISB.RB1 = 0; // Motor B 2A
 TRISB.RB2 = 0; // Motor B 2B
 TRISD.RD0 = 0; // Motor A 1A
 TRISD.RD1 = 0; // Motor A 1B
 PWM1_Init(PWMFreq); // Инициализация модулей ШИМ
 PWM2_Init(PWMFreq);
 }
}

// Управление скоростью двигателей
void MotorA_ChangeSpeed (unsigned short NewSpeed) {
 if (NewSpeed != MotorA_Speed) {
 MotorA_Speed = NewSpeed;
 PWM1_Set_Duty(MotorA_Speed);
 }
}

void MotorB_ChangeSpeed (unsigned short NewSpeed) {
 if (NewSpeed != MotorB_Speed) {
 MotorB_Speed = NewSpeed;
 PWM2_Set_Duty(MotorB_Speed);
 }
}

// Двигатель А - Команда "вперед"
void MotorA_FWD (void) {
 PWM1_Start();
 PORTD.RD0 = 0;
 PORTD.RD1 = 1;
 MotorA_Status = 1;
}

// Двигатель B - Команда "вперед"
void MotorB_FWD (void) {
 PWM2_Start();
 PORTB.RB1 = 0;
 PORTB.RB2 = 1;
 MotorB_Status = 1;
}

// Двигатель А - Команда "назад"
void MotorA_BWD (void) {
 PWM1_Start();
 PORTD.RD0 = 1;
 PORTD.RD1 = 0;
 MotorA_Status = 2;
}

// Двигатель B - Команда "назад"
void MotorB_BWD (void) {
 PWM2_Start();
 PORTB.RB1 = 1;
 PORTB.RB2 = 0;
 MotorB_Status = 2;
}

// Двигатель А - Команда "стоп"
void MotorA_Stop (void) {
 PWM1_Stop();
 PORTD.RD0 = 0;
 PORTD.RD1 = 0;
 MotorA_Status = 0;
}

// Двигатель B - Команда "стоп"
void MotorB_Stop (void) {
 PWM2_Stop();
 PORTB.RB1 = 0;
 PORTB.RB2 = 0;
 MotorB_Status = 0;
}

// Команда "движение вперед"
void Forward (unsigned short MotorA_Speed, unsigned short MotorB_Speed) {
 MotorInit();
 MotorA_ChangeSpeed(MotorA_Speed);
 MotorB_ChangeSpeed(MotorB_Speed);
 MotorA_FWD();
 MotorB_FWD();
}

// Команда "движение назад"
void Backward (unsigned short MotorA_Speed, unsigned short MotorB_Speed) {
 MotorInit();
 MotorA_ChangeSpeed(MotorA_Speed);
 MotorB_ChangeSpeed(MotorB_Speed);
 MotorA_BWD();
 MotorB_BWD();
}

// Команда "разворот вправо"
void RotateRight (unsigned short MotorA_Speed, unsigned short MotorB_Speed) {
 MotorInit();
 MotorA_ChangeSpeed(MotorA_Speed);
 MotorB_ChangeSpeed(MotorB_Speed);
 MotorA_FWD();
 MotorB_BWD();
}

// Команда "разворот влево"
void RotateLeft (unsigned short MotorA_Speed, unsigned short MotorB_Speed) {
 MotorInit();
 MotorA_ChangeSpeed(MotorA_Speed);
 MotorB_ChangeSpeed(MotorB_Speed);
 MotorA_BWD();
 MotorB_FWD();
}

// Команда "стоп"
void Stop (void) {
 MotorA_ChangeSpeed(0);
 MotorB_ChangeSpeed(0);
 MotorA_Stop();
 MotorB_Stop();
 MotorInitStatus = 0;
}