Lesson 25:Diego 1# 4WD —No.3:上位机通讯

ROS Arduino Bridge本质上是上位机通过串口发送控制命令来实现对Arduino的控制,所以我们要实现4驱的控制,我们也必须的修改通讯部分

1.Arduino firmware修改

1.1 command.h

在此文件中增加4驱所需的命令及宏定义

#ifndef COMMANDS_H
#define COMMANDS_H

#define ANALOG_READ    'a'
#define GET_BAUDRATE   'b'
#define PIN_MODE       'c'
#define DIGITAL_READ   'd'
#define READ_ENCODERS  'e'
#define MOTOR_SPEEDS   'm'
#define PING           'p'
#define RESET_ENCODERS 'r'
#define SERVO_WRITE    's'
#define SERVO_READ     't'
#define UPDATE_PID     'u'
#define DIGITAL_WRITE  'w'
#define ANALOG_WRITE   'x'
#define LEFT            0
#define RIGHT           1
#define LEFT_H          2 //新增
#define RIGHT_H         3 //新增
#define READ_PIDOUT    'f'
#define READ_PIDIN     'i'
#define READ_MPU6050   'g'

#endif

1.2 RosArduinoBridge-diego.ino

此文件是主程序,由于此文件代码比较多,故这里只介绍新增部分代码,完整的代码请见github

在runCommand()函数中修改在4驱模式下读取pidin 的代码

    case READ_PIDIN:
      Serial.print(readPidIn(LEFT));
      Serial.print(" ");
#ifdef L298P      
      Serial.println(readPidIn(RIGHT));
#endif
#ifdef L298P_4WD 
      Serial.print(readPidIn(RIGHT));
      Serial.print(" ");
      Serial.print(readPidIn(LEFT_H));
      Serial.print(" ");
      Serial.println(readPidIn(RIGHT_H));
#endif      
      break;

在runCommand()函数中修改在4驱模式下读取pidout 的代码

    case READ_PIDOUT:
      Serial.print(readPidOut(LEFT));
      Serial.print(" ");
#ifdef L298P      
      Serial.println(readPidOut(RIGHT));
#endif
#ifdef L298P_4WD 
      Serial.print(readPidOut(RIGHT));
      Serial.print(" ");
      Serial.print(readPidOut(LEFT_H));
      Serial.print(" ");
      Serial.println(readPidOut(RIGHT_H));
#endif       
      break;

在runCommand()函数中修改在4驱模式下读取编码器数据的代码

    case READ_ENCODERS:
      Serial.print(readEncoder(LEFT));
      Serial.print(" ");
#ifdef L298P 
      Serial.println(readEncoder(RIGHT));
#endif
#ifdef L298P_4WD
      Serial.print(readEncoder(RIGHT));
      Serial.print(" ");
      Serial.print(readEncoder(LEFT_H));
      Serial.print(" ");
      Serial.println(readEncoder(RIGHT_H));
#endif      
      break;

在runCommand()函数中修改在4驱模式下设置马达速度的代码

    case MOTOR_SPEEDS:
      /* Reset the auto stop timer */
      lastMotorCommand = millis();
      if (arg1 == 0 && arg2 == 0) {
#ifdef L298P        
        setMotorSpeeds(0, 0);
#endif

#ifdef L298P_4WD
        setMotorSpeeds(0, 0, 0, 0);
#endif        
        moving = 0;
      }
      else moving = 1;
      leftPID.TargetTicksPerFrame = arg1;
      rightPID.TargetTicksPerFrame = arg2;
#ifdef L298P_4WD      
      leftPID_h.TargetTicksPerFrame = arg1;
      rightPID_h.TargetTicksPerFrame = arg2;
#endif       
      Serial.println("OK");
      break;

在runCommand()函数中修改在4驱模式下更新PID参数的代码

    case UPDATE_PID:
      while ((str = strtok_r(p, ":", &p)) != '\0') {
        pid_args[i] = atoi(str);
        i++;
      }

      left_Kp = pid_args[0];
      left_Kd = pid_args[1];
      left_Ki = pid_args[2];
      left_Ko = pid_args[3];

      right_Kp = pid_args[4];
      right_Kd = pid_args[5];
      right_Ki = pid_args[6];
      right_Ko = pid_args[7];

#ifdef L298P_4WD

      left_h_Kp = pid_args[0];
      left_h_Kd = pid_args[1];
      left_h_Ki = pid_args[2];
      left_h_Ko = pid_args[3];

      right_h_Kp = pid_args[4];
      right_h_Kd = pid_args[5];
      right_h_Ki = pid_args[6];
      right_h_Ko = pid_args[7];
#endif
      
      Serial.println("OK");
      break;

在setup()函数中设置在4驱模式下新增的pin对应的寄存器的操作

void setup() {
  Serial.begin(BAUDRATE);
#ifdef USE_BASE
#ifdef ARDUINO_ENC_COUNTER
  //set as inputs
  DDRD &= ~(1 << LEFT_ENC_PIN_A);
  DDRD &= ~(1 << LEFT_ENC_PIN_B);
  DDRC &= ~(1 << RIGHT_ENC_PIN_A);
  DDRC &= ~(1 << RIGHT_ENC_PIN_B);

#ifdef L298P_4WD
  DDRD &= ~(1 << LEFT_H_ENC_PIN_A);
  DDRD &= ~(1 << LEFT_H_ENC_PIN_B);
  DDRC &= ~(1 << RIGHT_H_ENC_PIN_A);
  DDRC &= ~(1 << RIGHT_H_ENC_PIN_B);
#endif  

  //enable pull up resistors
  PORTD |= (1 << LEFT_ENC_PIN_A);
  PORTD |= (1 << LEFT_ENC_PIN_B);
  PORTC |= (1 << RIGHT_ENC_PIN_A);
  PORTC |= (1 << RIGHT_ENC_PIN_B);

#ifdef L298P_4WD
  PORTD &= ~(1 << LEFT_H_ENC_PIN_A);
  PORTD &= ~(1 << LEFT_H_ENC_PIN_B);
  PORTC &= ~(1 << RIGHT_H_ENC_PIN_A);
  PORTC &= ~(1 << RIGHT_H_ENC_PIN_B);
#endif    
  // tell pin change mask to listen to left encoder pins
  PCMSK2 |= (1 << LEFT_ENC_PIN_A) | (1 << LEFT_ENC_PIN_B);
  // tell pin change mask to listen to right encoder pins
  PCMSK1 |= (1 << RIGHT_ENC_PIN_A) | (1 << RIGHT_ENC_PIN_B);

#ifdef L298P_4WD
  // tell pin change mask to listen to left encoder pins
  PCMSK2 |= (1 << LEFT_ENC_PIN_A) | (1 << LEFT_ENC_PIN_B) | (1 << LEFT_H_ENC_PIN_A) | (1 << LEFT_H_ENC_PIN_B);
  // tell pin change mask to listen to right encoder pins
  PCMSK1 |= (1 << RIGHT_ENC_PIN_A) | (1 << RIGHT_ENC_PIN_B) | (1 << RIGHT_H_ENC_PIN_A) | (1 << RIGHT_H_ENC_PIN_B);
#endif
  // enable PCINT1 and PCINT2 interrupt in the general interrupt mask
  PCICR |= (1 << PCIE1) | (1 << PCIE2);
#endif
  initMotorController();
  resetPID();
#endif

  /* Attach servos if used */
#ifdef USE_SERVOS
  int i;
  for (i = 0; i < N_SERVOS; i++) {
    servosPos[i]=90;
  }
  servodriver.begin();
  servodriver.setPWMFreq(50);
#endif
}

在loop()函数中修改在4驱模式下自动停止的逻辑

  // Check to see if we have exceeded the auto-stop interval
  if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {
    ;
#ifdef L298P    
    setMotorSpeeds(0, 0);
#endif
#ifdef L298P_4WD
    setMotorSpeeds(0, 0, 0, 0);
#endif
    moving = 0;
  }

#endif

2.上位机代码修改

上位机的代码修改主要是针对arduino_driver.py和base_controller.py的修改。

2.1 arduino_driver.py修改

修改def get_pidin(self):使其支持4个电机的pidin读取

    def get_pidin(self):
        values = self.execute_array('i')
        print("pidin_raw_data: "+str(values))
        if len(values) not in [2,4]:
            print "pidin was not 2 or 4 for 4wd"
            raise SerialException
            return None
        else:                                                                  
            return values

修改def get_pidout(self):使其支持4个电机的pidout读取

    def get_pidout(self):
        values = self.execute_array('f')
        print("pidout_raw_data: "+str(values))
        if len(values) not in [2,4]:
            print "pidout was not 2 or 4 for 4wd"
            raise SerialException
            return None
        else:                                                                  
            return values

修改def get_encoder_counts(self):使其支持4个电机的编码器数据读取

    def get_encoder_counts(self):
        values = self.execute_array('e')
        if len(values) not in [2,4]:
            print "Encoder count was not 2 or 4 for 4wd"
            raise SerialException
            return None
        else:

修改完后,将arduino的固件更新,就可以启动4驱底盘控制了,要注意的时候要同时打开上位机和arduino上的4驱开关,如果是使用2驱的代码,也要记得关掉4驱的开关。

3.启动4驱底盘控制

现在执行如下命令可以启动4驱底盘控制

roslaunch diego_nav diego_arduino_run.launch 

现在我们可以通过配套的ROS APP连接Diego#进行控制

首先创建一个新的ROS机器人连接,设置相应的ROS Master ip,和cmd_vel主题。

连接上机器人后,可以选择操作杆,和重力感应来对机器人操作

Lesson 24:Diego 1# 4WD —No.2:motor control

针对马达控制我们主要需要修改两部分:

  • 驱动部分,使其可以支持同时控制4个马达
  • PID调速部分,试验表明,虽然是一样的型号的马达,但是给相同的PWM值,马达的转速也不一样,所以我们需要分别对4个马达进行PID调速

1.马达驱动

1.1. DualL298PMotorShield4WD.h文件的修改
增加了4个马达对应的PWM和方向控制的Pin引脚定义,和4个马达速度的设置函数

#ifndef DualL298PMotorShield4WD_h
#define DualL298PMotorShield4WD_h

#include 

class DualL298PMotorShield4WD
{
  public:  
    // CONSTRUCTORS
    DualL298PMotorShield4WD(); // Default pin selection.
    
    // PUBLIC METHODS
    void init(); // Initialize TIMER 1, set the PWM to 20kHZ. 
    void setM1Speed(int speed); // Set speed for M1.
    void setM2Speed(int speed); // Set speed for M2.
    void setM3Speed(int speed); // Set speed for M3.
    void setM4Speed(int speed); // Set speed for M4.
    void setSpeeds(int m1Speed, int m2Speed, int m3Speed, int m4Speed); // Set speed for both M1 and M2.
    
  private:
  
    // left motor
    static const unsigned char _M1DIR = 12;
    static const unsigned char _M2DIR = 7;
    static const unsigned char _M1PWM = 10;
    static const unsigned char _M2PWM = 6;
    
    // right motor
    static const unsigned char _M4DIR = 8;
    static const unsigned char _M3DIR = 13;
    static const unsigned char _M4PWM = 9;
    static const unsigned char _M3PWM = 11;
    
};

#endif

1.2. DualL298PMotorShield4WD.cpp文件的修改

主要是针对4个马达速度设置函数的实现,逻辑都是一样的,这里只截取一个马达的代码,代码的逻辑请看代码注释

// Set speed for motor 4, speed is a number betwenn -400 and 400
void DualL298PMotorShield4WD::setM4Speed(int speed)
{
  unsigned char reverse = 0;
  
  if (speed < 0) //速度是否小于0 { speed = -speed; // 如果小于0,则是反转 reverse = 1; // 反转标志变量设置为1 } if (speed > 255)  // 限定最大速度为255
    speed = 255;
  if (reverse)  //反转状态下
  {
    digitalWrite(_M4DIR,LOW);//设定马达转向的pin位低电平
    analogWrite(_M4PWM, speed);
  }
  else //正向转动状态下
  {
    digitalWrite(_M4DIR,HIGH);设定马达转向的pin位高电平
    analogWrite(_M4PWM, speed);
  }
}

在setSpeeds函数中分别调用4个马达的速度设置函数。

// Set speed for motor 1, 2, 3, 4
void DualL298PMotorShield4WD::setSpeeds(int m1Speed, int m2Speed, int m3Speed, int m4Speed)
{
  setM1Speed(m1Speed);
  setM2Speed(m2Speed);
  setM3Speed(m3Speed);
  setM4Speed(m4Speed);  
}

DualL298PMotorShield4WD.h和DualL298PMotorShield4WD.cpp修改完成后,在arduino 的library目录下新建一个名为dual-L298P-motor-shield-master-4wd的目录,将两个文件放到此文件夹下

现在我们打开android ide的library就可以看到我们刚才添加的库

1.1.3 motor_driver.h的修改

增加4驱马达控制的函数setMotorSpeeds,参数为4个马达的速度

void initMotorController();
void setMotorSpeed(int i, int spd);
#ifdef L298P
void setMotorSpeeds(int leftSpeed, int rightSpeed);
#endif
#ifdef L298P_4WD
void setMotorSpeeds(int leftSpeed_1, int leftSpeed_2, int rightSpeed_1, int rightSpeed_2);
#endif

1.1.4 motor_driver.ino的修改

#ifdef L298P_4WD
// A convenience function for setting both motor speeds
void setMotorSpeeds(int leftSpeed_1, int leftSpeed_2, int rightSpeed_1, int rightSpeed_2){
  setMotorSpeed(1, leftSpeed_1);
  setMotorSpeed(3, rightSpeed_1);
  setMotorSpeed(2, leftSpeed_2);
  setMotorSpeed(4, rightSpeed_2);
}
#endif
#else
#error A motor driver must be selected!
#endif

2.PID控制

PID控制都在diff_controller.h文件中定义修改

新增两个在4驱模式下的PID控制变量

#ifdef L298P_4WD
SetPointInfo leftPID_h, rightPID_h;
#endif

新增两个在4驱模式下的PID控制参数

#ifdef L298P_4WD

int left_h_Kp=Kp;
int left_h_Kd=Kd;
int left_h_Ki=Ki;
int left_h_Ko=Ko;

int right_h_Kp=Kp;
int right_h_Kd=Kd;
int right_h_Ki=Ki;
int right_h_Ko=Ko;

#endif

新增两个在4驱模式下的PID控制函数

#ifdef L298P_4WD
/* PID routine to compute the next motor commands */
void dorightPID_h(SetPointInfo * p) {
  long Perror;
  long output;
  int input;

  //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
  input =  p->Encoder-p->PrevEnc ;
  Perror = p->TargetTicksPerFrame - input;

  /*
  * Avoid derivative kick and allow tuning changes,
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  */
  //output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
  // p->PrevErr = Perror;
  output = (right_h_Kp * Perror - right_h_Kd * (input - p->PrevInput) + p->ITerm) / right_h_Ko;
  p->PrevEnc = p->Encoder;

  output += p->output;
  // Accumulate Integral error *or* Limit output.
  // Stop accumulating when output saturates
  if (output >= MAX_PWM)
    output = MAX_PWM;
  else if (output <= -MAX_PWM) output = -MAX_PWM; else /* * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ p->ITerm += left_h_Ki * Perror;

  p->output = output;
  p->PrevInput = input;
//  Serial.println("right output:");
//  Serial.println(p->output);
}
#endif
...
#ifdef L298P_4WD

/* PID routine to compute the next motor commands */
void doleftPID_h(SetPointInfo * p) {
  long Perror;
  long output;
  int input;

  //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
  input = p->Encoder-p->PrevEnc ;
  Perror = p->TargetTicksPerFrame - input;

  /*
  * Avoid derivative kick and allow tuning changes,
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
  * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  */
  //output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
  // p->PrevErr = Perror;
  output = (left_h_Kp * Perror - left_h_Kd * (input - p->PrevInput) + p->ITerm) / left_h_Ko;
  p->PrevEnc = p->Encoder;

  output += p->output;
  // Accumulate Integral error *or* Limit output.
  // Stop accumulating when output saturates
  if (output >= MAX_PWM)
    output = MAX_PWM;
  else if (output <= -MAX_PWM) output = -MAX_PWM; else /* * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ p->ITerm += left_h_Ki * Perror;

  p->output = output;
  p->PrevInput = input;
//  Serial.println("left output:");
//  Serial.println(p->output);
}
#endif

修改readPIDIn函数,增加在4驱模式下,pidin的读取

long readPidIn(int i) {
  long pidin=0;
  if (i == LEFT){
    pidin = leftPID.PrevInput;
  }else if (i == RIGHT){
    pidin = rightPID.PrevInput;
  }
#ifdef L298P_4WD
  else if (i== RIGHT_H){
    pidin = rightPID_h.PrevInput;
  }else{
    pidin = leftPID_h.PrevInput;
  }
#endif  
  return pidin;
}

修改readPIDOut函数,增加在4驱模式下,pidOut的读取

long readPidOut(int i) {
  long pidout=0;
  if (i == LEFT){
    pidout = leftPID.output;
  }else if (i == RIGHT){
    pidout = rightPID.output;
  }
#ifdef L298P_4WD
  else if (i == RIGHT_H){
    pidout = rightPID_h.output;
  }else{
    pidout = leftPID_h.output;
  }
#endif   
  return pidout;
}

修改updatePID函数,增加在4驱模式下马达PID控制的调用

void updatePID() {
  /* Read the encoders */
  leftPID.Encoder =readEncoder(LEFT);
  rightPID.Encoder =readEncoder(RIGHT);
#ifdef L298P_4WD 
  leftPID_h.Encoder =readEncoder(LEFT_H);
  rightPID_h.Encoder =readEncoder(RIGHT_H);
#endif
  
  /* If we're not moving there is nothing more to do */
  if (!moving){
    /*
    * Reset PIDs once, to prevent startup spikes,
    * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
    * PrevInput is considered a good proxy to detect
    * whether reset has already happened
    */
#ifdef L298P    
    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
#endif
#ifdef L298P_4WD
    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0 || leftPID_h.PrevInput != 0 || rightPID_h.PrevInput != 0) resetPID();
#endif    
    return;
  }

  /* Compute PID update for each motor */
  dorightPID(&rightPID);
  doleftPID(&leftPID);
#ifdef L298P_4WD
  dorightPID_h(&rightPID_h);
  doleftPID_h(&leftPID_h);
#endif  

  /* Set the motor speeds accordingly */
#ifdef L298P   
  setMotorSpeeds(leftPID.output, rightPID.output);
#endif

#ifdef L298P_4WD
  setMotorSpeeds(leftPID.output,leftPID_h.output, rightPID.output,rightPID_h.output);
#endif

}

至此4个马达已经可以独立驱动,独立的PID调速了,在下一篇教程中,会介绍如和上位机互动起来,让4驱底盘跑起来。

Lesson 23:Diego 1# 4WD —NO.1:ENCODER

4驱底盘由于四个轮子可以独立控制,所以具有优秀的通过性,这篇文章介绍Diego 1#的四驱底盘,所有源代码都已经上传到github。

这里会分几篇文章来介绍4驱动版diego 1#的开发,这篇我们主要说明4驱动底盘4个马达编码器数据的读取。

1.1硬件说明

  • 底盘材质:铝合金材质
  • 轮胎:12cm 橡胶轮胎
  • 电机:370直流电机,带霍尔码盘测速,输出AB项编码信号

1.2 控制器

  • 主控制器 arduino UNO,使用uno分别对4个电机进行方向,PWM控制,并采用终端方式采集4个电机输出的编码信号
  • 电机控制器 两块L298p, 网上采购,控制引脚不同
  • 上位机:树莓派,或者mini pc

2.编码器数据读取

所以代码都基于diego 1# github上的代码进行修改,这里只说明4驱版本部分的代码

首先我们在ROSAduinoBridge_diego.ino文件中定义一个预编译符号,这个预编译符号可以启动或者关闭4驱的代码,这样方便我们开关4驱的功能

#define L298P_4WD

在diego 1 4wd中实现了使用arduino uno读取4个马达的AB项编码输出,一般的网上说明中arduino uno只有两个外部中断,好像只能读取一个马达的AB项编码输出,但事实上arudino中所有IO引脚都可以作为中断使用,通过操作中断寄存器的方式。

首先我们在encoder_driver.h文件中定义编码器连接的Arduino uno引脚:

#ifdef ARDUINO_ENC_COUNTER
  //below can be changed, but should be PORTD pins; 
  //otherwise additional changes in the code are required
  #define LEFT_ENC_PIN_A PD2  //pin 2
  #define LEFT_ENC_PIN_B PD3  //pin 3
  
  //below can be changed, but should be PORTC pins
  #define RIGHT_ENC_PIN_A PC2  //pin A2
  #define RIGHT_ENC_PIN_B PC3   //pin A3

#ifdef L298P_4WD
  #define LEFT_H_ENC_PIN_A PD4  //pin 4
  #define LEFT_H_ENC_PIN_B PD5  //pin 5
  
  //below can be changed, but should be PORTC pins
  #define RIGHT_H_ENC_PIN_A PC0  //pin A0
  #define RIGHT_H_ENC_PIN_B PC1   //pin A1
#endif  
#endif

从此文件中可以看到在原来2驱的基础上增加了4WD模式下的编码器连接引脚定义,其中左后方的电机AB项连接数字IO的D4和D5,而右后方电机AB项连接模拟IO的A0和A1,这样4个马达的编码器数据读取我们就用到8个IO口,加上PWM控制,转动方向的控制,在Diego 1# 4WD版本中,底盘控制一共用了16个IO,最后只剩D0,D1作为串口和上位机通讯,和A4,A5作为I2C的接口与I2C接口模块通讯,可以说Arduino UNO做到了充分利用。

在encoder_driver.ino文件中增加对4WD新增引脚的中断处理。

arduino uno中一共有3个引脚中断函数分别是

  • ISR (PCINT0_vect)对应 D8 to D13
  • ISR (PCINT1_vect) 对应 A0 to A5
  • ISR (PCINT2_vect) 对应 D0 to D7

diego1# 4wd中只需要用到两个中断处理函数 ISR(PCINT2_vect)和ISR(PCINT1_vect),代码如下:

  ISR (PCINT2_vect){
     static uint8_t enc_last=0;
#ifdef L298P_4WD        
     static uint8_t enc_last_h=0;
#endif          
     enc_last <<=2; //shift previous state two places
     enc_last |= (PIND & (3 << 2)) >> 2; //read the current state into lowest 2 bits

#ifdef L298P_4WD
     enc_last_h<<=2;
     enc_last_h |=(PIND & (3 << 4))>>4;
#endif 
  
     left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
#ifdef L298P_4WD   
     left_h_enc_pos +=ENC_STATES[(enc_last_h & 0x0f)];
#endif    
  }
  
  /* Interrupt routine for RIGHT encoder, taking care of actual counting */
  ISR (PCINT1_vect){
     static uint8_t enc_last=0;
#ifdef L298P_4WD        
     //uint8_t pinct=PINC;
     static uint8_t enc_last_h=0;
#endif   	
     enc_last <<=2; //shift previous state two places
     enc_last |= (PINC & (3 << 2)) >> 2; //read the current state into lowest 2 bits

#ifdef L298P_4WD
     enc_last_h<<=2;
     enc_last_h |=(PINC & 3);
#endif  
     right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
#ifdef L298P_4WD   
     right_h_enc_pos +=ENC_STATES[(enc_last_h & 0x0f)];
#endif
  }

这段代码中主要是针对中断寄存器PINC和PIND的操作,每个IO pin都对应PINC或者PIND的一位,IO有中断产生时,对应的位就会被置位,我们只需要读取相应为即可,如(PIND & (3 << 4))>>4读取的就是PIND的第4,5位,也就是D4,D5的数据。

Scroll to top