arduino控制小车霍尔

2019-05-20 10:25:04 qq_16775293 阅读数 21897

Arduino智能小车——小车测速

  可以用于测速的模块很多,比如加速度计、激光、超声波、编码器等等,由于我们对小车速度的测量精度要求不高,因此我们可以借助小车套件里面的码盘配上测速模块对其速度进行测量。
##准备材料
测速模块
  网上的测速模块有很多种外观,但是其工作原理类似,下面列出来了几种常见的测速模块,这些测速模块接线类似。

  以上两种模块只能对一路电机的速度进行测量,下面这个可以同时测量两路电机,但是由于其间距固定,在安装时可能不兼容不同小车底盘,因此为了安装方便,在此篇教程中使用第一种模块。

固定铜柱

  建议铜柱长度30CM,大小为M3

  由于铜柱导电,有些电路板如果固定孔设计不当的话很容易导致电路板烧坏,尤其是在以后的项目中可能会用到更高的电压,因此在这里我建议大家可以准备一些尼龙柱来固定电路板。尼龙柱的尺寸跟铜柱尺寸相同M3,30CM。大家也可以顺便买一点尼龙螺丝。

  除此之外还需要准备杜邦线

测速模块的安装

  驱动模块的安装需要由尼龙柱支撑

  驱动模块安装时需要注意,不能影响轮子的正常工作,不能触碰到轮轴上的码盘。
  编码器上有三个引脚分别是“VCC”,“GND”,“OUT”。左右两边两个测速模块的“VCC”引脚接电源或开发板的“5V”或“3.3V”引脚,“GND”接电源或开发板的“GND”引脚,左边测速模块“OUT”接开发板的“3”引脚,右边测速模块“OUT”接开发板的“2”引脚。引脚接错的话可以再随后调试过程中换过来,也可以在代码里更改。
  最终接线图如下:

测速模块讲解

  测速模块的工作原理比较简单,如下图所示,在于电机同轴的码盘上有很多开孔(光栅),编码器相当于光敏元件。码盘随着小车轮子的运动转动时,码盘(光栅)会不断遮挡光敏元件发出的光波,这时候编码器就会根据光栅的遮挡不断的产生方波信号,方波信号会从“OUT”引脚输出,我们只需不断检测“OUT”引脚的输出,根据方波信号的周期简介计算出小车运行的速度。小车上使用的码盘(光栅)精度不高,在某些高精度的编码器上光栅会更加密集,测量效果会更好。

  由于要不断检测编码器输出端的输出,因此我们需要借助Arduino的外部中断来读取编码器的输出。Arduino开发板外部中断对应的引脚如下:
型号 int0 int1 int2 int3 int4 int5
UNO\Ethernet 2 3
Mega2560 2 3 21 20 19 18
Leonardo 3 2 0 1
Due All All All All All All

  由表中可以知道在此我们使用的Arduino UNO只有“2”,“3”引脚可以触发外部中断,因此在接线的时候我们便将左右两边的输出“OUT”引脚分别接在“2”“3”引脚上。
  在程序初始化阶段中调用函数attachInterrupt(interrupt, function, mode)可以对中断引脚初始化,其中
interrupt: 要初始化的外部中断编号,由上表可知我们Arduino UNO只能使用外部中断0和外部中断1;
function: 中断服务函数的名字,即当外部中断被触发时,将会自动调用这个函数;
mode: 中断触发的方式,可选方式如下

mode 含义
LOW 低电平触发
CHANGE 电平变化,高电平变低电平、低电平变高电平
RISING 上升沿触发
FALLING 下降沿触发
HIGH 高电平触发(该中断模式仅适用于Arduino due)

测试代码如下

int leftCounter=0,  rightCounter=0;
unsigned long time = 0, old_time = 0; // 时间标记
unsigned long time1 = 0; // 时间标记
float lv,rv;//左、右轮速度

#define STOP      0
#define FORWARD   1
#define BACKWARD  2
#define TURNLEFT  3
#define TURNRIGHT 4

int leftMotor1 = 4;
int leftMotor2 = 5;
int rightMotor1 = 6;
int rightMotor2 = 7;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600); 
  attachInterrupt(0,RightCount_CallBack, FALLING);
  attachInterrupt(1,LeftCount_CallBack, FALLING);

  pinMode(leftMotor1, OUTPUT);
  pinMode(leftMotor2, OUTPUT);
  pinMode(rightMotor1, OUTPUT);
  pinMode(rightMotor2, OUTPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
  SpeedDetection();
  if(Serial.available()>0)
  {
    char cmd = Serial.read();
  
    Serial.print(cmd);
    motorRun(cmd);
      
  }  
}
/*
 * *速度计算
 */
bool SpeedDetection()
{
  time = millis();//以毫秒为单位,计算当前时间 
  if(abs(time - old_time) >= 1000) // 如果计时时间已达1秒
  {  
    detachInterrupt(0); // 关闭外部中断0
    detachInterrupt(1); // 关闭外部中断1
    //把每一秒钟编码器码盘计得的脉冲数,换算为当前转速值
    //转速单位是每分钟多少转,即r/min。这个编码器码盘为20个空洞。
    lv =(float)leftCounter*60/20;//小车车轮电机转速
    rv =(float)rightCounter*60/20;//小车车轮电机转速
    Serial.print("left:");
    Serial.print(lv);//向上位计算机上传左车轮电机当前转速的高、低字节
    Serial.print("     right:");
    Serial.println(rv);//向上位计算机上传左车轮电机当前转速的高、低字节
    //恢复到编码器测速的初始状态
    leftCounter = 0;   //把脉冲计数值清零,以便计算下一秒的脉冲计数
    rightCounter = 0;
    old_time=  millis();     // 记录每秒测速时的时间节点   
    attachInterrupt(0, RightCount_CallBack,FALLING); // 重新开放外部中断0
    attachInterrupt(1, LeftCount_CallBack,FALLING); // 重新开放外部中断0
    return 1;
  }
  else
    return 0;
}
/*
 * *右轮编码器中断服务函数
 */
void RightCount_CallBack()
{
  rightCounter++;
}
/*
 * *左轮编码器中断服务函数
 */
void LeftCount_CallBack()
{
  leftCounter++;
}
/*
 * *小车运动控制函数
 */
void motorRun(int cmd)
{
  switch(cmd){
    case FORWARD:
      Serial.println("FORWARD"); //输出状态
      digitalWrite(leftMotor1, HIGH);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, HIGH);
      digitalWrite(rightMotor2, LOW);
      break;
     case BACKWARD:
      Serial.println("BACKWARD"); //输出状态
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, HIGH);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, HIGH);
      break;
     case TURNLEFT:
      Serial.println("TURN  LEFT"); //输出状态
      digitalWrite(leftMotor1, HIGH);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, HIGH);
      break;
     case TURNRIGHT:
      Serial.println("TURN  RIGHT"); //输出状态
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, HIGH);
      digitalWrite(rightMotor1, HIGH);
      digitalWrite(rightMotor2, LOW);
      break;
     default:
      Serial.println("STOP"); //输出状态
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, LOW);
  }
}

  看了这个代码大家应该就明白为什么之前我建议大家将每个功能都封装成函数了,封装成函数很利于程序的移植。

测速效果

  借助于蓝牙串口助手我们可以清楚的看到小车左右轮的转速如图,由于电机自身的误差和摩擦力的作用,因此两个轮子的转速不相等,但是差别不是很大。

欢迎各位有兴趣的朋友加入Q群1:789127261点评、交流

2020-03-11 17:14:58 qq_35898865 阅读数 508

MsTime2,霍尔编码器测速

100毫秒触发一次中断,打印出这100毫秒内,霍尔编码器测到的脉冲数。
代码如下:

#include <MsTimer2.h>

//TB6612引脚定义
const int right_R1=8;    
const int right_R2=12;
const int PWM_R=10;
const int left_L1=7;
const int left_L2=6;
const int PWM_L=9;

const int PinA_left = 5;        //定义检测左电机脉冲的引脚为D5
const int PinA_right = 4;       //定义检测右电机脉冲的引脚为D4

int times=0,newtime=0,d_time=100;   //时间,最新的时间,时间间隔
int valA=0,valB=0,flagA=0,flagB=0;   //变量valA和valB用于计算脉冲数

void setup()
{
  Serial.begin(9600);
  
  pinMode(right_R1,OUTPUT);     //TB6612的引脚都设置为输出
  pinMode(right_R2,OUTPUT);
  pinMode(PWM_R,OUTPUT);
  pinMode(left_L1,OUTPUT);
  pinMode(left_L2,OUTPUT);
  pinMode(PWM_L,OUTPUT);

  pinMode(PinA_left,INPUT);      //设置检测脉冲的引脚为输入状态
  pinMode(PinA_right,INPUT);
  
  MsTimer2::set(100, inter); // 100ms 触发一次中断,调用函数inter()
  MsTimer2::start();    //开启中断
}

void loop() 
{
  //两电机都正转
  digitalWrite(right_R1,HIGH);
  digitalWrite(right_R2,LOW);
  digitalWrite(left_L1,HIGH);
  digitalWrite(left_L2,LOW);
  analogWrite(PWM_R,100);    //写入PWM值0~255(速度)
  analogWrite(PWM_L,200);

  if(digitalRead(PinA_left)==HIGH&&flagA==0)     //计算脉冲值
    {
      valA++;
      flagA=1;
    }
    if(digitalRead(PinA_left)==LOW&&flagA==1)
    {
      valA++;
      flagA=0;
    }
    
    if(digitalRead(PinA_right)==HIGH&&flagB==0)
    {
      valB++;
      flagB=1;
    }
    if(digitalRead(PinA_right)==LOW&&flagB==1)
    {
      valB++;
      flagB=0;
    }
    
}

//中断函数
void inter()    
{
    sei();    //允许全局中断
    Serial.print("valA = ");     //在串口监视器上打印出脉冲值
    Serial.println(valA);
    Serial.print("valB = ");
    Serial.println(valB);
    valA = valB = 0;         //清0
}
2019-06-05 00:20:49 withings 阅读数 3350

依赖库下载:
Servo

依赖库的安装方法,如不清楚,可以参考官方文档《安装其他的Arduino库》

实物图:基于Arduino的超声波智能避障小车

程序如下:

#include <Servo.h> 
#include <stdlib.h>
#define speed1 5  //定义EA(PWM调速)接口
#define IN1 6  
#define IN2 7     
#define speed2 11  //定义EB(PWM调速)接口
#define IN3 12  
#define IN4 13    
#define sensor 4  //霍尔传感器
#define TrigPin 8  //超声波模块触发控制信号输入端口 
#define EchoPin 9  //超声波模块回响信号输入端口
Servo myservo;  //创建一个舵机控制对象  
float route = 0, distance, distance_right, distance_left; 
int Loop = 0, temploop = 0, rate = 0;
boolean sign = 0;
unsigned long seconds, temp_seconds = 1;
int Speed = 100;

void setup() {
  Serial.begin(9600);  //初始化串口通信及连接SR04的引脚
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(speed1, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(speed2, OUTPUT);
  pinMode(sensor, INPUT);
  pinMode(TrigPin, OUTPUT);  //输出超声波信号  
  pinMode(EchoPin, INPUT);  //要检测引脚上输入的脉冲宽度,需要先设置为输入状态
  myservo.attach(10);  //该舵机由arduino第10脚控制  
}

void Millis() {
  if (seconds > temp_seconds) {  //每过一秒      
    rate = 2 * temploop;
    route = 0.2 * Loop;

    temploop = 0;
    temp_seconds ++;

    Display();
  }
}

void count() {  //计算轮子转动的圈数
  if (digitalRead(sensor) == HIGH)
    sign = 1;
  if (digitalRead(sensor) == LOW && sign == 1) {
    Loop ++;
    temploop ++;
    sign = 0;
  }
}

void Display() {
  // Serial.print("Speed: ");
  // Serial.print(rate);
  // Serial.println(" dm/s");  

  // Serial.print("Loop: ");
  // Serial.println(Loop);

  // Serial.print("Distance: ");
  // Serial.print(route);
  // Serial.println(" m"); 

  // Serial.print("Distance_front: ");
  // Serial.print(distance);
  // Serial.println(" cm");

  // Serial.println();

  // Serial.print('"'); 
  // Serial.print("Speed");
  // Serial.print('"');
  // Serial.print(':');
  // Serial.print('"');
  // Serial.print(rate);
  // Serial.print('"');
  // Serial.print(",");  

  Serial.print(rate);
  Serial.print(" ");

  Serial.print(Loop);
  Serial.print(" ");

  Serial.print(route);
  Serial.print(" ");

  Serial.print(distance);
  Serial.print(" ");

  // Serial.println();
}

//参数pin是输入的高低电平的IO口,pwmpin表示输入的PWM波形的IO口,state指电机状态(正转或反转),val是调速值大小0-255
void motor_right(int state, int val) {
  if (state == 1) {
    analogWrite(speed1, val);  //前进
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  } else if (state == -1) {
    analogWrite(speed1, val);  //后退
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
  } else if (state == 0) {
    analogWrite(speed1, val);  //停止
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
  }
}

void motor_left(int state, int val) {
  if (state == 1) {
    analogWrite(speed2, val);  //前进
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
  } else if (state == -1) {
    analogWrite(speed2, val);  //后退
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
  } else if (state == 0) {
    analogWrite(speed2, val);  //停止
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
  }
}

void forward() {
  motor_right(1, Speed);  //前进 
  motor_left(1, Speed);
}

void back() {
  motor_right(-1, Speed);  //后退
  motor_left(-1, Speed);
}

void left() {
  motor_left(-1, Speed);  //左转
  motor_right(1, Speed);   
}

void turn_left() {
  motor_left(-1, 150);  //左转
  motor_right(1, 150);  
  delay(600);
  forward();
}

void right() {
  motor_right(-1, Speed);  //右转
  motor_left(1, Speed);
}

void turn_right() {
  motor_right(-1, 150);   //右转
  motor_left(1, 150);
  delay(600);
  forward();
}

void stop() {
  if (State() == 'f') {
    back();
    delay(100);
    motor_right(0, 0);   //停止
    motor_left(0, 0);
  } else if (State() == 'b') {
    forward();
    delay(100);
    motor_right(0, 0);   //停止
    motor_left(0, 0);
  } else if(State() == 'l') {
    right();
    delay(100);
    motor_right(0, 0);   //停止
    motor_left(0, 0);
  } else if(State() == 'r') {
    left();
    delay(100);
    motor_right(0, 0);   //停止
    motor_left(0, 0);
  } else {
    motor_right(0, 0);   //停止
    motor_left(0, 0);
  }
}

String comdata = "";
boolean avoid = true;
void Bluetooth() {
  while (Serial.available() > 0) {
    comdata += char(Serial.read());
    delay(2);
  }

  if (comdata.length() > 0) {
    char ch = comdata[0];
    const char* c_s = comdata.c_str();
    int num = atoi(c_s);
    comdata = "";  
    
    if (num > 0 && num <= 255) {  //调速        
      Speed = num ;
      changeSpeed();
    } else if( ch == 'f' )  //前进   
      forward();    
    else if( ch == 'b' )  //后退
      back();
    else if( ch == 'l' )  //左转
      left();
    else if( ch == 'r' )  //右转
      right();
    else if( ch == 's' )  //停止
      stop();
    else if( ch == '+') {  //调速      
      Speed += 25;
      if(Speed == 225)
        Speed = 200;
      changeSpeed(); 
    } else if( ch == '-') {
      Speed -= 25;
      if(Speed == 25)
        Speed = 50;
      changeSpeed();
    } else if( ch == '0') {
      avoid = !avoid;
    }
  }      
}

void changeSpeed() {
  if (State() == 'f')
    forward();
  else if (State() == 'b')
    back();
  else if (State() == 'l')
    left();
  else if (State() == 'r')
    right();
}

void ultrasound() {
  digitalWrite(TrigPin, LOW);  // 产生一个10us的高脉冲去触发TrigPin
  delayMicroseconds(2); 
  digitalWrite(TrigPin, HIGH); 
  delayMicroseconds(10);
  digitalWrite(TrigPin, LOW);     
  distance = pulseIn(EchoPin, HIGH) / 58.00;  // 检测脉冲宽度,并计算出距离
  //Serial.print("distance: ");
  //Serial.print(distance); 
  //Serial.println(" cm"); 
  delay(10);
}

char State() {
  if (digitalRead(IN1) == HIGH &&  digitalRead(IN2) == LOW &&digitalRead(IN3) == HIGH &&  digitalRead(IN4) == LOW)
    return 'f';
  if (digitalRead(IN1) == LOW &&  digitalRead(IN2) == HIGH &&digitalRead(IN3) == LOW &&  digitalRead(IN4) == HIGH)
    return 'b';
  if (digitalRead(IN1) == HIGH &&  digitalRead(IN2) == LOW &&digitalRead(IN3) == LOW &&  digitalRead(IN4) == HIGH)
    return 'l';
  if (digitalRead(IN1) == LOW &&  digitalRead(IN2) == HIGH &&digitalRead(IN3) == HIGH &&  digitalRead(IN4) == LOW)
    return 'r';
}

void servo() {
  if (distance < 20 && State() == 'f') {
  	stop();  //小于20cm时先停止

    // Serial.print("front: ");  //前方距离
    // Serial.print(distance); 
    // Serial.println(" cm");     
     
    myservo.write(0);  //右边距离
    delay(400);
    ultrasound();
    distance_right = distance;
    // Serial.print("right: ");
    // Serial.print(distance_right); 
    // Serial.println(" cm"); 

    myservo.write(180);  //左边距离
    delay(600);
    ultrasound();
    distance_left = distance;
    // Serial.print("left: ");
    // Serial.print(distance_left); 
    // Serial.println(" cm");  
    // Serial.println();

    myservo.write(90);  //回到中间
    delay(400);  

    if (distance_left > distance_right)
    	turn_left();

    if (distance_left < distance_right)
    	turn_right(); 
  }
}

void loop() {
  seconds = millis() / 1000;
  ultrasound();
  if(avoid) 
    servo();
  count();
  Bluetooth();   
  Millis();
}
2016-12-21 12:14:44 ling3ye 阅读数 14918

 

这次介绍一下霍尔传感器,

可用于开关接触,例如IPAD的保护套,盖上,IPAD就自动关闭屏幕

可用于磁悬浮,例如之前很火的悬浮盆栽就需要用到线性霍尔传感器

还有...

 

先看看长怎么样先吧

 

实验效果

 

BOM表

Arduino Uno * 1

线性霍尔传感器(推荐使用KY-024 ,但也可以使用S49E)  *1

跳线若干

 

接线方式

Arduino Uno                KY-024                      如果用的是S49E

5V                    <--->         +              <--->                +

GND                <--->        G              <--->                 -

A0                    <--->        A0            <--->                out 

 

开源代码

例程中,使用中间值是500,但可能每个线性霍尔传感器会有所不同,请自行测试

磁铁靠近时,请观察数值变化,有分南北极的,方向不对的话,越靠近,数值越大

程序打包下载:https://u16460183.ctfile.com/fs/16460183-296053650

 

//Code Write by Moz for YouTube changel LogMaker360, 20-10-2015
//Code belongs to this video: https://www.youtube.com/watch?v=xMTKs240lBU

int readpin = A0;  
int incoming = 0;
void setup ()
{

pinMode (readpin, INPUT) ;
//pinMode(DO, INPUT);
//pinMode (LED, OUTPUT);

Serial.begin(9600);
}
void loop ()
{
incoming = analogRead(readpin) ;
if(incoming > 500){
  Serial.print("nothing detected ");
  }
 if(incoming < 500){
  Serial.print("detecting something.");
  }

Serial.println(incoming);
delay(400);
}

 

 

 

 

 

 

2018-06-01 09:23:58 qq_41931519 阅读数 2804

Arduino智能小车——小车测速

准备材料

1.     测速模块

2.     

3.     固定铜柱

a)      建议铜柱长度30CM,大小为M3

b)      由于铜柱导电,有些电路板如果固定孔设计不当的话很容易导致电路板烧坏,尤其是在以后的项目中可能会用到更高的电压,因此在这里我建议大家可以准备一些尼龙柱来固定电路板。尼龙柱的尺寸跟铜柱尺寸相同M3,30CM。大家也可以顺便买一点尼龙螺丝。

c)       

4.     准备杜邦线

5.     驱动模块的安装需要由尼龙柱支撑

6.     

7.     驱动模块安装时需要注意,不能影响轮子的正常工作,不能触碰到轮轴上的码盘。

  编码器上有三个引脚分别是“VCC”,“GND”,“OUT”。左右两边两个测速模块的“VCC”引脚接电源或开发板的“5V”或“3.3V”引脚,“GND”接电源或开发板的“GND”引脚,左边测速模块“OUT”接开发板的“3”引脚,右边测速模块“OUT”接开发板的“2”引脚。引脚接错的话可以再随后调试过程中换过来,也可以在代码里更改。

8.     

9.     测速模块的工作原理比较简单,如下图所示,在于电机同轴的码盘上有很多开孔(光栅),编码器相当于光敏元件。码盘随着小车轮子的运动转动时,码盘(光栅)会不断遮挡光敏元件发出的光波,这时候编码器就会根据光栅的遮挡不断的产生方波信号,方波信号会从“OUT”引脚输出,我们只需不断检测“OUT”引脚的输出,根据方波信号的周期简介计算出小车运行的速度。小车上使用的码盘(光栅)精度不高,在某些高精度的编码器上光栅会更加密集,测量效果会更好。

10.  

11.  我们使用的Arduino UNO只有“2”,“3”引脚可以触发外部中断,因此在接线的时候我们便将左右两边的输出“OUT”引脚分别接在“2”“3”引脚上。

12.  在程序初始化阶段中调用函数attachInterrupt(interrupt,function, mode)可以对中断引脚初始化,其中

13.  interrupt:要初始化的外部中断编号,由上表可知我们Arduino UNO只能使用外部中断0和外部中断1;

14.  function:中断服务函数的名字,即当外部中断被触发时,将会自动调用这个函数;

15.  mode:中断触发的方式,可选方式如下


16.  脉宽调制(PWM)基本原理:控制方式就是对逆变电路开关器件的通断进行控制,使输出端得到一系列幅值相等的脉冲,用这些脉冲来代替正弦波或所需要的波形。也就是在输出波形的半个周期中产生多个脉冲,使各脉冲的等值电压为正弦波形,所获得的输出平滑且低次谐波少。按一定的规则对各脉冲的宽度进行调制,即可改变逆变电路输出电压的大小,也可改变输出频率。

17.  

18.   通俗一点讲那,就是当如果我们想输出5V电压时,只需一直输出高电平即可;当我们想输出3.75V电压时,那我们就需要在一个周期内(一个高电平和一个低电平为一个周期)3.75÷5=75%时间输出高电平,25%时间输出低电平;同理,如果想输出2.5V电压时,我们需要在一个周期内50%时间输出高电平,50%时间输出低电平。

19.  

20.  Arduino UNO开发板上只有带有“~”表示的引脚才具有PWM功能,因此我们在控制驱动时可以使用这几个引脚。

21.  在前面的教程中已经讲过如果想控制驱动的输出时,需要对驱动的“ENA”“ENB”进行控制,因此我们需要将图中被选中部分的两个跳线帽拔掉。并将“ENA”连接Arduino UNO开发板的“5”引脚,“ENB”连接“6”引脚。

22.  由于之前设计不太合理,占用了太多的PWM引脚,因此在代码里对控制小车电机的引脚做了点小改动

23.  16到19号引脚

24.  

25.  大家记得要把对应的接线也改过来

a)      int leftMotor1 = 5;

b)      int leftMotor2 = 6;

c)       int rightMotor1 = 7;

d)      int rightMotor2 = 8;

e)      现在改为

f)        int leftMotor1 = 16;   

g)      int leftMotor2 = 17;

h)      int rightMotor1 = 18;

i)        int rightMotor2 = 19;

26.  代码如下

a)      #include <ros.h>

b)      #include <Servo.h>

c)       #include<geometry_msgs/Twist.h>

d)       

e)      int leftCounter = 0,rightCounter = 0;

f)        unsigned long time = 0,old_time = 0; //时间标记

g)      unsigned long time1 = 0;

h)      float lv, rv;  //左、右轮速度

i)         

j)        //定义五中运动状态

k)       #define STOP      0

l)        #define FORWARD   1

m)    #define BACKWARD  2

n)      #define TURNLEFT  3

o)      #define TURNRIGHT 4

p)      #define CHANGESPEED 5 //换挡的

q)      //定义需要用到的引脚

r)       int leftMotor1 = 16;

s)       int leftMotor2 = 17;

t)       int rightMotor1 = 18;

u)      int rightMotor2 = 19;

v)       bool speedLevel = 1;

w)      

x)       //调速

y)       int leftPWM =5;

z)       int rightPWM = 6;

aa)    

bb)   //x轴方向的速度

cc)    double lin_vel = 0.0;

dd)   //y轴方向的速度

ee)   double ang_vel = 0.0;

ff)      //定义接受的键

gg)   int cmd_ctrl = 0;

hh)    

ii)       //注册ROS节点

jj)       ros::NodeHandle nh;

kk)     

ll)       //回调函数

mm)voidmessageCb(const geometry_msgs::Twist& vel)

nn)   {

oo)         lin_vel = vel.linear.x;

pp)         ang_vel = vel.angular.z;

qq)         cmd_ctrl = 1 * lin_vel + 3 * ang_vel;

rr)      }

ss)     //设置订阅的消息类型和发布的主题

tt)      ros::Subscriber<geometry_msgs::Twist>sub("/turtle1/cmd_vel", messageCb);

uu)    

vv)    void setup() {

ww)    // put your setup code here,to run once:

xx)          nh.initNode();

yy)          nh.subscribe(sub);

zz)            

aaa)        attachInterrupt(0,RightCount_CallBack, FALLING);

bbb)           attachInterrupt(1, LeftCount_CallBack,FALLING);

ccc)        

ddd)           pinMode(leftMotor1, OUTPUT);

eee)       pinMode(leftMotor2,OUTPUT);

fff)          pinMode(rightMotor1, OUTPUT);

ggg)           pinMode(rightMotor2, OUTPUT);

hhh)       pinMode(leftPWM,OUTPUT);

iii)           pinMode(rightPWM, OUTPUT);

jjj)     }

kkk)   

lll)     void loop() {

mmm)    // put your main code here,to run repeatedly:

nnn)     SpeedDetection();

ooo)         switch(cmd_ctrl)

ppp)       {

qqq)           case 2:

rrr)           motorRun(FORWARD); 

sss)          delay(1000);

ttt)             motorRun(STOP); 

uuu)         break;

vvv)         case -2:

www)            motorRun(BACKWARD);   

xxx)           delay(1000);

yyy)           motorRun(STOP); 

zzz)           break;

aaaa)           case 6:

bbbb)           motorRun(TURNLEFT);    

cccc)              delay(1000);

dddd)           break;

eeee)          case -6:

ffff)           motorRun(TURNRIGHT);   

gggg)           delay(1000);

hhhh)           motorRun(STOP); 

iiii)             break;

jjjj)          default:

kkkk)            motorRun(STOP);      

llll)            break;

mmmm)           }

nnnn)      if (speedLevel) //根据不通的档位输出不同速度

oooo)      {

pppp)          analogWrite(leftPWM, 120);

qqqq)          analogWrite(rightPWM, 120);

rrrr)      }

ssss)     else

tttt)      {

uuuu)          analogWrite(leftPWM, 250);

vvvv)            analogWrite(rightPWM, 250); 

wwww)               }

xxxx)         motorRun(STOP); 

yyyy)        nh.spinOnce();

zzzz)     }

aaaaa) /*

bbbbb)           * *速度计算

ccccc)   */

ddddd)          bool SpeedDetection()

eeeee) {

fffff)       time = millis(); //以毫秒为单位,计算当前时间

ggggg)                if (abs(time - old_time) >= 1000) // 如果计时时间已达1秒

hhhhh)                 {

iiiii)               detachInterrupt(0);// 关闭外部中断0

jjjjj)               detachInterrupt(1);// 关闭外部中断1

kkkkk)              //把每一秒钟编码器码盘计得的脉冲数,换算为当前转速值

lllll)               //转速单位是每分钟多少转,即r/min。这个编码器码盘为20个空洞。

mmmmm)              lv = (float)leftCounter * 60 / 20;//小车车轮电机转速

nnnnn)                       rv = (float)rightCounter * 60 / 20;

ooooo)                      //恢复到编码器测速的初始状态

ppppp)                      leftCounter = 0;  //把脉冲计数值清零,以便计算下一秒的脉冲计数

qqqqq)                      rightCounter = 0;

rrrrr)            old_time = millis();    // 记录每秒测速时的时间节点  

sssss)               attachInterrupt(0,RightCount_CallBack, FALLING); // 重新开放外部中断0

ttttt)             attachInterrupt(1,LeftCount_CallBack, FALLING);

uuuuu)                       return 1;       

vvvvv)        }

wwwww)             else

xxxxx)                return 0;

yyyyy)  }

zzzzz)   

aaaaaa)          

bbbbbb)       /*

cccccc) * *右轮编码器中断服务函数

dddddd)        */

eeeeee)         void RightCount_CallBack()

ffffff)     {

gggggg)             rightCounter++; 

hhhhhh)        }

iiiiii) /*

jjjjjj)  * *左轮编码器中断服务函数

kkkkkk)           */

llllll) void LeftCount_CallBack()

mmmmmm)         {

nnnnnn)              leftCounter++; 

oooooo)       }

pppppp)        

qqqqqq)        

rrrrrr)   //运动控制函数

ssssss) void motorRun(int cmd)

tttttt)    {

uuuuuu)              switch(cmd) {

vvvvvv)                 case FORWARD:

wwwwww)        digitalWrite(leftMotor1, LOW);

xxxxxx)       digitalWrite(leftMotor2,HIGH);

yyyyyy)                 digitalWrite(rightMotor1, HIGH);

zzzzzz)                 digitalWrite(rightMotor2, LOW);

aaaaaaa)               break;

bbbbbbb)         case BACKWARD:

ccccccc)              digitalWrite(leftMotor1, HIGH);

ddddddd)         digitalWrite(leftMotor2, LOW);

eeeeeee)            digitalWrite(rightMotor1, LOW);

fffffff)         digitalWrite(rightMotor2, HIGH);

ggggggg)         break;

hhhhhhh)         case TURNLEFT:

iiiiiii)       digitalWrite(leftMotor1,LOW);

jjjjjjj)       digitalWrite(leftMotor2,  HIGH);

kkkkkkk)              digitalWrite(rightMotor1, LOW);

lllllll)       digitalWrite(rightMotor2,HIGH);

mmmmmmm)          break;

nnnnnnn)         case TURNRIGHT:

ooooooo)          digitalWrite(leftMotor1, HIGH);

ppppppp)         digitalWrite(leftMotor2, LOW);

qqqqqqq)         digitalWrite(rightMotor1, HIGH);

rrrrrrr)       digitalWrite(rightMotor2, LOW);

sssssss)                break;

ttttttt)      case CHANGESPEED:

uuuuuuu)           if (speedLevel)   //接收到换挡命令的时候切换档位

vvvvvvv)                      speedLevel = 0;

wwwwwww)                 else

xxxxxxx)                        speedLevel = 1;

yyyyyyy)                break;

zzzzzzz)             default:

aaaaaaaa)          digitalWrite(leftMotor1, LOW);

bbbbbbbb)       digitalWrite(leftMotor2,LOW);

cccccccc)            digitalWrite(rightMotor1, LOW);

dddddddd)       digitalWrite(rightMotor2, LOW);

eeeeeeee)     }

ffffffff)  }

27.  Arduion的PWM引脚需要和正常引脚一样,在voidsetup()函数中初始化为输出模式

28.   

a)      pinMode(leftPWM, OUTPUT);

b)      pinMode(rightPWM, OUTPUT);

29.  在小车的控制状态函数void motorRun(intcmd)中添加多一个选择项,用来切换速度。

30.   

a)      case CHANGESPEED:

b)            Serial.println("CHANGE SPEED");//输出状态

c)             if(speedLevel)  //接收到换挡命令的时候切换档位

d)              speedLevel=0;

e)            else

f)                speedLevel=1;

g)            break;

31.  在主函数void loop()中添加PWM输出的函数,analogWrite(pin, value)函数中“pin”代表使用的引脚,“value”代表输出PWM值的大小,范围是0~255。

32.   

a)      if(speedLevel)  //根据不通的档位输出不同速度

b)          {

c)             analogWrite(leftPWM, 120);

d)            analogWrite(rightPWM, 120);

e)          }

f)            else

g)          {

h)            analogWrite(leftPWM, 250);

i)              analogWrite(rightPWM, 250);

j)            }