Project tutorial
Robotic Arm Plays Connect-Four

Robotic Arm Plays Connect-Four © GPL3+

Play Connect Four (the common board game) against a robotic arm.

  • 3,512 views
  • 1 comment
  • 11 respects

Components and supplies

About this project


I wanted to create a robotic arm which could move in an harmonic way and in this project, I have used it to play to Connect-Four.

The robotic arm is controlled by Arduino Due ( you can use any Arduino board, even Arduino Uno, but it will be quite slow )

I use as input a IR remote controller in order to choose both who starts (Arduino or player) and where you put your pawn. I used the minimax algorithm of Gianluca Ghettini, because I was focused on controlling the movement of the 6DOF arm.

Here's how I built it...

STEP ONE: Assembly

I calibrated the 6 MG966R servos, because every servo has a different pulse width for 0 degrees and 180 degrees. Then I assembled the kit ROTU3 with the 6 servo and I created a simple code for controlling and testing the arm, in which I could move only one servo by one.

STEP TWO: Simulation

Moving the servo one by one was boring and the arm was difficult to control, so I thougth it would have been better sending to Arduino the three cordinates (x, y, z), and changing these, it would have calculated the angle of all the 6 servos.

In order to do this, I created a 3D simulation on GeoGebra and with a bit of trigonometry: these were the results

Then I have implemented the code on Arduino ( with the right regulations ). Now, changing only three parameters (x, y, z) I could control the arm in an easy and intuitive way.

STEP THREE: Geometric Calculation

In this paragrafh I'll show you how Arduino find the the angle of all the servos on the base of the three coordinates (x, y, z).

We know that:

  • ‘A’ is the origin of our Cartesian sistem, so it has cordinates of 0, 0, 0..
  • ‘D’ is the point that we have to reach;
  • AB, BC, CD (humerus, ulna, gripper) are the lengths of each arm segments.

We want to know the angle in A, B, C.

Firstly we calculate AD with Pythagorean theorem ( I prefer that the point z = 0, is on the ground, so I put z = z-BaseHeight; however this doesn't change anything for user ).

Secondly, we make assumption that angle B and C are the same and to simplify our calculation we add the point ‘E’. The triangle BCE is isosceles, so BE = CE. This triangle can be divided in two right triangle, so:

We know all the angle, so we can send the angle with opporune regulation to the servos (in my case adding or subtracting 90° due to MY arm's frame). Now we can control our arm. Well yes, but there are some problems when

  • 'z' or 'y' are negative; nothing have been changing since when they are positive
  • If the coordinates are too big or too small it simply goes crazy.

The last problem is easier to correct, we have to add some limitation ( I have called them checkmin and checkmax) but to calculate them, we have to found where the calculation can give impossible result. The problem is in alfa, infact we have a square root, so we put everything is under that greater than zero (checkmin); moreover the function 'arrcos' accept valor from -1 to 1, so we set this limit (checkmax).

If we want that it reaches also negative 'z', we simply give at the second servo non gamma+omega, but gamma-omega.

Instead if we want that the arm goes to negative 'y', we gave to all the servos 180-the angle we should have given to them.

STEP FOUR: Improvements

After the first tries, I noticed that the arm movement were too fast, and therefore it was not nice to look and sometimes it crashed into something. So I written a function ( MyServoWriteGradual() ), where if the variation of servo position is too big, this function move the servos all togheter in a period of time which depends on the variation of the biggest position. (for example if a servo has to move of 50 degrees, the servo will arrive in position after 500 milliseconds, if it has to move of 100, it will arrive in 1000 millisecond).

STEP Five: Starting to Play to Connect-Four

I chose Gianluca Ghettini's code because it was simpler and more intuituve than others ones. I adapted it to my needs, making it more fast (reducing the number of future moves calculated)

A full game


Code

Robotic Arm CodeArduino
This is the arduino Code
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <IRremote2.h>
#include <LiquidCrystal.h>
int StartPos [3] = {0,-290,240};
int FourPos [6][7][3] = {  { { 95,150, 65},{ 60,145, 75},{ 28,135, 70},{  0,130, 70},{ -25,130, 70},{ -58,125, 75},{ -90,120, 65} },                   //x,y,z
                           { { 95,185, 70},{ 65,185, 70},{ 35,185, 70},{  0,180, 70},{ -30,175, 70},{ -63,170, 65},{ -95,165, 65} },
                           { { 95,210, 75},{ 65,220, 75},{ 35,220, 75},{  0,208, 70},{ -30,213, 75},{ -65,208, 70},{-100,203, 70} },
                           { { 95,250, 80},{ 65,250, 80},{ 35,245, 80},{  0,245, 80},{ -28,245, 80},{ -63,245, 80},{ -97,235, 75} },
                           { { 95,290, 90},{ 65,285, 90},{ 35,285, 90},{  5,280, 90},{ -30,285, 90},{ -63,280, 90},{ -98,280, 90} },
                           { {103,325,100},{ 68,325,100},{ 35,325,100},{  5,320,100},{ -25,325,100},{ -63,323,100},{ -98,318, 90} }, };
#define SKILL 10
#define ROW 6
#define COLUMN 7
 
int board[ROW*COLUMN];
long int depth,skill;
unsigned long int nodes;
/* ---------- IR VARIABLE ---------- */
// RECV_PIN = 52;
IRrecv irrecv(52);
decode_results results;

/* ---------- LIQUIDCRYSTAL VARIABLE ---------- */
// pin lcd: rs = 8, en = 9, d4 = 4, d5 = 5, d6 = 6, d7 = 7;
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);

/* ---------- ROBOTIC ARM VARIABLE ---------- */
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define BASE_HGT 115 //base height
/*#define HUMERUS  105   shoulder-to-elbow "bone"
  #define ULNA     100   elbow-to-wrist    "bone"
  #define GRIPPER  170   wrist-to-gripper  "bone"   */
#define SERVO1  1
#define SERVO2  2
#define SERVO3  3
#define SERVO4  4
#define SERVO5  5
#define SERVO6  6

#define SERVOMIN1  140
#define SERVOMAX1  475
#define SERVOMIN2  120
#define SERVOMAX2  415
#define SERVOMIN3  170
#define SERVOMAX3  465
#define SERVOMIN4  140
#define SERVOMAX4  485
#define SERVOMIN5  105
#define SERVOMAX5  415
#define SERVOMIN6  220
#define SERVOMAX6  355
unsigned int ServoMinMax[6][2] = {{SERVOMIN1,SERVOMAX1},
                                  {SERVOMIN2,SERVOMAX2},
                                  {SERVOMIN3,SERVOMAX3},
                                  {SERVOMIN4,SERVOMAX4},
                                  {SERVOMIN5,SERVOMAX5},
                                  {SERVOMIN6,SERVOMAX6}};

float x1,x2,x3,x4,x5,x6 = 90;                          // servo position
float oldx1,oldx2,oldx3,oldx4,oldx5,oldx6 = x1;        // old servo position

float xaxis =   0;
float yaxis = 150;
float zaxis = 100;

bool checkmin = true;
bool checkmax = true;

/* ---------- setup ---------- */
void setup() {
  Serial.begin(115200);
  randomSeed(analogRead(A0));
  lcd.begin(16, 2);
  lcd.clear();
  lcd.print(">> 4 in a row <<");
  lcd.setCursor(0,1);
  lcd.print("   by Danny003");
  delay(2000);
  pwm.begin();
  pwm.setPWMFreq(50);
  MyServoWrite(SERVO2,90);
  delay(1000);
  Set_Arm(xaxis,yaxis,zaxis);
  irrecv.enableIRIn();
  lcd.clear();
  delay(1000);
  ShowWin();
  delay(1000);
  ShowDefeat();
  delay(1000);
  Set_Arm(xaxis,yaxis,zaxis);
  lcd.print("Vuoi calibrare ? ");
  lcd.setCursor(0,1);
  lcd.print(" 1 = SI   0 = No");
  if ( Read_RC() == 1 ) { CalibraPos() ; }
}

void loop() {
  int move,j,i,coins;
  coins = ROW*COLUMN;
  skill = SKILL-5;
  int choice = 0;
  for (i=0; i<(ROW*COLUMN); i++) {board[i] = 0;}
  lcd.clear();
  lcd.print("Choose Who Start");
  lcd.setCursor(0,1);
  lcd.print("CH+=Comp CH-=You");
  Serial.println("Choose Who Start");
  Serial.println("CH+=Comp CH-=You");
  choice = Read_RC();   
  if (choice==10) { 
    display_board();
    while(coins!=0)  {
      if (coins==34) {skill=SKILL-4;} /* quick start */
      if (coins==30) {skill=SKILL-3;} /* quick start */
      if (coins==26) {skill=SKILL-2;} /* quick start */
      if (coins==22) {skill=SKILL-1;} /* quick start */
      if (coins==18) {skill=SKILL;  } /* to  maximum */
      do {     
        lcd.clear();
        lcd.print(" choose column  ");
        lcd.setCursor(0,1);
        lcd.print("   from 1 to 7  ");
        Serial.println("choose column [1-7]...");
        move=Read_RC();
        if (board[move-1]!=0) {
          lcd.clear();
          lcd.print("column is full");
          Serial.println("column is full");
          delay(500); }
        if ((move<1)||(move>7)) {
          lcd.clear();
          lcd.print("column not valid"); 
          Serial.println("column is not valid");
          delay(500); }
      } while((board[move-1]!=0)||(move<1)||(move>7));
      lcd.clear();
      lcd.print("You moved in ");
      lcd.setCursor(13,0);
      lcd.print(move);
      lcd.setCursor(0,1);
      lcd.print("I am thinking..."); 
      move--;
      j = ROW-1;
      while ((board[move+j*COLUMN]!=0)&&(j>=0)) {j--;}
      board[move+j*COLUMN] = 1;
      coins--;
      display_board();
      if(checkwin(1,move,4)) {
        lcd.clear();
        lcd.print("    Player  ");
        lcd.setCursor(0,1);
        lcd.print("    W I N S   ");
        Serial.println("You win\n");
        ShowDefeat();
        delay(3000);
        break;
      }
      move = best_move(-1);
      j = ROW-1;
      while ((board[move+j*COLUMN]!=0)&&(j>=0)) {j--;}
      board[move+j*COLUMN] = -1;
      Reach_Position(StartPos[0],StartPos[1],StartPos[2],90+degrees(atan2(-StartPos[0],abs(StartPos[1]))),FourPos[j][move][0],FourPos[j][move][1],FourPos[j][move][2],90+degrees(atan2(-FourPos[j][move][0],abs(FourPos[j][move][1]))));
      display_board();
      lcd.clear();
      lcd.print(" CPU move in ");
      lcd.print(move+1);
      Serial.println("CPU move in ");
      Serial.print(move+1);
      coins--;
      if (checkwin(-1,move,4)) {
        display_board();
        lcd.clear();
        lcd.print("    Computer  ");
        lcd.setCursor(0,1);
        lcd.print("    W I N S   ");
        Serial.println("CPU wins\n");
        ShowWin();
        delay(3000);
        break;
      }
      Serial.println("");
    }
    Serial.println("");
  }
  else if (choice == 11) {
    display_board();
    while(coins!=0) {
      if (coins==34) skill=SKILL-4; /* quick start */
      if (coins==30) skill=SKILL-3; /* quick start */
      if (coins==26) skill=SKILL-2; /* quick start */
      if (coins==22) skill=SKILL-1; /* quick start */
      if (coins==18) skill=SKILL;   /* ...to maximum */
      if (coins==ROW*COLUMN) {move=(random(0,8));}
      else  { move = best_move(-1); }
      j = ROW-1;
      while ((board[move+j*COLUMN]!=0)&&(j>=0)) {j--;}
      board[move+j*COLUMN] = -1;
      display_board();
      Reach_Position(StartPos[0],StartPos[1],StartPos[2],90+degrees(atan2(-StartPos[0],abs(StartPos[1]))),FourPos[j][move][0],FourPos[j][move][1],FourPos[j][move][2],90+degrees(atan2(-FourPos[j][move][0],abs(FourPos[j][move][1]))));
      lcd.clear();
      lcd.print(" CPU move in ");
      lcd.print(move+1);
      Serial.println("CPU move in ");
      Serial.print(move+1);
      coins--;
      if (checkwin(-1,move,4)) {
        display_board();
        lcd.clear();
        lcd.print("    Computer  ");
        lcd.setCursor(0,1);
        lcd.print("    W I N S   ");
        Serial.println("CPU wins\n");
        ShowWin();
        delay(3000);
        break;
      }
      Serial.println("");
      do {     
        lcd.clear();
        lcd.print(" choose column  ");
        lcd.setCursor(0,1);
        lcd.print("   from 1 to 7  ");
        Serial.println("choose column [1-7]...");
        move=Read_RC();
        if (board[move-1]!=0) {
          lcd.clear();
          lcd.print("column is full");
          Serial.println("column is full");
          delay(500);}
        if ((move<1)||(move>7)) {
          lcd.clear();
          lcd.print("column not valid"); 
          Serial.println("column is not valid");
          delay(500);}
      } while ((board[move-1]!=0)||(move<1)||(move>7));
      lcd.clear();
      lcd.print("You moved in ");
      lcd.setCursor(13,0);
      lcd.print(move);
      lcd.setCursor(0,1);
      lcd.print("I am thinking..."); 
      move--;
      j = ROW-1;
      while ((board[move+j*COLUMN]!=0)&&(j>=0)) {j--;}
      board[move+j*COLUMN] = 1;
      coins--;
      display_board();
      if (checkwin(1,move,4)) {
        Serial.println("You win\n");
        lcd.clear();
        lcd.print("    Player  ");
        lcd.setCursor(0,1);
        lcd.print("    W I N S   ");
        ShowDefeat();
        delay(3000);
        break; 
      }
    
      Serial.println("");
    }  
  }
}

/* ---------- MyServoWrite ---------- */
/* This funciont is used to move a singular servo*/
void MyServoWrite(int servo, float gradi) {
     pwm.setPWM( servo, 0, MapNew(gradi, 0.0, 180.0, ServoMinMax[servo-1][0], ServoMinMax[servo-1][1] ));
}

/* ---------- MyServoWriteAll ---------- */
/* This funciont is used to move the first 4 servo, it has been created to avoid to repeat the funciont MyServoWrite 4 times */
void MyServoWriteAll(float y1,float y2,float y3,float y4) {
  pwm.setPWM( SERVO1, 0, MapNew(y1, 0.0, 180.0, ServoMinMax[SERVO1-1][0], ServoMinMax[SERVO1-1][1] ));
  pwm.setPWM( SERVO2, 0, MapNew(y2, 0.0, 180.0, ServoMinMax[SERVO2-1][0], ServoMinMax[SERVO2-1][1] ));
  pwm.setPWM( SERVO3, 0, MapNew(y3, 0.0, 180.0, ServoMinMax[SERVO3-1][0], ServoMinMax[SERVO3-1][1] ));
  pwm.setPWM( SERVO4, 0, MapNew(y4, 0.0, 180.0, ServoMinMax[SERVO4-1][0], ServoMinMax[SERVO4-1][1] ));
}

/* ---------- MyServoWriteGradual ---------- */
/*This function is similar to MyServoWriteAll; but to avoid too rapid movement, 
 in particular when the robotic arm goes from a position to another one,this function creates 
 a smooth and simultaneous movement of the 4 servo, whose time depends on the variation of the bigger angle*/
void MyServoWriteGradual( float newpos1, float newpos2, float newpos3, float newpos4 ) {
  float oldpos1 = oldx1;
  float oldpos2 = oldx2;
  float oldpos3 = oldx3;
  float oldpos4 = oldx4;
  float differ1 = newpos1 - oldpos1;
  float differ2 = newpos2 - oldpos2;
  float differ3 = newpos3 - oldpos3;
  float differ4 = newpos4 - oldpos4;
  float largest = max(abs(differ1),max(abs(differ2), max(abs(differ3),abs(differ4))));
  for ( int i = 0; i <= largest; i++ ) {
     oldpos1 += (differ1/largest);
     oldpos2 += (differ2/largest);
     oldpos3 += (differ3/largest);
     oldpos4 += (differ4/largest);
     MyServoWriteAll(oldpos1,oldpos2,oldpos3,oldpos4);
     delay(10);       
  } 
  MyServoWriteAll(newpos1,newpos2,newpos3,newpos4);
} 
 
/* ---------- Set_Arm ---------- */
/* This function is the most important for the movement of the robotic arm, 
   because it calculates the postions of the first 4 servo from the three coordinates x,y,z(taken from TakePos and TrisPos).
   The following code is specific to the configuration of MY robotic arm, so if you want to use it, you have to adapt it to your arm.*/
void Set_Arm( float x, float y, float z) {
  oldx1 = x1;
  oldx2 = x2;
  oldx3 = x3;
  oldx4 = x4;
  z = z-BASE_HGT;
  float dist_y_z = sqrt( sq(z) +  sq(x) + sq(y) ); 
  if ( dist_y_z < 120.0 ) { checkmin=false; } 
    else { checkmin=true; }
  if ( dist_y_z > 370.0 ) { checkmax=false; } 
    else { checkmax=true; }
  if ( checkmin && checkmax ) {
    float alfa = 180.0-(degrees(acos((-27500 + sqrt( sq(27500) - 71400*(14225 - sq(dist_y_z))))/71400)));
    float gamma = degrees(acos((27875 -sq(dist_y_z) - (34000*cos(radians(alfa))))/(-210*dist_y_z)));
    float omega = degrees(acos(sqrt(sq(x)+sq(y))/dist_y_z));
    x1 = 90.0-degrees(atan2(-x,abs(y)));
    if ( z > 0.0 ) { x2=180.0-(gamma+omega); }
      else { x2=180.0-(gamma-omega); }  
    if ( y < 0.0) { x3=x4=(alfa-90.0); }
      else {
        x3=x4=(270.0-alfa);
        x2=180.0-x2;
        x1=180.0-x1;
      }
    if ((abs(oldx1-x1)+abs(oldx2-x2)+abs(oldx3-x3)+abs(oldx4-x4)) > 10.0) { MyServoWriteGradual(x1,x2,x3,x4); }
      else { MyServoWriteAll(x1,x2,x3,x4); }
  } 
}


/* ---------- Reach_Position ---------- */
/*Through this function, the Robotic Arm grabs a pawn  from a position and it leaves the pawn in another position  */
void Reach_Position ( float fromx, float fromy, float fromz, float fromangle, float tox, float toy, float toz, float toangle ) {
  Serial.println("test");
  Serial.println(tox);
  Serial.println(toy);
  Serial.println(toz);
  Serial.println(toangle);
  Set_Arm( fromx, fromy+50, fromz+50.0 );
  delay(300);
  MyServoWrite( SERVO6, 0 );
  MyServoWrite( SERVO5, fromangle );
  delay(1000);
  Set_Arm( fromx, fromy+50, fromz );
  delay(300);
  Set_Arm( fromx, fromy, fromz );
  delay(300);
  MyServoWrite( SERVO6, 165 );
  delay(500);
  Set_Arm( fromx, fromy+100, fromz );
  delay(500);
  Set_Arm( tox, toy, toz+150.0);
  delay(500);
  MyServoWrite( SERVO5, toangle );
  delay(300);
  Set_Arm( tox, toy, toz+50.0);
  delay(500);
  Set_Arm( tox, toy, toz );
  delay(300);
  MyServoWrite( SERVO6, 10 );
  delay(500);
  Set_Arm( tox, toy, toz + 50.0);
  delay(500);
  Set_Arm( xaxis = 0.0, yaxis = 130.0 , zaxis = 200.0 );
  MyServoWrite( SERVO5, 90 );
  }

/* ---------- MapNew ---------- */
/*This function has been created because the Arduino map() has not the funciont  constrain() and it does not return a int value */
int MapNew( float x, float in_min, float in_max, float out_min, float out_max ) {
   return round((constrain(x,in_min,in_max) - in_min) * (out_max - out_min) / (in_max - in_min) + out_min);
}
 
/* ---------- Check_Win ---------- */
/*This function check all the 8 possible combinations to win at tris. */

int checkwin(int player,int column,int lenght) {   
  long int j,r,l,i,height;
    lenght--;
    i = column;
    j = ROW-1;
    while(board[i+j*COLUMN]!=0) j--;
    j++;
    height = j;
 
    r = 0;
    l = 0;
    while(((++i)<COLUMN)&&(board[i+j*COLUMN]==player)) r++;
    i = column;
    while(((--i)>=0)&&(board[i+j*COLUMN]==player)) l++;
    if ((r+l)>=lenght) return 1;
    i = column;
 
    r = 0;
    if((j<0)){r--;}
    while(((++j)<ROW)&&(board[i+j*COLUMN]==player)) r++;
    if (r>=lenght) return 1;
    j = height;
 
    r = 0;
    l = 0;
    while(((++i)<COLUMN)&&((++j)<ROW)&&(board[i+j*COLUMN]==player)) r++;
    i = column;
    j = height;
    while(((--i)>=0)&&((--j)>=0)&&(board[i+j*COLUMN]==player)) l++;
    if ((r+l)>=lenght) return 1;
    i = column;
    j = height;
 
    r = 0;
    l = 0;
    while(((++i)<COLUMN)&&((--j)>=0)&&(board[i+j*COLUMN]==player)) r++;
    i = column;
    j = height;
    while(((--i)>=0)&&((++j)<ROW)&&(board[i+j*COLUMN]==player)) l++;
    if ((r+l)>=lenght) return 1;
 
    return 0;
}


int extimated_value(int player) {
    long int i,j,value,l; 
    value = 0;
    for(l=2;l<4;l++)
    {
        for(i=0;i<COLUMN;i++)
        {
            if(checkwin(player,i,l)) value = value + l;
        }
    }
 
    return value;
}

int goodness(int player,int depth,int column,int trigger) {
  long int max,i,value,j;
    max = -200;
    if (checkwin(-player,column,4)) return -128;
    if (depth==0) return 0;
    for(i=0;i<COLUMN;i++)
    {
        if(board[i]==0)
        {
            j = ROW-1;
            while(board[i+j*COLUMN]!=0) j--;
            board[i+j*COLUMN] = player;
            nodes++;
            value = -goodness(-player,depth-1,i,-max)/2;
            board[i+j*COLUMN] = 0;
            if (value>max) max = value;
            if (value>trigger) return max;
        }
    }
    return max;
}

int best_move(int player)
{
    long int i,j,max,value,best,same,trigger,old,att;
    max = -100;
    best = -1;
    long int maxnodes = -1;
    for(i=0;i<COLUMN;i++)
    {
        if(board[i]==0)
        {
            nodes = 0;
            j = ROW-1;
            while((board[i+j*COLUMN]!=0)&&(j>=0)) j--;
            board[i+j*COLUMN] = player;
            value = -goodness(-player,skill,i,200);
            //printf("\nmove %d goodness: %d   tree size for this move: %d nodes",i+1,value,nodes);
            board[i+j*COLUMN] = 0;
            if ((value == max) && (nodes>maxnodes))  {
          maxnodes=nodes;
        max = value;
                best = i;}
      if (value>max) {
          maxnodes=nodes;
        max = value;
                best = i; } 

         
      }
    }
    if(best==-1)
    {
        for(i=0;i<COLUMN;i++) if(board[i]==0) return i;
    }
 
    return best;
}



/* ---------- Read_RC ---------- */
/* This function read the button you pressed  */
int Read_RC(){
  int returnvalue=99;
  while (returnvalue==99){
    while (!irrecv.decode(&results)) {};
    switch(results.value){
      case 0xFFA25D: returnvalue = 10; break;     // CH-
      case 0xFFE21D: returnvalue = 11; break;     // CH+
      case 0xFF6897: returnvalue =  0; break;     //  0
      case 0xFF30CF: returnvalue =  1; break;     //  1 
      case 0xFF18E7: returnvalue =  2; break;     //  2
      case 0xFF7A85: returnvalue =  3; break;     //  3
      case 0xFF10EF: returnvalue =  4; break;     //  4
      case 0xFF38C7: returnvalue =  5; break;     //  5
      case 0xFF5AA5: returnvalue =  6; break;     //  6
      case 0xFF42BD: returnvalue =  7; break;     //  7
      case 0xFF4AB5: returnvalue =  8; break;     //  8
      case 0xFF52AD: returnvalue =  9; break;     //  9     
    }
    irrecv.resume();      
  }
  return returnvalue;
}



void display_board(void) {
  long int i,j;
  Serial.println("");
  for(j=0; j<ROW; j++) {
    for(i=0; i<COLUMN; i++) {
    if (board[i+j*COLUMN]== 1) {Serial.print("X ");}
    if (board[i+j*COLUMN]==-1) {Serial.print("O ");}
    if (board[i+j*COLUMN]== 0) {Serial.print(". ");}
    }
    Serial.println("");
  }
  for(i=0; i<(COLUMN*2)-1; i++) {Serial.print("-");}
  Serial.println("");
  for(i=0; i<COLUMN; i++) {Serial.print(i+1),Serial.print(" ");}
  Serial.println("");
}

void CalibraPos() {
  int scelta = 99;
  int col,row;
  while( scelta != 0 )  { 
    scelta = 99;
    lcd.clear();
    lcd.print(" Exit = 0  ");
    lcd.setCursor(0,1);
    lcd.print("cont. key not 0");
    if (Read_RC()==0) {break;}
    do {     
      lcd.clear();
      lcd.print(" choose column  ");
      lcd.setCursor(0,1);
      lcd.print("   from 1 to 7  ");
      Serial.println("choose column [1-7]...");
      col=Read_RC();
      if((col<1)||(col>7)) {
        lcd.clear();
        lcd.print("column not valid"); 
        Serial.println("column is not valid");
        delay(500);}
    } while((col<1)||(col>7));
    do {     
      lcd.clear();
      lcd.print("   choose row   ");
      lcd.setCursor(0,1);
      lcd.print("   from 1 to 6  ");
      Serial.println("choose row [1-6]...");
      row=Read_RC();
      if((row<1)||(row>6)) {
        lcd.clear();
        lcd.print("row not valid"); 
        Serial.println("row is not valid");
        delay(500);}
    } while((row<1)||(row>6)); 
    row--;
    col--;
    int calx = FourPos[row][col][0];
    int caly = FourPos[row][col][1];
    int calz = FourPos[row][col][2];
    while (scelta!=10) {
      lcd.clear();
      lcd.print("CH+=OK CH-=Exit"); 
      lcd.setCursor(0,1);
      lcd.print("x=   y=    z=   ");
      lcd.setCursor(2,1);
      lcd.print(calx);
      lcd.setCursor(7,1);
      lcd.print(caly);
      lcd.setCursor(13,1);
      lcd.print(calz);
      scelta=Read_RC();    
      switch(scelta) {
        case 1: calx--; break;
        case 3: calx++; break;
        case 4: caly--; break;
        case 6: caly++; break;
        case 7: calz--; break;
        case 9: calz++; break;
        case 11:
          FourPos[row][col][0] = calx;
          FourPos[row][col][1] = caly;
          FourPos[row][col][2] = calz;
          Reach_Position(StartPos[0],StartPos[1],StartPos[2],90+degrees(atan2(-StartPos[0],abs(StartPos[1]))),calx,caly,calz,90+degrees(atan2(-calx,abs(caly))));
          break;
      }
    }
  }  
}  

void ShowDefeat() {
  Set_Arm(0,130,100);
  for(int k=0; k<=180*2; k++) {
    MyServoWrite(SERVO6, abs(180*sin(radians(k)))); 
    delay(10);}
  }

void ShowWin() {
  Set_Arm(0,100,300);
  for(int k=0; k<=180*3; k ++){
    MyServoWrite(SERVO6, abs(180*sin(radians(k)))); 
    delay(4);}}

Schematics

SCHEMATICS
Follow the above wiring diagram to wire Arduino
Bracciorobotico tclze0lfht d7a2ejqsqw
SCHEMATICS
Follow the above wiring diagram to wire Arduino
bracciorobotico_6bnrlritut_Iwv5LJIuT9.fzz

Comments

Author

Danny003
DANNY003
  • 3 projects
  • 18 followers

Additional contributors

  • I get calculation tip from him by phpoc_man
  • Gianluca ghettini has created the minimax algoritm for playing to connect-four ( attribution also in the code) by Gianluca Ghettini

Published on

March 7, 2020

Members who respect this project

214 mk6jma8ycgDefaultAlbert einsteinSumayyah

and 7 others

See similar projects
you might like

Similar projects you might like

Robotic Arm Plays Tic-Tac-Toe

Project in progress by DANNY003

  • 5,167 views
  • 2 comments
  • 15 respects

Robotic Arm Controlled by Human Arm

Project showcase by zezarandrade

  • 12,175 views
  • 1 comment
  • 22 respects

Local and Remote Programmable Robotic Arm

Project tutorial by MJRoBot (Marcelo Rovai)

  • 29,772 views
  • 8 comments
  • 74 respects

Chatbot controlled Robotic Arm

Project tutorial by Keval Doshi

  • 6,351 views
  • 0 comments
  • 32 respects

PC Controlled Robotic Arm

Project tutorial by AhmedAzouz

  • 30,997 views
  • 16 comments
  • 109 respects

Arduino 3D-Printed Robotic Arm

Project tutorial by Mirko Pavleski

  • 24,571 views
  • 4 comments
  • 45 respects
Add projectSign up / Login