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 oneint enA = 5;int in1 = 7;int in2 = 8;// motor twoint 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);  }