ROS小车底盘3-电机控制

编码器：

我们把JGA25-371中编码器1的HoutA(黄线)和HoutB(白线) 接 Arduino mega2560的pin2和pin3。

当轮子转动，pin2和pin3引脚就会产生0号和1号中断，每次产生中断，根据轮子转动的方向计数器+1或-1。

我们把JGA25-371中编码器2的HoutA(黄线)和HoutB(白线) 接 Arduino mega2560的pin19和pin18。

对应的中断号是4和5（当然也可节pin21和pin20, 对应的中断2和3）。

代码如下：

encoder_driver.h

long readEncoder(int i);//编码器数值反馈
void resetEncoder(int i);//复位编码器，编码器数据清零
void resetEncoders();//复位编码器，编码器数据清零

void initEncoders();//初始化编码器
void encoderRightISR();//编码器中断服务
void encoderLeftISR();//编码器中断服务

encoder_driver.ino

#include "motor_driver.h"//包含上面的头文件
#include "commands.h";//这个头文件目前不清楚
/* 编码*/
volatile long left_enc_pos = 0L;
volatile long right_enc_pos = 0L;
int left_rotate = 0;
int right_rotate = 0;

//初始化编码器
void initEncoders(){
  pinMode(2, INPUT);
  pinMode(3, INPUT);
  pinMode(19, INPUT);
  pinMode(18, INPUT);
  attachInterrupt(0, encoderLeftISR, CHANGE);  
  attachInterrupt(1, encoderLeftISR,  CHANGE);  
  attachInterrupt(4, encoderRightISR, CHANGE);
  attachInterrupt(5, encoderRightISR, CHANGE);
}

void encoderLeftISR(){
    if(direction(LEFT) == BACKWARDS){

        left_enc_pos--;
    }

    else{
        left_enc_pos++;
     }
}

void encoderRightISR(){
    if(direction(RIGHT) == BACKWARDS){
      right_enc_pos--;
    }else{
      right_enc_pos++;
    }
}

  long readEncoder(int i) {
      long encVal = 0L;
  if (i == LEFT)  {
    noInterrupts();
    encVal = left_enc_pos;
    interrupts();
  }
  else {
    noInterrupts();
    encVal = right_enc_pos;
    interrupts();
  }
  return encVal;
  }

电机：

电机通过L298N控制具体接线方式如下： 

L298N的ENA接Arduino mega 2560的pin5 

L298N的ENB接Arduino mega 2560的pin6 

IN1(或A1)和IN2(或A2) 接Arduino mega 2560的pin7和pin8 

IN3(或B1)和IN4(或B2) 接Arduino mega 2560的pin9和pin10

通过IN1和IN2控制电机1转动方向(正转或反转), 通过ENA控制其的转速; 

通过IN3和IN4控制电机2转动方向(正转或反转), 通过ENB控制其的转速。

代码如下：

motor_driver.c

void initMotorController();//电机控制函数
void setMotorSpeed(int i, int spd);//电机速度设置函数
void setMotorSpeeds(int leftSpeed, int rightSpeed);//暂定

motor_driver.ino

#include "commands.h";
// motor one
int enA = 5;
int in1 = 7;
int in2 = 8;
// motor two
int enB = 6;
int in3 = 9;
int in4 = 10;
boolean directionLeft = false;
boolean directionRight = false;

boolean direction(int i){
   if(i == LEFT){
      return directionLeft;
   }else{
      return directionRight;
   }
}
  void initMotorController() {
  // set all the motor control pins to outputs
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  }

  void setMotorSpeed(int i, int spd) {
    if(spd>MAX_PWM){
      spd=MAX_PWM;
    }
       if(spd<-MAX_PWM){
      spd=-1*MAX_PWM;
    }
    if (i == LEFT){
        if(spd>=0){
            directionLeft = FORWARDS;
            digitalWrite(in2, HIGH);
            digitalWrite(in1, LOW);
            analogWrite(enA, spd);
        }else if(spd < 0){
            directionLeft = BACKWARDS;
            digitalWrite(in1, HIGH);
            digitalWrite(in2, LOW);
            analogWrite(enA, -spd);
        }
    }
    else {
        if(spd>=0){
            directionRight = FORWARDS;
            digitalWrite(in3, HIGH);
            digitalWrite(in4, LOW);
            analogWrite(enB, spd);
        }else if(spd<0){
            directionRight = BACKWARDS;
            digitalWrite(in4, HIGH);
            digitalWrite(in3, LOW);
            analogWrite(enB, -spd);
        }
    }
  }

  void setMotorSpeeds(int leftSpeed, int rightSpeed) {
    setMotorSpeed(LEFT, leftSpeed);
    setMotorSpeed(RIGHT, rightSpeed);
  }