Posts tagged PASCAL

По пожелания читателей изменил свойства страницы, поставил растягивающую.

Продолжаю писать про PICBOT.

Первые действия, устанавливает на загрузку библиотеки,  которые нам нужны.

ФОТО

Есть основные несколько процедур для управления двигателями.

procedure   Change_Duty(speedA,speedB:byte;);// ************* ИСМЕНЕНИЕ СКОРОСТИ ДВИГАТЕЛЕЙ**********

begin

motor_duty_A:=speedA; // Save for old speed

motor_duty_B:=speedB;

PWM1_Set_Duty(speedA); // Motor A

PWM2_Set_Duty(speedB); // Motor B

end;

procedure Start_motor; //********** Motor START ********/

begin

PWM1_Start();

PWM2_Start();

end;

procedure Stop_motor; //********** Motor STOP ********/

begin

PWM1_Stop();

PWM2_Stop();

end;

procedure Motor_A_FWD;                 //********** Motor A Forward ********/

begin

Pwm1_Start();

PORTD.0 :=0;

PORTD.1 :=1;

end;

procedure Motor_B_FWD;           //********** Motor B Forward ********/

begin

Pwm2_Start;

PORTB.1 :=0;

PORTB.2 :=1;

end;                //************************************/

procedure  Motor_A_BWD; //********** Motor A Backward *******/

begin

Pwm1_Start;

PORTD.0 :=1;

PORTD.1 :=0;

end;                //************************************/

procedure  Motor_B_BWD ; //********** Motor B Backward *******/

begin

Pwm2_Start;

PORTB.1 :=1;

PORTB.2 :=0;

end;                //************************************/

procedure Motor_A_Off;                //********** Motor A Off ************/

begin

Pwm1_Stop;

PORTD.0 :=0;

PORTD.1 :=0;

end;                //************************************/

procedure Motor_B_Off;                //********** Motor B Off ************/

begin

Pwm2_Stop;

PORTB.1 :=0;

PORTB.2 :=0;

end;                //************************************/

procedure Motor_Forward(speedA,speedB:byte);                //********** Go Forward ************/

begin

Motor_Init;

Change_Duty(speedA,speedB);

Motor_A_FWD;

Motor_B_FWD;

end;                //************************************/

procedure Motor_Backward(speedA,speedB:byte);                //********** Go Backward ************/

begin

Motor_Init;

Change_Duty(speedA,speedB);

Motor_A_BWD;

Motor_B_BWD;

end;                //************************************/

procedure Motor_S_Right(speedA,speedB:byte);                //********** Spin Left *************/

begin

Motor_Init;

Change_Duty(speedA,speedB);

Motor_A_FWD;

Motor_B_BWD;

end;                //************************************/

procedure Motor_S_Left(speedA,speedB:byte);                //********** Spin Right ************/

begin

Motor_Init;

Change_Duty(speedA,speedB);

Motor_A_BWD;

Motor_B_FWD;

end;                //************************************/

procedure  Motor_Stop;                //********** Stop Motor ************/

begin

Motor_Init;

Change_Duty(0,0);

Motor_A_Off;

Motor_B_Off;

end;                //************************************/

PROCEDURE  ADC_INIT;          // *****************НАСТРАИВАЕМ        GP2D120    ПОД ДАЛЬНОМЕР

BEGIN

INTCON := 0;

OPTION_REG := $80;

ADCON1     := $82;

TRISB.6:=0;                //  НАСТРОЙКА ПОД ФАРЫ КА ВЫХОДЫ

TRISB.7:=0;                //  НАСТРОЙКА ПОД ФАРЫ КА ВЫХОДЫ

TRISA.2    := 1;            // УСТАНАВЛИВАЕМ RA2 КАК ВХОД

ANSEL  := 0x04;              // НАСТРАИВАЕМ ПОРТ  AN2 КАК АНАЛОГОВЫЙ ВХОД

ANSELH := 0;

END; //************************************/

procedure  ADC_READ_GP2D120_PICBOT;    //  ОПРЕДЕЛЯЕМ РАСТОЯНИЕ ДО ПРЕДМЕТА

begin

A:=0;

adc_rd:= ADC_read(2);           // СЧИТЫВАЕМ ИНФОРМАЦИЮ С ДАЛЬНОМЕРА

adc_rd_h := adc_rd div 78 ;          // ПРОДГОНЯЕМ  ВЫХОДНЫЕ ДАННЫЕ

adc_rd_l := (adc_rd mod 78) div 7;  //  С АЦП ПОД ВОЛЬТЫ

A:=byte(adc_rd_h);                // И ПЕРЕВОДИМ В ЧИСЛУЮ ДАННУЮ БАЙТ

IF A>=3 THEN BEGIN PORTB.6:=1; PORTB.7:=0; END //  ЗАЖИГАЕМ ФАРЫ ЗЕЛЁНЫЕ СИНИ ВЫКЛЮЧАЕМ ЕСЛИ ЕСТЬ ПРЕПЯТСТВИЕ

ELSE BEGIN PORTB.6:=0; PORTB.7:=1; END

end;

Вот основной код программы которая двигает робот.

begin

ADC_INIT;   // Устанавливаем настройки АЦП

motor_drive:=0;  //

motor_init_:=0;  //

Motor_Init;      //  устанавливаем настройки на драйвер двигателя

ADC_READ_GP2D120_PICBOT;    //  ОПРЕДЕЛЯЕМ РАСТОЯНИЕ ДО ПРЕДМЕТА

Start_motor;                // ЗАПУСКАЕМ ДВИГАТЕЛИ

error := Soft_UART_Init(PORTC, 7, 6, 1200, 1); // УСТАНАВЛИВАЕМ УАРТ НА ПРИЁМ КОМАНД

Delay_ms(100); // ЗАДЕРЖКА НЕБОЛЬШАЯ

M1:

byte_read := Soft_UART_Read(error);  // СЧИТЫВАЕМ КОМАНДЫ С РАДИОМОДУЛЯ

while TRUE do                            //

begin

ADC_READ_GP2D120_PICBOT; //  ОПРЕДЕЛЯЕМ РАСТОЯНИЕ ДО ПРЕДМЕТА

IF A> 3 THEN              // ЕСЛИ ОПРЕДЕЛЁН ПРЕДМЕТ ТО

CASE motor_drive OF

0:  BEGIN Motor_Backward(255,255);  // ЕСЛИ  СТОИМ НАМЕСТЕ , и есть приближение предмета  ТО ОТЕЖАЕМ И ОСТАНАВЛИВАЕМСЯ

repeat

BEGIN

ADC_READ_GP2D120_PICBOT;

END;

until A <3 ;

Motor_Stop;

END;

5: BEGIN                       // ЕСЛИ ДВИГАЕМСЯ ВПЕРЁД ТО НУЖНО  ОТЪЕХАТЬ НАЗАД И ПОВЕРНУТЬ И СНОВА ЕХАТЬ ВПЕРЁД

Motor_Backward(255,255);

repeat

BEGIN

ADC_READ_GP2D120_PICBOT;

END;

until A <3 ;

Motor_S_Left(255,255);

Delay_ms(300);

IF motor_drive=5 THEN  Motor_Forward(255,255)

ELSE Motor_Stop;

END;

END;

if byte_read <> 230 then goto m1;

byte_read := Soft_UART_Read(error);

if (error = 0) then                 // ЕСЛИ НЕТ ОШИБОК

BEGIN

//————————————————————————

//————————————————————————

CASE byte_read OF

17:BEGIN  Motor_Forward(255,255);motor_drive:=5;    END;

12: BEGIN Motor_Backward(255,255);  motor_drive:=1; END;

15: BEGIN Motor_Stop;               motor_drive:=0; END;

14: BEGIN Motor_S_Left(255,255);    motor_drive:=3; END; // Call Spin Left END;

27: BEGIN Motor_S_Right(255,255);   motor_drive:=4; END;

END;

//————————————————————————————

//————————————————————————

END;

END;

end.

end.

Продолжение следует.