全向HCR家用机器人移动平台

简介

HCR Home Care Robot全向轮机器人是一款开源的家用移动机器人开发平台。与之前的版本相同,平台共分三层结构,每层都留有可用于安装距离传感器以及伺服舵机的通孔,并留有足够的空间让用户安装其他扩展模块。新版的HCR机器人配有三个全向轮,使机器人能够沿着任意方向进行平移或旋转。 全向HCR机器人移动平台兼容市面上主流的开发工具,包括Arduino,Raspberry Pi甚至个人电脑, 支持巡线、避障以及基于图像分析的互动功能。全向HCR能让作品随想法动起来。

注意事项:

产品规格

可选配件清单

序号 名称 编号 单位 数量
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安装手册

电源系统 电源系统

内部连接图

全向移动HCR示例代码

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

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--;
}

疑难解答

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

更多

shopping_car.png DFRobot商城购买链接