_SKU_ROB0124__全向HCR机器人移动平台 - jimaobian/DFRobotWikiCn GitHub Wiki
HCR Home Care Robot全向轮机器人是一款开源的家用移动机器人开发平台。与之前的版本相同，平台共分三层结构，每层都留有可用于安装距离传感器以及伺服舵机的通孔，并留有足够的空间让用户安装其他扩展模块。新版的HCR机器人配有三个全向轮，使机器人能够沿着任意方向进行平移或旋转。 全向HCR机器人移动平台兼容市面上主流的开发工具，包括Arduino，Raspberry Pi甚至个人电脑, 支持巡线、避障以及基于图像分析的互动功能。全向HCR能让作品随想法动起来。
注意事项：
- 本套件以散件形式提供，需用户自行组装。
- 本套件基础版包含所有的结构件、螺丝包、开关、插座及安装推荐方案所用到的全部零配件（如螺丝、铜柱、电源导线等，详情见装箱清单）。
- 说明书及我们的技术文档中提供了我们推荐的可选配置方案的安装说明，用户可根据自己的情况来决定购买可选配件。
- HCR 结构：铝合金4+1层结构
- URM04超声波模块安装位：6个
- GP2Y0A21红外测距模块安装位：12个
- 防跌落模块安装位：6个
- Kinect安装位：1个
- 舵机安装位：4个
- PC机安装位：2个
- 12V直流减速电机 122rpm 带编码器：3个
- 驱动轮数量：3个
- 驱动轮尺寸：直径100mm
- 组装后尺寸：31 * 31 * 60 cm
- 承重：10kg
| 序号 | 名称 | 编号 | 单位 | 数量 |
| 1 | 双路15A大功率电机驱动 | C1 | 件 | 2 |
| 2 | DC-DC模块 | C2 | 件 | 1 |
| 3 | GP2Y0A21距离传感器 | C3 | 件 | 6 |
| 4 | 数字防跌落传感器 | C4 | 件 | 6 |
| 5 | 14.8V、10A锂电池（带2A充电器） | C5 | 件 | 1 |
| 6 | Bluno Mega 2560控制器 | C6 | 件 | 1 |
| 7 | MEGA传感器扩展板V2.4 | C7 | 件 | 1 |
| 8 | USB/RS232/RS485/TTL协议转换器 | C8 | 件 | 1 |
| 9 | URM04 V2.0超声波测距模块 | C9 | 件 | 6 |
| 10 | 直径20mm胶圈 | C10 | 件 | 12 |
| 11 | DC2.1公头 | C10 | 件 | 1 |
可选配件清单
机械结构安装图
电源系统
内部连接图
在这个章节中，我们会介绍如何通过代码去控制HCR移动平台，样例中默认使用了推荐的设备。使用前，请先安装Arduino库
在这个章节，我们会用GoBLE手机APP作为手机远程遥控器，代码已集成PID调速控制。
|
#include "PID_v1.h"
#include <Metro.h>
#include "GoBLE.h"
#include <Math.h>
//Encoder variables
const int Interval=10; //Encoder data refresh time interval
const byte encoder1pinA = 18; //A pin -> the interrupt pin 18
const byte encoder1pinB = 21; //B pin -> the digital pin 21
const byte encoder2pinA = 19; //A pin -> the interrupt pin 19
const byte encoder2pinB = 22; //B pin -> the digital pin 22
const byte encoder3pinA = 20; //A pin -> the interrupt pin 20
const byte encoder3pinB = 23; //B pin -> the digital pin 23
byte encoder1PinALast;
byte encoder2PinALast;
byte encoder3PinALast;
int duration1; //the number of the pulses of Moter1 in the interval
int duration2; //the number of the pulses of Moter2 in the interval
int duration3; //the number of the pulses of Moter3 in the interval
int Speed1; //actual speed of motor1
int Speed2; //actual speed of motor2
int Speed3; //actual speed of motor3
double SpeedX; //actual speed of along X axis
double SpeedY; //actual speed of along Y axis
double SpeedZ; //actual speed of along Z axis
int SpeedInput1;
int SpeedInput2;
int SpeedInput3;
boolean Direction1; //the rotation Direction1
boolean Direction2; //the rotation Direction2
boolean Direction3; //the rotation Direction3
//Motor Driver variables
int M1 = 2; //M1 Direction Control
int M2 = 3; //M2 Direction Control
int M3 = 4; //M3 Direction Control
int E1 = 5; //M1 Speed Control
int E2 = 6; //M2 Speed Control
int E3 = 7; //M3 Speed Control
//PID variables
const double Motor_2[3]={0,2,0.03}; //PID parameters [P,I,D]
double Setpoint1,Input1,Output1; //PID input&output values for Motor1
double Setpoint2,Input2,Output2; //PID input&output values for Motor2
double Setpoint3,Input3,Output3; //PID input&output values for Motor3
PID myPID1(&Input1,&Output1,&Setpoint1,Motor_2[0],Motor_2[1],Motor_2[2],DIRECT);
PID myPID2(&Input2,&Output2,&Setpoint2,Motor_2[0],Motor_2[1],Motor_2[2],DIRECT);
PID myPID3(&Input3,&Output3,&Setpoint3,Motor_2[0],Motor_2[1],Motor_2[2],DIRECT);
char val='x';
//GoBLE Variables
void setup()
{
Goble.begin();
// Serial.begin(57600);//Initialize the serial port
EncoderInit();//Initialize encoder
int i; //Define output pin
for(i=2;i<=7;i++) pinMode(i, OUTPUT);
digitalWrite(E1,LOW);
digitalWrite(E2,LOW);
digitalWrite(E3,LOW);
myPID1.SetMode(AUTOMATIC);
myPID2.SetMode(AUTOMATIC);
myPID3.SetMode(AUTOMATIC);
myPID1.SetOutputLimits(-255,255);
myPID2.SetOutputLimits(-255,255);
myPID3.SetOutputLimits(-255,255);
}
void loop()
{
int joystickX, joystickY;
int buttonState[6];
int Turn=0; //actual speed of along Y axis
if(Goble.available()){
joystickX = Goble.readJoystickX();
joystickY = Goble.readJoystickY();
buttonState[SWITCH_UP] = Goble.readSwitchUp();
buttonState[SWITCH_DOWN] = Goble.readSwitchDown();
buttonState[SWITCH_LEFT] = Goble.readSwitchLeft();
buttonState[SWITCH_RIGHT] = Goble.readSwitchRight();
buttonState[SWITCH_SELECT] = Goble.readSwitchSelect();
buttonState[SWITCH_START] = Goble.readSwitchStart();
if (buttonState[2] == PRESSED) Turn=1;
if (buttonState[4] == PRESSED) Turn=-1;
// Serial.print(" X:");
// Serial.print(joystickX);
// Serial.print(" Y:");
// Serial.print(joystickY);
// for (int i = 1; i < 7; i++)
// {
// Serial.print(" B");
// Serial.print(i);
// Serial.print(":");
// if (buttonState[i] == PRESSED) Serial.print("On ");
// if (buttonState[i] == PRESSED) Serial.print("Off");
// }
SpeedInput1=(double(joystickY-128)+Turn*100)*1;
SpeedInput2=(0.866025 *double(joystickX-128)-0.5*double(joystickY-128)+Turn*100)*1;
SpeedInput3=(-0.866025 *double(joystickX-128)-0.5*double(joystickY-128)+Turn*100)*1;
// Serial.print(" Input1:");
// Serial.print( SpeedInput1);
// Serial.print(" Input2:");
// Serial.print( SpeedInput2);
// Serial.print(" Input3:");
// Serial.print( SpeedInput3);
// Serial.println("");
}
PIDMovement (SpeedInput1,SpeedInput2,SpeedInput3); //sets HCR to be moving in required state
Speed1=duration1*43/Interval; //calculates the actual speed of motor1, constant 43 for unifing the speed to the PWM input value
Speed2=duration2*43/Interval; //calculates the actual speed of motor1, constant 43 for unifing the speed to the PWM input value
Speed3=duration3*43/Interval; //calculates the actual speed of motor1, constant 43 for unifing the speed to the PWM input value
SpeedX=0.57735*Speed2-0.57735*Speed3; //calculates the actual speed alone X axis
SpeedY=0.666667*Speed1-0.333333*Speed2-0.333333*Speed3; //calculates the actual speed along Y axis
// Serial.print("Val:"); //speed serial print
// Serial.print(val);
// Serial.print(" M1:");
// Serial.print(Speed1);
// Serial.print(" M2:");
// Serial.print(Speed2);
// Serial.print(" M3:");
// Serial.print(Speed3);
// Serial.print(" X:");
// Serial.print(SpeedX);
// Serial.print(" Y:");
// Serial.print(SpeedY);
// Serial.println("");
duration1 = 0; //reset duration1 for the next counting cycle
duration2 = 0; //reset duration2 for the next counting cycle
duration3 = 0; //reset duration3 for the next counting cycle
delay(Interval);
//delay some certain time for aquiring pulse from encoder
}
//Motor modules
void stop(void)
{ //停止
digitalWrite(E1,0);
digitalWrite(M1,LOW);
digitalWrite(E2,0);
digitalWrite(M2,LOW);
digitalWrite(E3,0);
digitalWrite(M3,LOW);
}
//move without PWM control
void Movement(int a,int b,int c)
{
if (a>=0)
{
analogWrite (E1,a); //motor1 move forward at speed a
digitalWrite(M1,HIGH);
}
else
{
analogWrite (E1,-a); //motor1 move backward at speed a
digitalWrite(M1,LOW);
}
if (b>=0)
{
analogWrite (E2,b); //motor2 move forward at speed b
digitalWrite(M2,HIGH);
}
else
{
analogWrite (E2,-b); //motor2 move backward at speed b
digitalWrite(M2,LOW);
}
if (c>=0)
{
analogWrite (E3,c); //motor3 move forward at speed c
digitalWrite(M3,HIGH);
}
else
{
analogWrite (E3,-c); //motor3 move backward at speed c
digitalWrite(M3,LOW);
}
}
//PID modules
void PIDMovement(int a,int b,int c)
{
Setpoint1=a;
Setpoint2=b;
Setpoint3=c;
Input1=Speed1;
Input2=Speed2;
Input3=Speed3;
myPID1.Compute();
myPID2.Compute();
myPID3.Compute();
Movement (Output1,Output2,Output3);
}
//Encoder modules
void EncoderInit() //Initialize encoder interruption
{
Direction1 = true;//default -> Forward
Direction2 = true;//default -> Forward
Direction3 = true;//default -> Forward
pinMode(encoder1pinB,INPUT);
pinMode(encoder2pinB,INPUT);
pinMode(encoder3pinB,INPUT);
attachInterrupt(5, wheelSpeed1, CHANGE);
attachInterrupt(4, wheelSpeed2, CHANGE);
attachInterrupt(3, wheelSpeed3, CHANGE);
}
void wheelSpeed1() //motor1 speed count
{
int Lstate = digitalRead(encoder1pinA);
if((encoder1PinALast == LOW) && Lstate==HIGH)
{
int val = digitalRead(encoder1pinB);
if(val == LOW && Direction1)
{
Direction1 = false; //Reverse
}
else if(val == HIGH && !Direction1)
{
Direction1 = true; //Forward
}
}
encoder1PinALast = Lstate;
if(!Direction1) duration1++;
else duration1--;
}
void wheelSpeed2() //motor2 speed count
{
int Lstate = digitalRead(encoder2pinA);
if((encoder2PinALast == LOW) && Lstate==HIGH)
{
int val = digitalRead(encoder2pinB);
if(val == LOW && Direction2)
{
Direction2 = false; //Reverse
}
else if(val == HIGH && !Direction2)
{
Direction2 = true; //Forward
}
}
encoder2PinALast = Lstate;
if(!Direction2) duration2++;
else duration2--;
}
void wheelSpeed3() //motor3 speed count
{
int Lstate = digitalRead(encoder3pinA);
if((encoder3PinALast == LOW) && Lstate==HIGH)
{
int val = digitalRead(encoder3pinB);
if(val == LOW && Direction3)
{
Direction3 = false; //Reverse
}
else if(val == HIGH && !Direction3)
{
Direction3 = true; //Forward
}
}
encoder3PinALast = Lstate;
if(!Direction3) duration3++;
else duration3--;
}
|}
在这一章节中，HCR会执行自动避障程序，已集成PID控制。
|
#include "PID_v1.h"
#include <Metro.h>
#include "GoBLE.h"
#include <Math.h>
//Encoder variables
const int Interval=10; //Encoder data refresh time interval
const byte encoder1pinA = 18; //A pin -> the interrupt pin 18
const byte encoder1pinB = 21; //B pin -> the digital pin 21
const byte encoder2pinA = 19; //A pin -> the interrupt pin 19
const byte encoder2pinB = 22; //B pin -> the digital pin 22
const byte encoder3pinA = 20; //A pin -> the interrupt pin 20
const byte encoder3pinB = 23; //B pin -> the digital pin 23
byte encoder1PinALast;
byte encoder2PinALast;
byte encoder3PinALast;
int duration1; //the number of the pulses of Moter1 in the interval
int duration2; //the number of the pulses of Moter2 in the interval
int duration3; //the number of the pulses of Moter3 in the interval
int Speed1; //actual speed of motor1
int Speed2; //actual speed of motor2
int Speed3; //actual speed of motor3
double SpeedX; //actual speed of along X axis
double SpeedY; //actual speed of along Y axis
double SpeedZ; //actual speed of along Z axis
int SpeedInput1;
int SpeedInput2;
int SpeedInput3;
boolean Direction1; //the rotation Direction1
boolean Direction2; //the rotation Direction2
boolean Direction3; //the rotation Direction3
//Motor Driver variables
int M1 = 2; //M1 Direction Control
int M2 = 3; //M2 Direction Control
int M3 = 4; //M3 Direction Control
int E1 = 5; //M1 Speed Control
int E2 = 6; //M2 Speed Control
int E3 = 7; //M3 Speed Control
//PID variables
const double Motor_2[3]={0,2,0.03}; //PID parameters [P,I,D]
double Setpoint1,Input1,Output1; //PID input&output values for Motor1
double Setpoint2,Input2,Output2; //PID input&output values for Motor2
double Setpoint3,Input3,Output3; //PID input&output values for Motor3
PID myPID1(&Input1,&Output1,&Setpoint1,Motor_2[0],Motor_2[1],Motor_2[2],DIRECT);
PID myPID2(&Input2,&Output2,&Setpoint2,Motor_2[0],Motor_2[1],Motor_2[2],DIRECT);
PID myPID3(&Input3,&Output3,&Setpoint3,Motor_2[0],Motor_2[1],Motor_2[2],DIRECT);
char val='x';
//Infered distance sesnsor Variables
double IRdistance[6]; //distance return value of each sensor
double SensorPos[6]; //angles at which sensors are placed
double KIRp=10; //obsticle Feedback constant(Speed)
double KIRd=30; //obsticle Feedback constant(Distance)
double IRSetspeedX;
double IRSetspeedY;
double IRSetspeed1;
double IRSetspeed2;
double IRSetspeed3;
void setup()
{
Goble.begin();
//Serial.begin(9600);//Initialize the serial port
EncoderInit();//Initialize encoder
int i; //Define output pin
for(i=2;i<=7;i++) pinMode(i, OUTPUT);
digitalWrite(E1,LOW);
digitalWrite(E2,LOW);
digitalWrite(E3,LOW);
myPID1.SetMode(AUTOMATIC);
myPID2.SetMode(AUTOMATIC);
myPID3.SetMode(AUTOMATIC);
myPID1.SetOutputLimits(-255,255);
myPID2.SetOutputLimits(-255,255);
myPID3.SetOutputLimits(-255,255);
pinMode (A15, INPUT); //distance sensor
pinMode (A6, INPUT);
pinMode (A7, INPUT);
pinMode (A8, INPUT);
pinMode (A9, INPUT);
pinMode (A10, INPUT);
SensorPos[0]=30.0/180.0*3.14; //sensor position
SensorPos[1]=90.0/180.0*3.14;
SensorPos[2]=150.0/180.0*3.14;
SensorPos[3]=210.0/180.0*3.14;
SensorPos[4]=270.0/180.0*3.14;
SensorPos[5]=330.0/180.0*3.14;
}
void loop()
{
int joystickX, joystickY;
int buttonState[6];
int Turn=0;
Goble.available();
//if(Goble.available()){
if(1){
joystickX = Goble.readJoystickX();
joystickY = Goble.readJoystickY();
buttonState[SWITCH_UP] = Goble.readSwitchUp();
buttonState[SWITCH_DOWN] = Goble.readSwitchDown();
buttonState[SWITCH_LEFT] = Goble.readSwitchLeft();
buttonState[SWITCH_RIGHT] = Goble.readSwitchRight();
buttonState[SWITCH_SELECT] = Goble.readSwitchSelect();
buttonState[SWITCH_START] = Goble.readSwitchStart();
if (buttonState[2] == PRESSED) Turn=1;
if (buttonState[4] == PRESSED) Turn=-1;
// Serial.print(" X:");
// Serial.print(joystickX);
// Serial.print(" Y:");
// Serial.print(joystickY);
// for (int i = 1; i < 7; i++)
// {
// Serial.print(" B");
// Serial.print(i);
// Serial.print(":");
// if (buttonState[i] == PRESSED) Serial.print("On ");
// if (buttonState[i] == PRESSED) Serial.print("Off");
// }
IRdistance[0]=analogRead (A15);
IRdistance[1]=analogRead (A6);
IRdistance[2]=analogRead (A7);
IRdistance[3]=analogRead (A8);
IRdistance[4]=analogRead (A9);
IRdistance[5]=analogRead (A10);
IRSetspeedX=0;
IRSetspeedY=0;
for (int i=0;i<6;i++)
{
IRdistance[i]= GetIRdistance (IRdistance[i]);
if(IRdistance[i]<=15) IRdistance[i]=(20/IRdistance[i])*(20/IRdistance[i]);
else IRdistance[i]=0;
// Serial.print(" D");
// Serial.print(i+1);
// Serial.print(":");
// Serial.print(IRdistance[i]);
IRSetspeedX=IRSetspeedX- IRdistance[i]*double(KIRp)*cos(SensorPos[i]) - double(KIRd)* cos(SensorPos[i])*double(SpeedX)/sqrt(sq(cos(SensorPos[i]))+sq(sin(SensorPos[i])));
IRSetspeedY=IRSetspeedY- IRdistance[i]*double(KIRp)*sin(SensorPos[i]) - double(KIRd)* sin(SensorPos[i])*double(SpeedY)/sqrt(sq(cos(SensorPos[i]))+sq(sin(SensorPos[i])));
}
// Serial.print(" IRx:");
// Serial.print(IRSetspeedX);
// Serial.print(" IRy:");
// Serial.print(IRSetspeedY);
// SpeedInput1=(double(IRSetspeedY)+Turn*100)*1;
// SpeedInput2=(0.866025 *double(IRSetspeedX)-0.5*double(IRSetspeedY)+Turn*100)*1;
// SpeedInput3=(-0.866025 *double(IRSetspeedX)-0.5*double(IRSetspeedY)+Turn*100)*1;
SpeedInput1=(double(joystickY-128+IRSetspeedY)+Turn*100)*1;
SpeedInput2=(0.866025 *double(joystickX-128+IRSetspeedX)-0.5*double(joystickY-128+IRSetspeedY)+Turn*100)*1;
SpeedInput3=(-0.866025 *double(joystickX-128+IRSetspeedX)-0.5*double(joystickY-128+IRSetspeedY)+Turn*100)*1;
// Serial.print(" Input1:");
// Serial.print( SpeedInput1);
// Serial.print(" Input2:");
// Serial.print( SpeedInput2);
// Serial.print(" Input3:");
// Serial.print( SpeedInput3);
// Serial.println("");
}
PIDMovement (SpeedInput1,SpeedInput2,SpeedInput3); //sets HCR to be moving in required state
Speed1=duration1*43/Interval; //calculates the actual speed of motor1, constant 43 for unifing the speed to the PWM input value
Speed2=duration2*43/Interval; //calculates the actual speed of motor1, constant 43 for unifing the speed to the PWM input value
Speed3=duration3*43/Interval; //calculates the actual speed of motor1, constant 43 for unifing the speed to the PWM input value
SpeedX=0.57735*Speed2-0.57735*Speed3; //calculates the actual speed alone X axis
SpeedY=0.666667*Speed1-0.333333*Speed2-0.333333*Speed3; //calculates the actual speed along Y axis
// Serial.print("Val:"); //speed serial print
// Serial.print(val);
// Serial.print(" M1:");
// Serial.print(Speed1);
// Serial.print(" M2:");
// Serial.print(Speed2);
// Serial.print(" M3:");
// Serial.print(Speed3);
Serial.print(" X:");
Serial.print(SpeedX);
Serial.print(" Y:");
Serial.print(SpeedY);
Serial.println("");
if (SpeedX+SpeedY>100)
{
if(SpeedX>SpeedY) Keyboard.write(d);
else Keyboard.write(a));
}
duration1 = 0; //reset duration1 for the next counting cycle
duration2 = 0; //reset duration2 for the next counting cycle
duration3 = 0; //reset duration3 for the next counting cycle
delay(Interval);
//delay some certain time for aquiring pulse from encoder
}
//Motor modules
void stop(void)
{ //停止
digitalWrite(E1,0);
digitalWrite(M1,LOW);
digitalWrite(E2,0);
digitalWrite(M2,LOW);
digitalWrite(E3,0);
digitalWrite(M3,LOW);
}
//move without PWM control
void Movement(int a,int b,int c)
{
if (a>=0)
{
analogWrite (E1,a); //motor1 move forward at speed a
digitalWrite(M1,HIGH);
}
else
{
analogWrite (E1,-a); //motor1 move backward at speed a
digitalWrite(M1,LOW);
}
if (b>=0)
{
analogWrite (E2,b); //motor2 move forward at speed b
digitalWrite(M2,HIGH);
}
else
{
analogWrite (E2,-b); //motor2 move backward at speed b
digitalWrite(M2,LOW);
}
if (c>=0)
{
analogWrite (E3,c); //motor3 move forward at speed c
digitalWrite(M3,HIGH);
}
else
{
analogWrite (E3,-c); //motor3 move backward at speed c
digitalWrite(M3,LOW);
}
}
//PID modules
void PIDMovement(int a,int b,int c)
{
Setpoint1=a;
Setpoint2=b;
Setpoint3=c;
Input1=Speed1;
Input2=Speed2;
Input3=Speed3;
myPID1.Compute();
myPID2.Compute();
myPID3.Compute();
Movement (Output1,Output2,Output3);
}
//Encoder modules
void EncoderInit() //Initialize encoder interruption
{
Direction1 = true;//default -> Forward
Direction2 = true;//default -> Forward
Direction3 = true;//default -> Forward
pinMode(encoder1pinB,INPUT);
pinMode(encoder2pinB,INPUT);
pinMode(encoder3pinB,INPUT);
attachInterrupt(5, wheelSpeed1, CHANGE);
attachInterrupt(4, wheelSpeed2, CHANGE);
attachInterrupt(3, wheelSpeed3, CHANGE);
}
void wheelSpeed1() //motor1 speed count
{
int Lstate = digitalRead(encoder1pinA);
if((encoder1PinALast == LOW) && Lstate==HIGH)
{
int val = digitalRead(encoder1pinB);
if(val == LOW && Direction1)
{
Direction1 = false; //Reverse
}
else if(val == HIGH && !Direction1)
{
Direction1 = true; //Forward
}
}
encoder1PinALast = Lstate;
if(!Direction1) duration1++;
else duration1--;
}
void wheelSpeed2() //motor2 speed count
{
int Lstate = digitalRead(encoder2pinA);
if((encoder2PinALast == LOW) && Lstate==HIGH)
{
int val = digitalRead(encoder2pinB);
if(val == LOW && Direction2)
{
Direction2 = false; //Reverse
}
else if(val == HIGH && !Direction2)
{
Direction2 = true; //Forward
}
}
encoder2PinALast = Lstate;
if(!Direction2) duration2++;
else duration2--;
}
void wheelSpeed3() //motor3 speed count
{
int Lstate = digitalRead(encoder3pinA);
if((encoder3PinALast == LOW) && Lstate==HIGH)
{
int val = digitalRead(encoder3pinB);
if(val == LOW && Direction3)
{
Direction3 = false; //Reverse
}
else if(val == HIGH && !Direction3)
{
Direction3 = true; //Forward
}
}
encoder3PinALast = Lstate;
if(!Direction3) duration3++;
else duration3--;
}
//Distance sensor: return distance (cm)
double GetIRdistance (double value) {
if (value < 16) value = 16;
return 2076.0 / (value - 11.0);
}
|}
这一章节中，你可以通过串口来控制你的HCR机器人，已经成PID控制。
|
#include "PID_v1.h"
//Encoder variables
const int Interval=10;
const byte encoder1pinA = 18;//A pin -> the interrupt pin 18
const byte encoder1pinB = 21;//B pin -> the digital pin 21
const byte encoder2pinA = 19;//A pin -> the interrupt pin 19
const byte encoder2pinB = 22;//B pin -> the digital pin 22
const byte encoder3pinA = 20;//A pin -> the interrupt pin 20
const byte encoder3pinB = 23;//B pin -> the digital pin 23
byte encoder1PinALast;
byte encoder2PinALast;
byte encoder3PinALast;
int duration1;//the number of the pulses of Moter1
int duration2;//the number of the pulses of Moter2
int duration3;//the number of the pulses of Moter3
int Speed1;
int Speed2;
int Speed3;
boolean Direction1;//the rotation Direction1
boolean Direction2;//the rotation Direction1
boolean Direction3;//the rotation Direction1
//Motor Driver variables
int M1 = 2; //M1 Direction Control
int M2 = 3; //M2 Direction Control
int M3 = 4; //M3 Direction Control
int E1 = 5; //M1 Speed Control
int E2 = 6; //M2 Speed Control
int E3 = 7; //M3 Speed Control
//PID variables
const double Motor_2[3]={0.05,4,0.01};
double Setpoint1,Input1,Output1;
double Setpoint2,Input2,Output2;
double Setpoint3,Input3,Output3;
PID myPID1(&Input1,&Output1,&Setpoint1,Motor_2[0],Motor_2[1],Motor_2[2],DIRECT);
PID myPID2(&Input2,&Output2,&Setpoint2,Motor_2[0],Motor_2[1],Motor_2[2],DIRECT);
PID myPID3(&Input3,&Output3,&Setpoint3,Motor_2[0],Motor_2[1],Motor_2[2],DIRECT);
char val='s';
void setup()
{
Serial.begin(57600);//Initialize the serial port
EncoderInit();//Initialize encoder
int i; //Define output pin
for(i=2;i<=7;i++) pinMode(i, OUTPUT);
digitalWrite(E1,LOW);
digitalWrite(E2,LOW);
digitalWrite(E3,LOW);
myPID1.SetMode(AUTOMATIC);
myPID2.SetMode(AUTOMATIC);
myPID3.SetMode(AUTOMATIC);
myPID1.SetOutputLimits(-255,255);
myPID2.SetOutputLimits(-255,255);
myPID3.SetOutputLimits(-255,255);
}
void loop()
{
if(Serial.available()) val = Serial.read();
if(val != -1)
{
switch(val)
{
case 'w'://Movement1
PIDMovement (0,-200,200); //以最大速度前进
break;
case 'x'://Movement2
PIDMovement (0,150,-150); //以最大速度后退
break;
case 'a'://Turn Left
PIDMovement (100,100,100);
break;
case 'd'://Turn Right
PIDMovement (-100,-100,-100);
break;
case 'z':
stop();
break;
case 's':
PIDMovement (0,0,0);
break;
}
}
else stop();
Speed1=duration1*43/Interval;
Speed2=duration2*43/Interval;
Speed3=duration3*43/Interval;
Serial.print("Val Value:");
Serial.print(val);
Serial.print(" Motor1Speed:");
Serial.print(Speed1);
Serial.print(" Motor2Speed:");
Serial.print(Speed2);
Serial.print(" Motor3Speed:");
Serial.println(Speed3);
duration1 = 0;
duration2 = 0;
duration3 = 0;
delay(Interval);
}
//Motor modules
void stop(void)
{ //停止
digitalWrite(E1,0);
digitalWrite(M1,LOW);
digitalWrite(E2,0);
digitalWrite(M2,LOW);
digitalWrite(E3,0);
digitalWrite(M3,LOW);
}
void Movement(int a,int b,int c) //前进
{
if (a>=0)
{
analogWrite (E1,a); //PWM 速度控制
digitalWrite(M1,HIGH);
}
else
{
analogWrite (E1,-a); //PWM 速度控制
digitalWrite(M1,LOW);
}
if (b>=0)
{
analogWrite (E2,b); //PWM 速度控制
digitalWrite(M2,HIGH);
}
else
{
analogWrite (E2,-b); //PWM 速度控制
digitalWrite(M2,LOW);
}
if (c>=0)
{
analogWrite (E3,c); //PWM 速度控制
digitalWrite(M3,HIGH);
}
else
{
analogWrite (E3,-c); //PWM 速度控制
digitalWrite(M3,LOW);
}
}
//PID modules
void PIDMovement(int a,int b,int c)
{
Setpoint1=a;
Setpoint2=b;
Setpoint3=c;
Input1=Speed1;
Input2=Speed2;
Input3=Speed3;
myPID1.Compute();
myPID2.Compute();
myPID3.Compute();
Movement (Output1,Output2,Output3);
}
//Encoder modules
void EncoderInit() //Initialize encoder interruption
{
Direction1 = true;//default -> Forward
Direction2 = true;//default -> Forward
Direction3 = true;//default -> Forward
pinMode(encoder1pinB,INPUT);
pinMode(encoder2pinB,INPUT);
pinMode(encoder3pinB,INPUT);
attachInterrupt(5, wheelSpeed1, CHANGE);
attachInterrupt(4, wheelSpeed2, CHANGE);
attachInterrupt(3, wheelSpeed3, CHANGE);
}
void wheelSpeed1() //motor1 speed count
{
int Lstate = digitalRead(encoder1pinA);
if((encoder1PinALast == LOW) && Lstate==HIGH)
{
int val = digitalRead(encoder1pinB);
if(val == LOW && Direction1)
{
Direction1 = false; //Reverse
}
else if(val == HIGH && !Direction1)
{
Direction1 = true; //Forward
}
}
encoder1PinALast = Lstate;
if(!Direction1) duration1++;
else duration1--;
}
void wheelSpeed2() //motor2 speed count
{
int Lstate = digitalRead(encoder2pinA);
if((encoder2PinALast == LOW) && Lstate==HIGH)
{
int val = digitalRead(encoder2pinB);
if(val == LOW && Direction2)
{
Direction2 = false; //Reverse
}
else if(val == HIGH && !Direction2)
{
Direction2 = true; //Forward
}
}
encoder2PinALast = Lstate;
if(!Direction2) duration2++;
else duration2--;
}
void wheelSpeed3() //motor3 speed count
{
int Lstate = digitalRead(encoder3pinA);
if((encoder3PinALast == LOW) && Lstate==HIGH)
{
int val = digitalRead(encoder3pinB);
if(val == LOW && Direction3)
{
Direction3 = false; //Reverse
}
else if(val == HIGH && !Direction3)
{
Direction3 = true; //Forward
}
}
encoder3PinALast = Lstate;
if(!Direction3) duration3++;
else duration3--;
}
|}
| 更多问题及有趣的应用，可以 访问论坛 进行查阅或发帖！ |
