ardunio uno智能小车

最新推荐文章于 2024-02-10 21:14:36 发布

没有的念头

最新推荐文章于 2024-02-10 21:14:36 发布

阅读量492

点赞数 5

文章标签： c++ 青少年编程

本文链接：https://blog.csdn.net/qingt0/article/details/135914922

版权

#include <Arduino.h>

float geta00,geta01;
float geta0,geta1;
float geta02,geta12;
int geti=0;
int chei,ultrasonic_x_i;
float ultrasonic_x_old;
int pinultrasonic=12;

// 定义霍尔传感器输入引脚
int Pina0 = A0;
int Pina1 = A1;
int Pina2 = A4;
int Pina2i = 0;
int geta2=700;

int encoderPin1A = 2;
int encoderPin1B = 8;
int encoderPin2A = 3;
int encoderPin2B = 11;
long encoderCount1 = 0;
long encoderCount2 = 0;
float encoderspeed1 = 0;
float encoderspeed2 = 0;
float millisold=0;
// 定义pwm电磁铁输出引脚
int pwm0 = 9;
int pwm1 = 10;

struct _pid{
float SetSpeed; //定义设定期望值,目标值
float ActualSpeed; //定义实际值
float err; //定义偏差值
float err_last; //定义上一个偏差值
float Kp,Ki,Kd; //定义比例、积分、微分系数
float voltage; //定义电压值（控制执行器的变量）
float integral; //定义积分值
float derivative; //计算微分项
float umax;
float umin;

}pid0,pid1;
//电机298N
void motorModule(int pinDir1,int pinDir2, int pinPwm, int gspeed)
{
pinMode(pinDir1, OUTPUT);
pinMode(pinDir2, OUTPUT);
if(gspeed==0){
digitalWrite(pinDir1, 0);
digitalWrite(pinDir2, 0);
}else
if(gspeed>0){
digitalWrite(pinDir1, 1);
digitalWrite(pinDir2, 0);
}else
if(gspeed<0){
digitalWrite(pinDir1, 0);
digitalWrite(pinDir2, 1);
}
analogWrite(pinPwm, abs(gspeed));//analogWrite(3, 200); pwm引脚3设置为200,
//占空比为abs(gspeed)/255=200/255
}
void motorModule2(int pinDir1,int pinDir2, int gspeed)
{
pinMode(pinDir1, OUTPUT);
pinMode(pinDir2, OUTPUT);
if(gspeed==0){
digitalWrite(pinDir1, 0);
digitalWrite(pinDir2, 0);
}else
if(gspeed>0){
digitalWrite(pinDir1, 1);
digitalWrite(pinDir2, 0);
}else
if(gspeed<0){
digitalWrite(pinDir1, 0);
digitalWrite(pinDir2, 1);
}
}

void settimer20(int pinDir1,int pinDir2,float ocr2a,float ocr2b,int mode,int timer)
{
pinMode(pinDir1, OUTPUT);
pinMode(pinDir2, OUTPUT);

if(timer==1)//timer1 ,10,9
{
   //WGM12=1是相位修正，WGM10=1是8位
   if(mode==0)//快速8位
   {
TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);
TCCR1B = _BV(WGM12) | _BV(CS11) | _BV(CS10);
   }else
if(mode==1)//相位修正8位
{
TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);
TCCR1B = _BV(CS11) | _BV(CS10);
}
   //ICR1=0x00ff; //最大值

OCR1AH =0x00;
OCR1AL =abs((int)ocr2a);
OCR1BH =0x00;
OCR1BL =abs((int)ocr2b);
//占空比 = (OCR1A / ICR1) * 100%

}
else
if(timer==2)//timer2 ,11,3
{
if(mode==0)
{ //快速模式WGM设置为011 _BV(WGM21)
TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM21) | _BV(WGM20);
   //波形产生模式。COM2A1，表示COM2A的第1位（从0开始）。
   //所以：_BV(COM2A1)表示COM2A = 10；_BV(WGM21) | _BV(WGM20) 表示 WGM2 = 011。
TCCR2B = _BV(CS22);
   //定时器的预除数，表示100,意为：CS22=1,CS21=0,CS20=0 分频为64
}else
if(mode==1)
{//相位修正模式WGM设置为001,WGM20=1,这种模式频率接近降低了一倍,计算频率的时候要除以2 =/2
TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM20);
TCCR2B = _BV(CS22);

}else
if(mode==2)
{ // _BV(CS22)修改频率模式,OCRA用来设置总数，OCRB用来设置比较器,每次计数器数到头的时候，输出A做一次反相，这样能凑合输出一个占空比为50%的方波。因为A做了一次反向，最后计算要多除以2 =/2
   //在修改频率的模式下，时钟从0开始计数到OCRA而不是255，注意这个OCRA我们之前是用来做比较用的。这样一来，频率的设置就非常灵活了。对Timer1来说，OCRA可以设置到16位（应该是0~65535),计算的时候是除以修改值 /(180+1)
   //快速PWM下，修改时钟的计数上限 WGM设置为111
TCCR2A = _BV(COM2A0) | _BV(COM2B1) | _BV(WGM21) | _BV(WGM20);
TCCR2B = _BV(WGM22) | _BV(CS22);

}else
if(mode==3)
{ //相位修正PWM下，修改时钟的计数上限 WGM设置为101
TCCR2A = _BV(COM2A0) | _BV(COM2B1) | _BV(WGM20);
TCCR2B = _BV(WGM22) | _BV(CS22);

}

//Timer2所控制的管脚是pin11和pin3
//Timer0控制5，6；
//Timer1控制9，10
//定时器速度(HZ) = Arduino UNO时钟速度(16MHz) / 预分频器系数
//中断频率（Hz）=（Arduino时钟速度16MHz）/（预分频器*（比较匹配寄存器+ 1）)
//比较匹配寄存器= [16,000,000Hz /(预分频器*所需的中断频率)] - 1

//主板频率是16 MHz 时钟都是从0计数到255
//TCCR2B = _BV(CS22); 表示100,意为：CS22=1,CS21=0,CS20=0 分频为64
//OCR2A = 180;
//输出 A 频率: 16 MHz / 64 / 256 = 976.5625Hz 16*1024*1024Hz/256 20kHz就听不到噪音
//输出 A 占空比: (180+1) / 256 = 70.7%
//OCR2B = 50;
//输出 B 频率: 16 MHz / 64 / 256 = 976.5625Hz
//输出 B 占空比: (50+1) / 256 = 19.9%
//表示两路输出的占空比=((int)ocr2a+1)/256

OCR2A =abs((int)ocr2a); //决定连接到该引脚的pin11 PWM信号的占空比
OCR2B =abs((int)ocr2b); //决定连接到该引脚的pin3 PWM信号的占空比
Serial.print("OCR2A=");
Serial.println(OCR2A);
Serial.print("OCR2B=");
Serial.println(OCR2B);
}

}

//超声波测距
float ultrasonicSensor(int trigPin, int echoPin)
{
float distance;
unsigned int temp;
pinMode(trigPin, OUTPUT);
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
pinMode(echoPin, INPUT);
temp = pulseIn(echoPin, HIGH);
distance = (float)temp / 58.2;
// un-comm this for nekomimi ultrasonic
/*
if(distance > 6){
distance *= 1.28;
}
*/
if(distance == 0){ distance = 999; }
return distance;
}

//履带车---------------------------------------------------------------
void smartcar_run(int speed1,int speed2,int delay0)//强度1，强度2，延时0
{

settimer20(pwm0,pwm1,speed1,speed2,0,1);//设置力度
motorModule2(4,5,speed1);
motorModule2(6,7,speed2);
delay(delay0);
}
void smartcar_forward(int speed,int delay0)
{

smartcar_run(speed,speed,delay0);

}
void smartcar_backward(int speed,int delay0)
{
smartcar_run(-speed,-speed,delay0);
}
void smartcar_right(int speed,int delay0)
{
smartcar_run(speed,-speed,delay0);
}
void smartcar_left(int speed,int delay0)
{
smartcar_run(-speed,speed,delay0);
}
void smartcar_stop()
{
smartcar_run(0,0,0);
}

void smartcar_forward50(int delay0){ smartcar_run(50,50,delay0);}
void smartcar_forwardright10050(int delay0){ smartcar_run(100,50,delay0);}
void smartcar_forwardleft10050(int delay0){ smartcar_run(50,100,delay0);}
void smartcar_backward50(int delay0){ smartcar_run(-50,-50,delay0);}
void smartcar_right50(int delay0){ smartcar_run(50,-50,delay0);}
void smartcar_left50(int delay0){ smartcar_run(-50,50,delay0);}

//超声波取平均值
float smartcar_csb(int pin)
{
int i=0;
int ultrasonic_x=0;

while (i<10)
   {
ultrasonic_x=ultrasonic_x+ultrasonicSensor(pin,pin);//获取超声波测距,这个设置不能和其它引脚冲突，否则会发生问题
   if(ultrasonic_x>0) i++;
   }
ultrasonic_x=ultrasonic_x/10;
Serial.print("超声波=");
Serial.println(ultrasonic_x);
return ultrasonic_x;
}

//选择转向
void smartcar_select_leftright(int delay0)
{
float csb1,csb2;
smartcar_right50(delay0*2);
csb1=smartcar_csb(pinultrasonic);
smartcar_left50(delay0*4);
csb2=smartcar_csb(pinultrasonic);
if(csb1<csb2) {}
else
{
smartcar_right50(delay0*4);
}

}
//读取轮子
void smartcar_reads(float ultrasonic_x_c)
{
noInterrupts(); // 禁止中断
long gencoderCount1 = encoderCount1;
long gencoderCount2 = encoderCount2;

   float gmillis=(float)millis();
   float smillis=gmillis-millisold;
   millisold=gmillis;
   encoderspeed1=(float)gencoderCount1*ultrasonic_x_c/(float)smillis;
   encoderspeed2=(float)gencoderCount2*ultrasonic_x_c/(float)smillis;
   encoderCount1=0;
   encoderCount2=0;
Serial.print("Motor millis= ");
Serial.println(smillis);
Serial.print("Motor count1= ");
Serial.print(gencoderCount1);
Serial.print(" speed1= ");
Serial.println(encoderspeed1);
Serial.print("Motor count2= ");
Serial.print(gencoderCount2);
Serial.print(" speed2= ");
Serial.println(encoderspeed2);
interrupts(); // 允许中断
}

void updateEncoder1()
{

if (digitalRead(encoderPin1A) == digitalRead(encoderPin1B))
{
encoderCount1++;
}
else
{
encoderCount1--;
}
}
void updateEncoder2()
{

if (digitalRead(encoderPin2A) == digitalRead(encoderPin2B))
{
encoderCount2--;
}
else
{
encoderCount2++;
}
}

void geta_init_smartcar()
{
ultrasonic_x_old=0;
ultrasonic_x_i=0;

pinMode(encoderPin1A, INPUT);
pinMode(encoderPin1B, INPUT);
pinMode(encoderPin2A, INPUT);
pinMode(encoderPin2B, INPUT);
attachInterrupt(digitalPinToInterrupt(encoderPin1A), updateEncoder1, CHANGE);
attachInterrupt(digitalPinToInterrupt(encoderPin2A), updateEncoder2, CHANGE);
interrupts(); // 允许中断
// attachInterrupt(中断引脚,中断函数,CHANGE)
//中断引脚只能用2引脚、3引脚，Arduino Uno具有两个外部中断引脚，分别是2号引脚（INT0）和3号引脚（INT1）
//LOW：当引脚为低电平时触发中断服务程序
//CHANGE：当引脚电平发生变化时触发中断服务程序
//RISING：当引脚电平由低电平变为高电平时触发中断服务程序
//FALLING：当引脚电平由高电平变为低电平时触发中断服务程序

}
void smartcar()//履带车
{
int speed1,speed2,delay0;
float ultrasonic_x_c;
float speedrange=0.1*5;
speed1=50;
speed2=100;
delay0=500;

float ultrasonic_x=smartcar_csb(pinultrasonic);//超声波获取3个的平均值

ultrasonic_x_c=ultrasonic_x_old-ultrasonic_x;
ultrasonic_x_old=ultrasonic_x;

smartcar_reads(abs(ultrasonic_x_c));//读取智能小车的轮子

//开发板外接电源必须9v，作用板子上的电压才为5v
//如果>30 就前进，否则就停止
//如果被卡住，就后退，右转。
//如果<=30 可以右转，也可以左转。
//可以让其先右转，看看距离，再左转，看看距离，然后判断方向，

if(ultrasonic_x>30)
{
   if((encoderspeed1>-speedrange && encoderspeed1<speedrange) || (encoderspeed2>-speedrange && encoderspeed2<speedrange))
       //正常范围在0.2左右，<0.1轮子转速慢，被阻挡，再*超声波测距，大约0.1*5
   {

           smartcar_stop();
       delay(100);
           smartcar_backward50(delay0*6);
           smartcar_select_leftright(delay0);
}
   else
   {
   smartcar_forward50(delay0);
   }

}
else
{
smartcar_select_leftright(delay0);
}
smartcar_stop();
delay(10);