Arduino红外避障

ID:672963 · 发表于 2019-12-29 10:23

#include <LiquidCrystal.h>
// initialize the library by associatingany needed LCD interface pin
// with the arduino pin number it isconnected to
const int rs = 53, en = 49, d4 = 47, d5 =45, d6 = 43, d7 = 41.;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
#define INA1 22 //TB6612FNG驱动模块控制信号共6个
#define INA2 26
#define INB1 24
#define INB2 28
#define INC1 30 //TB6612FNG驱动模块控制信号共6个
#define INC2 34
#define IND1 32
#define IND2 36
#define PWMA 8 //OC4C
#define PWMB 7 //OC4B
#define PWMC 6 //OC4C
#define PWMD 5 //OC4B
float Kp = 15, Ki = 0 , Kd = 0;
static float error = 0, P = 0, I = 0, D =0, PID_value = 0;
float previous_error = 0, previous_I = 0;
static int initial_motor_speed = 140; //期望速度
int left_motor_speed = 0;
int right_motor_speed = 0;
uint8_t irs = 0;
static int a = 9 , b = 6, c = 5, d;
static int key = 0;
static int MERIDIAN = 0;
static int WEFT = 0;
static int meridian = 0;
static int weft = 0;
static int flag1 = 0;
static int flag2 = 0;
unsigned int aa;
const int IR_PIN[] = {A0, A1, A2, A3, A4};//红外对管引脚定义
void read_ir_values(void);
void calculate_pid(void);
void motor_control(void);
void LCD_PRINT(void);
void ISRR1();
void ISRL1();
void STEP_FRONT(void);
void STEP_RIGHT(void);
void STEP_LEFT(void);
void STEP_TOUCH(void);
void Set_Pwm(int left_motor_speed, intright_motor_speed);
void setup()
{
lcd.begin(16, 2);
meridian = 0;
weft = 0;
key= 0;
pinMode(INA1, OUTPUT); //TB6612控制引脚，控制电机1的方向，01为正转，10为反转
pinMode(INA2, OUTPUT); //TB6612控制引脚，
pinMode(INB1, OUTPUT); //TB6612控制引脚，控制电机1的方向，01为正转，10为反转
pinMode(INB2, OUTPUT); //TB6612控制引脚，
pinMode(PWMA, OUTPUT); //TB6612控制引脚，电机PWM
pinMode(PWMB, OUTPUT); //TB6612控制引脚，电机PWM
pinMode(INC1, OUTPUT); //TB6612控制引脚，控制电机1的方向，01为正转，10为反转
pinMode(INC2, OUTPUT); //TB6612控制引脚，
pinMode(IND1, OUTPUT); //TB6612控制引脚，控制电机1的方向，01为正转，10为反转
pinMode(IND2, OUTPUT); //TB6612控制引脚，
pinMode(PWMC, OUTPUT); //TB6612控制引脚，电机PWM
pinMode(PWMD, OUTPUT); //TB6612控制引脚，电机PWM
pinMode(A0, 0); //TB6612控制引脚，电机PWM
pinMode(A1, 0);
pinMode(A5, 0); //TB6612控制引脚，电机PWM
pinMode(A6, 0);
pinMode(A8, 0);
digitalWrite(14, 0); //TB6612控制引脚拉低
digitalWrite(15, 0); //TB6612控制引脚拉低
analogWrite(PWMA, 0); //TB6612控制引脚拉低
analogWrite(PWMB, 0); //TB6612控制引脚拉低
analogWrite(PWMC, 0); //TB6612控制引脚拉低
analogWrite(PWMD, 0); //TB6612控制引脚拉低
for(int i = 0; i < 4; i++)
{
pinMode(IR_PIN[i], INPUT);
}
Serial2.begin(9600);
attachInterrupt(3, ISRL1, CHANGE); //开启外部中断2
attachInterrupt(2, ISRR1, CHANGE); //开启外部中断3
attachInterrupt(4, XIAN1, CHANGE); //开启外部中断2
//attachInterrupt(5, XIAN2, CHANGE); //开启外部中断3
Kp= 20 , Ki = 0, Kd = 0, a = 9 , c = 5, initial_motor_speed = 60; //期望速度;
}
void loop()
{
if((digitalRead(A0)==0)&&(digitalRead(A1)==0))//右
{
Set_Pwm(-100,-100);
delay(500);
}
else if( digitalRead(A0)==0)//右
{
Set_Pwm(-100, 100);
delay(500);
}
else if( digitalRead(A1)==0)//右
{
Set_Pwm(100, -100);
delay(500);
}
else
{
Set_Pwm(100, 100);
}
}
void Set_Pwm(int motoA, int motoB)
{
if(motoA > 0)
{
digitalWrite(INA1, HIGH), digitalWrite(INA2, LOW); //TB6612的电平控制
digitalWrite(INC1, HIGH), digitalWrite(INC2, LOW); //TB6612的电平控制
}
else
{
digitalWrite(INA1, LOW), digitalWrite(INA2, HIGH); //TB6612的电平控制
digitalWrite(INC1, LOW), digitalWrite(INC2, HIGH); //TB6612的电平控制
}
if(motoB > 0)
{
digitalWrite(INB1, HIGH), digitalWrite(INB2, LOW); //TB6612的电平控制
digitalWrite(IND1, HIGH), digitalWrite(IND2, LOW); //TB6612的电平控制
}
else
{
digitalWrite(INB1, LOW), digitalWrite(INB2, HIGH); //TB6612的电平控制
digitalWrite(IND1, LOW), digitalWrite(IND2, HIGH); //TB6612的电平控制
}
{
analogWrite(PWMA, abs(motoA)); //赋值给PWM寄存器
analogWrite(PWMB, abs(motoB)); //赋值给PWM寄存器
analogWrite(PWMC, abs(motoA)); //赋值给PWM寄存器
analogWrite(PWMD, abs(motoB)); //赋值给PWM寄存器
}
}
void ISRL1()//外部中断2
{
flag1 = 1;
}
void ISRR1()//外部中断3
{
flag1 = 1;
}
void XIAN1()//外部中断2
{
flag1 = 2;
}
void XIAN2()//外部中断3
{
flag1 = 2;
}

复制代码

帐号		自动登录	找回密码
密码			立即注册