M i c r o p a s c a l / D e l p h i / 3 D П е ч а т ь
Пишем программу для PICBOT.
По пожелания читателей изменил свойства страницы, поставил растягивающую.
Продолжаю писать про 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.
Добавить комментарий