Hardware components | ||||||
 |
| × | 1 | |||
| × | 4 | ||||
| × | 2 | ||||
| × | 1 | ||||
Robo Bar Core
Read moreProject was built during classes of the Basics of Mechatronics on the second year of studying Mechatronical Engineering on the AGH University of Science and Technology.
Our task was to build a service robot. Guess what students of second year of Mechatronic Engineering could dream about? Of course it is a robot which can make every student party more memorable. We made up some assumptions. The robot should be able to pour drinks, tell jokes and fill the space around with colorful lights. We called it RoboBarCore.
The longest part of our robot is a rail which stretches along the bar. A trolley, at which we put a glass, moves along the rail. When we order a drink, the trolley starts to move in direction where stand our BarmanBot.
While the order is being executed according to the programmed recipe, our client is being entertained with jokes referring to, e.g., life of students. When the drink is ready, the trolley comes back to the laughing client, who can then return to friends and enjoy the party.
Bill of materialsTo build the RoboBarCore we used:
- 1x Husarion RoboCORE
- 4x Lego Mindstorms engines,
- 2x Husarion’s ultrasonic sensors,
- 1x bluetooth module (to receive data from wireless remote control working on Atmega),
- 4x strips with 8x SMT RGB 5050 LED,
- Besides Lego bricks, we used decorative ribbon (transmission of drive for trolley).
Purposes of using things listed above:
- wireless communication between RoboBarCore and user (client)
- checking if client put a glass property
- checking the liquid level In the glass
- checking if the bottle is empty
- registration of bottles during the movement of robot
- measuring the desired volume of a liquid
- returning information about errors
- controlling the lighting
1 / 3
RoboBarCore - program
C/C++This code was edited for basic wire communication, because we decided to not share our own remote control and its communicatrion protocol.
/*
Mot1 - Arm Motor
Mot2 - Grabber Motor
Mot3 - Turn Motor
Mot5 - Wheeler Motor
Mot6 - Interfejs
Sens1 - UltraFront
Sens2 - UltraGlass
Sens3 - Interfejs
*/
#include <hFramework.h>
#include <Lego_Touch.h>
#include <stdio.h>
#include <DistanceSensor.h>
#include "Lego_Touch.h"
using namespace hModules;
using namespace hFramework;
using namespace hSensors;
int StartingStep = 20;
int CntMargin = 40;
int Kp = 2;
int K = 6;
int BotCnt=0;
int BotCurrent=0;
bool Comment=true;
int_fast16_t iloscBotelek=7;
int_fast16_t iloscDrinkow=7;
// DrinkBook[iloscDrinkow][iloscBotelek];
int_fast8_t DrinkBook[16][8];
int ArmGrab;
int ArmUp;
int ArmTurn;
int ArmTank;
int Open;
int Close;
int TankConst=5;
int UnVelocity=70;
int Barman;
int TrackEnd;
int Client3;
int Client2;
int Client1;
DistanceSensor SensorFront(hSens1);
DistanceSensor SensorUpper(hSens2);
Lego_Touch sensor(hSens3);
int FrontSens(){
int state,state2;
int dist = SensorFront.getDistance();
if (dist > 1 && dist < 8) { // wykryto Botelke
state = 1;
}
else if (dist > 15 && dist < 22) { // wykryto koniec
state = 2;
}
else { // wykryto nic
state = 0;
}
sys.delay_ms(30);
dist = SensorFront.getDistance();
if (dist > 1 && dist < 8) { // wykryto Botelke
state2 = 1;
LED1.on();
LED2.off();
LED3.off();
}
else if (dist > 15 && dist < 22) { // wykryto koniec
state2 = 2;
LED2.on();
LED1.off();
LED3.off();
}
else { // wykryto nic
state2 = 0;
LED2.on();
LED3.on();
LED1.on();
}
if(state!=state2){
state=3;
LED3.on();
LED2.off();
LED1.off();
}
return state;
}
int UpperSens(){
int state,state2;
int dist = SensorUpper.getDistance();
if (dist > 1 && dist < 8) { // wykryto brak szklanki
state = 1;
}
else if (dist > 15 && dist < 22) { // wykryto pust1 szkanke
state = 2;
}
else if (dist > 15 && dist < 22) { // wykryto pe3n1 szklanke
state = 3;
}
else { // wykryto nic
state = 0;
}
sys.delay_ms(30);
dist = SensorUpper.getDistance();
if (dist > 1 && dist < 8) { // wykryto brak szklanki
state2 = 1;
}
else if (dist > 15 && dist < 22) { // wykryto pust1 szkanke
state2 = 2;
}
else if (dist > 15 && dist < 22) { // wykryto pe3n1 szklanke
state2 = 3;
}
else { // wykryto nic
state2 = 0;
}
if(state!=state2){
state=4;
}
return state;
}
int TurnZero(int Dir) {
if(Dir>0){Dir=1;}else{Dir=-1;}
int enc=0;
bool True=true;
while(True){
int state = FrontSens();
if(state!=2){
hMot3.setPower(Dir*437);
sys.delay_ms(20);
}
else
{
hMot3.setPower(0);
enc=hMot3.getEncoderCnt();
True=false;
}
}
LED3.off();
LED2.off();
LED1.off();
return enc;
}
int TurnBottleZero(int Dir) {
if(Dir>0){Dir=1;}else{Dir=-1;}
BotCnt=0;
bool True=true;
bool Botelka=false;
hMot3.setPower(Dir*400);
sys.delay_ms(500);
while(True){
int state = FrontSens();
if(hBtn2.isPressed()){state=2; sys.delay_ms(50);}
if(state==1 && !Botelka){
Botelka=true;
BotCnt++;
}
else if(state==0 && Botelka){
Botelka=false;
}
else if(state==2)
{
hMot3.setPower(0);
Botelka=false;
True=false;
}
}
LED3.off();
LED2.off();
LED1.off();
return BotCnt;
}
void TurnBottleGoto(int Goal) {
int Dir;
bool ToRight=false;
bool ToLeft=false;
bool Botelka;
if(Comment==true){printf("Goal %d\r\n", Goal); printf("BotCurrent %d\r\n", BotCurrent);}
if(Goal==BotCurrent){
Dir=0;
}else if(Goal>BotCurrent){
Dir=-1;
ToRight=true;
Botelka=false;
}else{
Dir=1;
ToLeft=true;
Botelka=true;
}
if(Comment==true){printf("ToRight %d\r\n", ToRight); printf("ToLeft %d\r\n", ToLeft);}
hMot3.setPower(Dir*400);
sys.delay_ms(400); // korekcja pierwszej butelki
while(ToRight){
int state = FrontSens();
if(state==1 && !Botelka){
Botelka=true;
}
else if(state==0 && Botelka){
Botelka=false;
BotCurrent++;
if(Comment==true){printf("BotCurrent %d\r\n", BotCurrent);}
if(Goal==BotCurrent){ ToRight=false; break; hMot3.setPower(0);}
else if(BotCurrent==BotCnt+1){//error();
}
}
else if(state==2)
{
hMot3.setPower(0);
Botelka=false;
ToRight=false;
}
}
while(ToLeft){
int state = FrontSens();
if(state==1 && !Botelka){
Botelka=true;
BotCurrent--;
if(Comment==true){printf("BotCurrent %d\r\n", BotCurrent);}
if(Goal==BotCurrent){ ToLeft=false; break; hMot3.setPower(0);}
else if(BotCurrent==0){//error();
}
}
else if(state==0 && Botelka){
Botelka=false;
}
else if(state==2)
{
hMot3.setPower(0);
Botelka=false;
ToLeft=false;
}
}
if(Dir==1) {
hMot3.setPower(-Dir*100);
ToRight=true;
while(ToRight){
int state = FrontSens();
if(state==1 && !Botelka){
Botelka=true;
}
else if(state==0 && Botelka){
Botelka=false;
ToRight=false;
hMot3.setPower(0);
}
else if(state==2)
{
hMot3.setPower(0);
Botelka=false;
ToRight=false;
}
}
}else if(Dir==-1){
hMot3.setPower(-Dir*100);
ToLeft=true;
while(ToLeft){
int state = FrontSens();
if(state==1 && !Botelka){
Botelka=true;
ToLeft=false;
hMot3.setPower(0);
}
else if(state==0 && Botelka){
Botelka=false;
}
else if(state==2)
{
hMot3.setPower(0);
Botelka=false;
ToLeft=false;
}
}
}else{}
LED3.off();
LED1.off();
LED2.off();
}
void GrabberGoto(int CntEnd) {
int CntCurrent = hMot2.getEncoderCnt();
int Sub = 0;
bool True = true;
int Dir = 0;
int sub = 0;
Sub = CntEnd - CntCurrent;
if (Sub < 0) {
Dir = -1;
} else {
Dir = 1;
}
for (int k = 0; k < 1000; k = k + StartingStep) {
hMot2.setPower(Dir * k);
sys.delay_ms(20);
CntCurrent = hMot2.getEncoderCnt();
Sub = CntEnd - CntCurrent;
CntCurrent = hMot2.getEncoderCnt();
if (abs(Sub) < CntMargin) {
k = 1000;
}
}
while (True) {
CntCurrent = hMot2.getEncoderCnt();
Sub = CntEnd - CntCurrent;
hMot2.setPower(Sub * K);
sys.delay_ms(20);
if (Sub < 0) {
sub = -Sub;
} else {
sub = Sub;
}
if (sub < CntMargin) {
True = false;
hMot2.setPower(0);
}
}
}
int_fast32_t ArmZero(int Dir){
if(Dir>0){Dir=1;}else{Dir=-1;}
int_fast32_t CntLast=hMot1.getEncoderCnt();
bool True=true;
hMot1.setPower(Dir*1000);
sys.delay_ms(20);
int_fast32_t CntCurrent=hMot1.getEncoderCnt();
while(True){
CntCurrent=hMot1.getEncoderCnt();
if(CntCurrent*Dir>CntLast*Dir)
{
hMot1.setPower(Dir*200);
}
else{
hMot1.stop();
LED2.on();
break;
}
sys.delay_ms(20);
CntLast=CntCurrent;
}
return CntCurrent;
}
void ArmGoto(int CntEnd) {
int CntCurrent = hMot1.getEncoderCnt();
int Sub = 0;
bool True = true;
int Dir = 0;
int sub = 0;
Sub = CntEnd - CntCurrent;
if (Sub < 0) {
Dir = -1;
} else {
Dir = 1;
}
for (int k = 0; k < 1000; k = k + StartingStep) {
hMot1.setPower(Dir * k);
sys.delay_ms(20);
CntCurrent = hMot1.getEncoderCnt();
Sub = CntEnd - CntCurrent;
CntCurrent = hMot1.getEncoderCnt();
if (abs(Sub) < CntMargin) {
k = 1000;
}
}
while (True) {
CntCurrent = hMot1.getEncoderCnt();
Sub = CntEnd - CntCurrent;
hMot1.setPower(Sub * K);
sys.delay_ms(20);
if (Sub < 0) {
sub = -Sub;
} else {
sub = Sub;
}
if (sub < CntMargin) {
True = false;
hMot1.setPower(0);
}
}
}
int_fast32_t WheelerZero(int Dir){
if(Dir>0){Dir=1;}else{Dir=-1;}
int_fast32_t CntLast=hMot5.getEncoderCnt();
bool True=true;
hMot5.setPower(Dir*1000);
sys.delay_ms(20);
int_fast32_t CntCurrent=hMot5.getEncoderCnt();
while(True){
CntCurrent=hMot5.getEncoderCnt();
if(CntCurrent*Dir>CntLast*Dir)
{
hMot5.setPower(Dir*400);
}
else{
hMot5.stop();
LED2.on();
break;
}
sys.delay_ms(20);
CntLast=CntCurrent;
}
return CntCurrent;
}
void WheelerGotoHardCore(int CntEnd){
hMot5.rotAbs(CntEnd);
while(true){
if(hMot5.getEncoderCnt()==CntEnd){
break;
}
}
}
void WheelerGoto(int CntEnd) {
int CntCurrent = hMot5.getEncoderCnt();
int Sub = 0;
bool True = true;
int Dir = 0;
int sub = 0;
Sub = CntEnd - CntCurrent;
if (Sub < 0) {
Dir = -1;
} else {
Dir = 1;
}
for (int k = 0; k < 1000; k = k + StartingStep) {
hMot5.setPower(Dir * k);
sys.delay_ms(20);
CntCurrent = hMot5.getEncoderCnt();
Sub = CntEnd - CntCurrent;
CntCurrent = hMot5.getEncoderCnt();
if (abs(Sub) < CntMargin) {
k = 1000;
}
}
while (True) {
CntCurrent = hMot5.getEncoderCnt();
Sub = CntEnd - CntCurrent;
hMot5.setPower(Sub * K);
sys.delay_ms(20);
if (Sub < 0) {
sub = -Sub;
} else {
sub = Sub;
}
if (sub < CntMargin) {
True = false;
hMot5.setPower(0);
}
}
}
void TurnGoto(int CntEnd) {
int CntCurrent = hMot3.getEncoderCnt();
int Sub = 0;
bool True = true;
int Dir = 0;
int sub = 0;
Sub = CntEnd - CntCurrent;
if (Sub < 0) {
Dir = -1;
} else {
Dir = 1;
}
for (int k = 0; k < 400; k = k + StartingStep) {
hMot3.setPower(Dir * k);
sys.delay_ms(20);
CntCurrent = hMot3.getEncoderCnt();
Sub = CntEnd - CntCurrent;
CntCurrent = hMot3.getEncoderCnt();
if (abs(Sub) < CntMargin) {
k = 1000;
}
}
int moc;
while (True) {
CntCurrent = hMot3.getEncoderCnt();
Sub = CntEnd - CntCurrent;
moc=Sub * K;
if(moc<-400){ moc=-400;}else if(moc>400){moc=400;}
hMot3.setPower(Sub * K);
sys.delay_ms(20);
if (Sub < 0) {
sub = -Sub;
} else {
sub = Sub;
}
if (sub < CntMargin) {
True = false;
hMot3.setPower(0);
}
}
}
int_fast32_t GrabberZero(int Dir) {
if (Dir > 0) {
Dir = 1;
} else {
Dir = -1;
}
int_fast32_t CntLast = hMot2.getEncoderCnt();
bool True = true;
hMot2.setPower(Dir * 1000);
sys.delay_ms(20);
int_fast32_t CntCurrent = hMot2.getEncoderCnt();
while (True) {
CntCurrent = hMot2.getEncoderCnt();
if (CntCurrent * Dir > CntLast * Dir)
{
hMot2.setPower(Dir * 200);
}
else {
hMot2.stop();
LED2.on();
break;
}
sys.delay_ms(20);
CntLast = CntCurrent;
}
return CntCurrent;
}
void WriteDrinkBook(){
//Drink #1
DrinkBook[1][1]=20;
DrinkBook[1][2]=20;
DrinkBook[1][3]=20;
DrinkBook[1][4]=20;
DrinkBook[1][5]=0;
DrinkBook[1][6]=0;
DrinkBook[1][7]=0;
//Drink #2
DrinkBook[2][1]=0;
DrinkBook[2][2]=0;
DrinkBook[2][3]=0;
DrinkBook[2][4]=0;
DrinkBook[2][5]=0;
DrinkBook[2][6]=0;
DrinkBook[2][7]=0;
//Drink #3
DrinkBook[3][1]=0;
DrinkBook[3][2]=0;
DrinkBook[3][3]=0;
DrinkBook[3][4]=0;
DrinkBook[3][5]=0;
DrinkBook[3][6]=0;
DrinkBook[3][7]=0;
//Drink #4
DrinkBook[4][1]=0;
DrinkBook[4][2]=0;
DrinkBook[4][3]=0;
DrinkBook[4][4]=0;
DrinkBook[4][5]=0;
DrinkBook[4][6]=0;
DrinkBook[4][7]=0;
//Drink #5
DrinkBook[5][1]=0;
DrinkBook[5][2]=0;
DrinkBook[5][3]=0;
DrinkBook[5][4]=0;
DrinkBook[5][5]=0;
DrinkBook[5][6]=0;
DrinkBook[5][7]=0;
//Drink #6
DrinkBook[6][1]=0;
DrinkBook[6][2]=0;
DrinkBook[6][3]=0;
DrinkBook[6][4]=0;
DrinkBook[6][5]=0;
DrinkBook[6][6]=0;
DrinkBook[6][7]=0;
//Drink #7
DrinkBook[7][1]=0;
DrinkBook[7][2]=0;
DrinkBook[7][3]=0;
DrinkBook[7][4]=0;
DrinkBook[7][5]=0;
DrinkBook[7][6]=0;
DrinkBook[7][7]=0;
//Drink #8
DrinkBook[8][1]=0;
DrinkBook[8][2]=0;
DrinkBook[8][3]=0;
DrinkBook[8][4]=0;
DrinkBook[8][5]=0;
DrinkBook[8][6]=0;
DrinkBook[8][7]=0;
//Drink #9
DrinkBook[9][1]=0;
DrinkBook[9][2]=0;
DrinkBook[9][3]=0;
DrinkBook[9][4]=0;
DrinkBook[9][5]=0;
DrinkBook[9][6]=0;
DrinkBook[9][7]=0;
//Drink #10
DrinkBook[10][1]=0;
DrinkBook[10][2]=0;
DrinkBook[10][3]=0;
DrinkBook[10][4]=0;
DrinkBook[10][5]=0;
DrinkBook[10][6]=0;
DrinkBook[10][7]=0;
//Drink #11
DrinkBook[11][1]=0;
DrinkBook[11][2]=0;
DrinkBook[11][3]=0;
DrinkBook[11][4]=0;
DrinkBook[11][5]=0;
DrinkBook[11][6]=0;
DrinkBook[11][7]=0;
//Drink #12
DrinkBook[12][1]=0;
DrinkBook[12][2]=0;
DrinkBook[12][3]=0;
DrinkBook[12][4]=0;
DrinkBook[12][5]=0;
DrinkBook[12][6]=0;
DrinkBook[12][7]=0;
//Drink #13
DrinkBook[13][1]=0;
DrinkBook[13][2]=0;
DrinkBook[13][3]=0;
DrinkBook[13][4]=0;
DrinkBook[13][5]=0;
DrinkBook[13][6]=0;
DrinkBook[13][7]=0;
//Drink #14
DrinkBook[14][1]=0;
DrinkBook[14][2]=0;
DrinkBook[14][3]=0;
DrinkBook[14][4]=0;
DrinkBook[14][5]=0;
DrinkBook[14][6]=0;
DrinkBook[14][7]=0;
//Drink #15
DrinkBook[15][1]=0;
DrinkBook[15][2]=0;
DrinkBook[15][3]=0;
DrinkBook[15][4]=0;
DrinkBook[15][5]=0;
DrinkBook[15][6]=0;
DrinkBook[15][7]=0;
}
void AddIngredient(int Volume){
ArmGoto(ArmGrab);
GrabberGoto(Close);
ArmGoto(ArmTank);
sys.delay_ms(UnVelocity*(Volume-TankConst));
ArmGoto(ArmGrab);
GrabberGoto(Open);
ArmGoto(ArmTurn);
}
void MakeDrink(int Num){
for(int i=1;i<iloscBotelek+1;i++){
if (DrinkBook[Num][i]!=0)
{
TurnBottleGoto(i);
AddIngredient(DrinkBook[Num][i]);
}
}
}
void CalibrateArm(){
ArmUp = ArmZero(1); // wykryj najwyzsza poz. ramienia
ArmGrab = ArmUp - 6017; //oblicz poz chwytu
ArmTurn = ArmUp - 3400; //oblicz poz obracania
ArmTank = ArmUp - 2600; //oblicz poz nalewania
}
void CalibrateTurn(){
TurnBottleZero(1);
sys.delay_ms(2000);
BotCurrent=TurnBottleZero(-1);
printf("BotCurrent %d\r\n", BotCurrent);
BotCurrent++;
}
void CalibrateGrabber(){
ArmGoto(ArmGrab);
Close = GrabberZero(1); // wykryj po3o?enie zamknietych szczek
Open = Close - 3200; // oblicz po3o?enie otwartych szczek.
GrabberGoto(Open);
ArmGoto(ArmTurn);
}
void CalibrateWheeler(){
TrackEnd = WheelerZero(-1); // wykryj koniec trasy
//WheelerGotoHardCore(TrackEnd + 5000);
Barman = WheelerZero(1); //wykryj poz barmana
Client3=TrackEnd;
Client2=TrackEnd+2000;
Client1=TrackEnd+4000;
}
void testNalewania(){
ArmUp = ArmZero(1); // wykryj najwyzsza poz. ramienia
ArmGrab = ArmUp - 6017; //oblicz poz chwytu
ArmTurn = ArmUp - 3400; //oblicz poz obracania
ArmTank = ArmUp - 2600; //oblicz poz nalewania
ArmGoto(ArmGrab);
Close = GrabberZero(1); // wykryj po3o?enie zamknietych szczek
Open = Close - 3200; // oblicz po3o?enie otwartych szczek.
ArmGoto(ArmTank);
sys.delay_ms(1000);
ArmGoto(ArmGrab);
GrabberGoto(Open);
}
void LED_GRY(bool G, bool R, bool Y){
if(G){LED1.on();}else{LED1.off();}
if(R){LED2.on();}else{LED2.off();}
if(Y){LED3.on();}else{LED3.off();}
}
void WaitForSW2(){
while(true)
{
if(hBtn2.isPressed()){
break;
}
else{
sys.delay_ms(50);
}
}
}
void wyslij(int num) {
for (int i = 0; i < num ;i++) {
hMot6.setPower(0);
sys.delay_ms(200);
hMot6.setPower(1000);
sys.delay_ms(50);
}
}
int odbierz() {
bool Read=sensor.isPressed();
bool LastRead;
int cnt=0;
if( Read ){
for(int i=0; i<500; i++){
Read=sensor.isPressed();
sys.delay_ms(1);
if(Read && !LastRead){
cnt++;
i=0;
}
LastRead=Read;
}
}
return cnt;
}
int waitForOdbierz(){
int war;
while(true)
{
war=odbierz();
if(war!=0){
break;
}
sys.delay_ms(20);
}
return war;
}
void Calibrate(){
CalibrateWheeler();
CalibrateArm();
CalibrateTurn();
CalibrateGrabber();
wyslij(1);
}
void program(){
hMot6.setPower(1000);
WriteDrinkBook();
Calibrate();
LED_GRY(true,true,true);
sys.delay_ms(200);
LED_GRY(false,false,false);
wyslij(1);
TurnBottleGoto(2);
while(true){
int client;
client=waitForOdbierz();
switch(client){
case 1:
WheelerGoto(Client1);
break;
case 2:
WheelerGoto(Client2);
break;
case 3:
WheelerGoto(Client3);
break;
}
sys.delay(500);
wyslij(1);
waitForOdbierz();
WheelerGoto(Barman);
wyslij(1);
int napoj;
napoj=waitForOdbierz();
MakeDrink(napoj);
switch(client){
case 1:
WheelerGoto(Client1);
break;
case 2:
WheelerGoto(Client2);
break;
case 3:
WheelerGoto(Client3);
break;
}
waitForOdbierz();
WheelerGoto(Barman);
TurnBottleGoto(2);
}
}
void TestTx(){
while(true){
LED_GRY(true,false,false);
wyslij(1);
sys.delay_ms(2000);
LED_GRY(false,true,false);
wyslij(2);
sys.delay_ms(2000);
LED_GRY(false,false,true);
wyslij(3);
sys.delay_ms(2000);
}
}
void TestRx(){
int a;
while(true){
a=waitForOdbierz();
if(a!=0){
switch(a){
case 1:
LED_GRY(true,false,false);
break;
case 2:
LED_GRY(false,true,false);
break;
case 3:
LED_GRY(false,false,true);
break;
}
}
}
}
void hMain()
{
program();
}
Thanks to Kamil Szpila and Piotr Zając.
Comments
Please log in or sign up to comment.