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:
- The platform is sold in kit, further assembly will be needed.
- Kit contains all structural parts including metal sheets parts, screw sets, switches and sockets (refer to packing list for more details).
- Recommended component list and circuit diagram is included in the installation manual, user may add other items based on demand.
Specification
- Structure: 4+1 Full Metal Structure
- URM04 Ultrasonic Sensor Module Installation Slot: 6
- GPY0A21 Inferred Sensor Module Installation Slot: 12
- Greyscale Sensor Module Installation Slot: 6
- Xbox Kinect Camera Mount: 1
- Servo Installation Slot: 4
- PC installation Slot: 2
- Motor type: 12V DC Motor 122rpm w/Encoder
- Quantity of Motor: 3
- Quantity of Driving Wheel: 3
- Driving Wheel diameter: 100mm
- Dimension (after assembly): 31*31*60 cm/ 12.2*12.2*23.6 inches
- Max Load: 10kg
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
More Structure Installation, please refer to Assemble Manual
Power System
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. |
|---|