HCR_Mobile_Platform_with_Omni_Wheels_SKU__ROB0124-DFRobot

Introduction

HCR (Home Care Robot) with Omni wheels is a full metal open source mobile platform designed for home robotics use and development. Same as previous generations, the structure of the HCR is divided into 3 layers, each with slots for installing distance sensors, servos or cameras. There is also plenty of space for other expansions modules. More than that, equipped with 3 high torque Omni wheels with encoders, it is capable of all-direction pinpoint transitional motions and turning motions. The new HCR is compatible with a wide range of development tools including “Arduino”, “Raspberry Pi”or even a Windows OS PC, allowing functions such as line tracking, obstacle avoiding or graphical based interaction. With HCR, you can turn anything into mobile.

Note:

Specification

Optional Add-ons

No. Name Number Quantity
1 DC Motor Driver 2×15A - Lite C1 2
2 72W DC-DC Converter 12V@6A C2 1
3 Sharp GP2Y0A21 Distance Sensor (10-80cm) C3 6
4 Gravity:Digital Infrared Distance Sensor (10cm) C4 6
5 14.8V@10A lipo battery C5 1 -
6 Bluno Mega 2560 - An Arduino Mega 2560 with Bluetooth 4.0 C6 1
7 Mega Sensor Shield V2.4 C7 1
8 Multi USB/RS232/RS485/TTL Converter C8 1
9 URM04 v2.0 Ultrasonic Sensor C9 6
10 URM ultrasonic sensor Rubber Ring C10 12
11 DC Barrel Jack Adapter - Male C10 1

Optional Add-ons

Connection Diagram

Mechanical Structure Installation HCR Mobile Platform with Omni Wheels Bottom HCR Mobile Platform with Omni Wheels top

More Structure Installation, please refer to Assemble Manual

Power System HCR Power System Internal Connection HCR Internal Connection

HCR Mobile Platform Sample Code

In this section, we'll teach you how to control HCR platform with sample code, using the recommended device. Please install the Arduino library first.

HCR_GoBLE_Control_with_PID

Control HCR via iPhone “GoBLE” APP. Supports all direction transitional motion and rotation control.


#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_Obstacle_Aviodance

Control HCR via iPhone “GoBLE” APP. Supports all direction transitional motion and rotation control. Also add all-direction obstacle avoidance.


#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_Serial_Control_with_PID

Control HCR via serial port. Supports all direction transitional motion and rotation control. Note: Bluno series microcontrollers support wireless programming and serial debugging. For more information and tutorial, go to link


#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':
     PIDMovement (0,-200,200);       //Move forward
        break;
      case 'x':
      PIDMovement (0,200,-200);      //Move backward
        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)                 //Stop
{
  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 Speed Control
    digitalWrite(M1,HIGH);
  }
  else
  {
    analogWrite (E1,-a);      //PWM Speed Control
    digitalWrite(M1,LOW);
  }
  if (b>=0)
  {
    analogWrite (E2,b);       //PWM Speed Control
    digitalWrite(M2,HIGH);
  }
  else
  {
    analogWrite (E2,-b);     //PWM Speed Control
    digitalWrite(M2,LOW);
  }
  if (c>=0)
  {
    analogWrite (E3,c);      //PWM Speed Control
    digitalWrite(M3,HIGH);
  }
  else
  {
    analogWrite (E3,-c);     //PWM Speed Control
    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--;
}

FAQ

For any question/advice/cool idea to share, please visit DFRobot Forum.

More