line 136-183:Init()函数，初始化引脚、时钟等
line 187-216:单电机控制函数集
line 218-307:机器人运动控制函数集
/*
#ifndef __TIMER2_FLAG__
#define __TIMER2_FLAG__
#include <FlexiTimer2.h>
#endif
*/#define WHEEL 127 //轮子直径127mm
#define STOP 0
#define RUNNING 1//电机驱动引脚连接方法：
//VCC接单片机5V电源，GND接单片机GND
//R_EN与L_EN短路并接5V电平，驱动器使能
//L_PWM输入PWM信号或高电平电机正转
//R_PWM输入PWM信号或高电平电机反转
typedef struct Motor
{int EN; //电机使能引脚int L_PWM; //L_PWM引脚int R_PWM; //R_PWM引脚int Encoder_A; //编码器A引脚int Encoder_B; //编码器B引脚int Encoder_Z; //编码器Z引脚int motor_state = STOP;volatile long Encoder_counter_A = 0;//volatile long Encoder_counter_B = 0;volatile double speed = 0.0;double time = 0.0;} Motor, *MotorPtr;//定义3个电机各个引脚,在"PIN"处填入实际的引脚号，PIN = 0表示未定义
//motor[0]，motor[1]，motor[2]分别对应：右底角处电机、顶角处电机、左底角处电机Motor motor[3];const double sampTime = 100; //采样周期/*****************************编码器函数*****************************/
void Speed() //计算速度与位移
{for (int i = 0; i < 3; i++){motor[i].speed = 60.0 * 1000.0 * (double)(motor[i].Encoder_counter_A) / (double)(13.0 * sampTime); //每转一圈输出13个脉冲//if debugSerial.print("The speed");Serial.print(i);Serial.print(" is: ");Serial.print(motor[i].speed);Serial.println("r/min");Serial.print("motor ");Serial.print(i);Serial.print(" counter: ");Serial.println(motor[i].Encoder_counter_A);motor[i].Encoder_counter_A = 0;}
}void Encode0()
{//为了不计入噪音干扰脉冲，//当2次中断之间的时间大于0.1ms时，计一次有效计数if ((millis() - motor[0].time) > 0.1){//当编码器码盘的OUTA脉冲信号下跳沿每中断一次，if ((digitalRead(motor[0].Encoder_A) == LOW) && (digitalRead(motor[0].Encoder_B) == HIGH)){motor[0].Encoder_counter_A++;}else{if ((digitalRead(motor[0].Encoder_A) == LOW) && (digitalRead(motor[0].Encoder_B) == LOW)){motor[0].Encoder_counter_A--;}}}motor[0].time == millis();
}void Encode1()
{//为了不计入噪音干扰脉冲，//当2次中断之间的时间大于0.1ms时，计一次有效计数if ((millis() - motor[1].time) > 0.1){//当编码器码盘的OUTA脉冲信号下跳沿每中断一次，if ((digitalRead(motor[1].Encoder_A) == LOW) && (digitalRead(motor[1].Encoder_B) == HIGH)){motor[1].Encoder_counter_A++;}else{if ((digitalRead(motor[1].Encoder_A) == LOW) && (digitalRead(motor[1].Encoder_B) == LOW)){motor[1].Encoder_counter_A--;}}}motor[1].time == millis();
}void Encode2()
{//为了不计入噪音干扰脉冲，//当2次中断之间的时间大于0.1ms时，计一次有效计数if ((millis() - motor[2].time) > 0.1){//当编码器码盘的OUTA脉冲信号下跳沿每中断一次，if ((digitalRead(motor[2].Encoder_A) == LOW) && (digitalRead(motor[2].Encoder_B) == HIGH)){motor[2].Encoder_counter_A++;}else{if ((digitalRead(motor[2].Encoder_A) == LOW) && (digitalRead(motor[2].Encoder_B) == LOW)){motor[2].Encoder_counter_A--;}}}motor[2].time == millis();
}/*****************************编码器函数*****************************/
//初始化电机引脚
void Init()
{motor[0].EN = 10;motor[0].L_PWM = 22;motor[0].R_PWM = 23;motor[0].Encoder_A = 2;motor[0].Encoder_B = 28;motor[1].EN = 11;motor[1].L_PWM = 24;motor[1].R_PWM = 25;motor[1].Encoder_A = 3;motor[1].Encoder_B = 29;motor[2].EN = 12;motor[2].L_PWM = 26;motor[2].R_PWM = 27;motor[2].Encoder_A = 21;motor[2].Encoder_B = 30;pinMode(motor[0].EN, OUTPUT);pinMode(motor[0].L_PWM, OUTPUT);pinMode(motor[0].R_PWM, OUTPUT);pinMode(motor[1].EN, OUTPUT);pinMode(motor[1].L_PWM, OUTPUT);pinMode(motor[1].R_PWM, OUTPUT);pinMode(motor[2].EN, OUTPUT);pinMode(motor[2].L_PWM, OUTPUT);pinMode(motor[2].R_PWM, OUTPUT);pinMode(motor[0].Encoder_A, INPUT); //编码器自带上拉，无需使用Arduino自带的上拉电阻pinMode(motor[0].Encoder_B, INPUT);pinMode(motor[1].Encoder_A, INPUT); //编码器自带上拉，无需使用Arduino自带的上拉电阻pinMode(motor[1].Encoder_B, INPUT);pinMode(motor[2].Encoder_A, INPUT); //编码器自带上拉，无需使用Arduino自带的上拉电阻pinMode(motor[2].Encoder_B, INPUT);attachInterrupt(0, Encode0, FALLING);attachInterrupt(1, Encode1, FALLING);attachInterrupt(2, Encode2, FALLING);//FlexiTimer2::set(sampTime, Speed); //单位时间测速//FlexiTimer2::start(); //打开定时器Timer2
}/****************************电机控制函数****************************/
//电机正转,num取值范围：{0,1,2}
void CW(int Rate, int num)
{Rate = constrain(Rate, 0, 255);analogWrite(motor[num].EN, Rate);digitalWrite(motor[num].L_PWM, HIGH);digitalWrite(motor[num].R_PWM, LOW);motor[num].motor_state = RUNNING;
}//电机反转,num取值范围：{0,1,2}
void CCW(int Rate, int num)
{Rate = constrain(Rate, 0, 255);analogWrite(motor[num].EN, Rate);digitalWrite(motor[num].L_PWM, LOW);digitalWrite(motor[num].R_PWM, HIGH);motor[num].motor_state = RUNNING;
}//刹车函数,num取值范围：{0,1,2}
void Stop(int num)
{digitalWrite(motor[num].EN, LOW);digitalWrite(motor[num].L_PWM, LOW);digitalWrite(motor[num].R_PWM, LOW);motor[num].motor_state = STOP;
}
/****************************电机控制函数****************************//*************************机器人运动控制函数*************************/
//刹车
void Brake()
{Stop(0);Stop(1);Stop(2);
}void Straight_forward_slow_start(int Rate)
{for (int i = 1; i <= Rate; i++){CW(i, 0);CCW(i, 2);delay(10);}
}//向前直行
void Straight_forward()
{//直行速度，待调参int Rate = 50;//motor[0]、motor[2]以相同的速率v反向旋转，motor[1]停转Stop(1);//若此时尚未启动，进入慢启动状态if (motor[0].motor_state == STOP || motor[2].motor_state == STOP){Straight_forward_slow_start(Rate);}CW(Rate, 0);CCW(Rate, 2);
}void Leftward_slow_start(int Rate)
{for (int i = 1; i <= Rate; i++){CW(i, 1);CCW((int)(i / 2), 0);CCW((int)(i / 2), 2);}
}//向左直行
void Leftward()
{//直行速度，待调参int Rate = 100;//motor[0]、motor[2]以相同的速率v反向旋转，motor[1]停转//若此时尚未启动if (motor[0].motor_state == STOP || motor[1].motor_state == STOP || motor[2].motor_state == STOP){Leftward_slow_start(Rate);}CW(Rate, 1);CCW((int)(Rate / 2), 0);CCW((int)(Rate / 2), 2);
}//向右直行
void Rightward()
{//直行速度，待调参int Rate = 100;//motor[0]、motor[2]以相同的速率v反向旋转，motor[1]停转CCW(Rate, 1);CW((int)(Rate / 2), 0);CW((int)(Rate / 2), 2);
}//向后直行
void Straight_backward()
{//直行速度，待调参int Rate = 100;//motor[0]、motor[2]以相同的速度反向旋转CCW(Rate, 0);CW(Rate, 2);
}
/*************************机器人运动控制函数*************************/