简介

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

可选配件清单

可选配件清单

系统连接图

机械结构安装图
更多结构安装，请参考全向HCR安装手册

电源系统

内部连接图

全向移动HCR示例代码

在这个章节中，我们会介绍如何通过代码去控制HCR移动平台，样例中默认使用了推荐的设备。使用前，请先安装Arduino库

• Arduino-PID-Library

• GoBLE

• Metro

GoBLE手机控制（带PID）

在这个章节，我们会用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避障

在这一章节中，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--;
}

疑难解答

更多问题及有趣的应用，可以 访问论坛 进行查阅或发帖！ |

更多

• Arduino-PID-Library

• GoBLE

• Metro

• 全向HCR安装手册

DFRobot商城购买链接