Robot Arm Automation

This project has been developed for educational purposes and can be used in the field of robot development.

Jan 7, 2022

•

22337 views

•

1 respects

smart appliances

remote control

robots

Components and supplies

HC-05 Bluetooth Module

AT24C256 Serial EEPROM I2C Interface EEPROM Data Storage Module For Arduino

Adafruit PCA9685 16-Channel Servo Driver

Arduino UNO

Arm Robot

Tools and machines

Breadboard, 170 Pin

10 Pc. Jumper Wire Kit, 5 cm Long

Premium Female/Male Extension Jumper Wires, 40 x 6" (150mm)

Apps and platforms

Arduino IDE

Project description

Code

Robot-bluetooth-prog-pca

arduino

1// Main programm - Robot controler
2// LCD =================================   
3#include <LiquidCrystal_PCF8574.h>
4#include <Wire.h>
5LiquidCrystal_PCF8574   lcd(0x38); // I2C
6int lcdpres = 0;                                // to put to   0 if LCD not present
7// EEPROM ==============================
8#define EEPROM_I2C_ADDRESS   0x50  // I2C
9int eprdata[6];                  // data read from eeprom
10int   epractmax[2];                // cursor position to/from eeprom
11// BLUETOOTH ===========================
12#include   "SoftwareSerial.h"
13SoftwareSerial bluetooth(2, 3);  // RX, TX
14int bluerec   ;                    // bluetooth reception variable
15// SERVOS =======================================================
16#include   <Adafruit_PWMServoDriver.h>
17Adafruit_PWMServoDriver pca= Adafruit_PWMServoDriver(0x40);   // I2C
18#define nbPCAServo 6
19int MIN_IMP [nbPCAServo] ={500, 500, 500, 500,   500, 500};
20int MAX_IMP [nbPCAServo] ={2500, 2500, 2500, 2500, 2500, 2500};
21int   MIN_ANG [nbPCAServo] ={0, 0, 0, 0, 0, 0} ;
22int MAX_ANG [nbPCAServo] ={180,180,   180, 180, 180, 180 };               // servo angle capability
23int tabanglim[2][6]   = {{0,43,65,0,10,80}, {180,177,180,140,175,116}};   // Limits the servo stroke.
24int   servodelay(15);              // delay for servo to achieve rotation
25int servo;                        // current servo number (from 0 to 5)
26int homepos[6] =   {93, 124, 115, 118, 83, 80};   // robot home position (will be stored to eeprom   on setup
27int tabserpos[6] = {90,90,90,90,90,90};  // to store current servo position/angle
28String   rot;                              // current direction of rotation (L or R)
29//   Other variables =====================
30int prog[2][10] = {{2,5,6,3,7,8,4,9,11,0},   {0,1,0,0,1,0,0,1,0,0}};  // little scenario as an example 
31
32//
33// == SETUP   ============================================================
34void setup() {
35//   SERIAL init =========================
36Serial.begin(9600);
37delay(500);
38//   LCD init ============================ 
39Wire.begin();
40Wire.beginTransmission(0x38);   
41if (lcdpres == 1) {lcd.begin(16, 2); lcdw(0,0,0," "); lcdw(1,0,0," READY   TO WORK"); }
42// BLUETOOTH init ====================== 
43bluetooth.begin(9600);
44//   SERVOINIT =================================
45pca.begin();
46pca.setPWMFreq(60);   // Analog servos run at ~60 Hz updates
47delay(500);
48// EEPROM init =======================
49//   EEPROM structure : two first bytes contains the current pointer and the max of records
50//   the a multiple of 6 bytes represending robot positions, one record represents 6   bytes, each for one servo
51readeeprom(0,2); epractmax[0] = eprdata[0]; epractmax[1]   = eprdata[1]; // pos 0,1 of eeprom indexes
52delay(100);
53writeeeprom(2,homepos,6);   // write robot home position
54delay(100);
55servomove(0,0); // move robot to   home position 
56// end EEPROM init
57Serial.println(" READY TO WORK");
58}
59//   == LOOP ==============================================================
60void loop()   {
61delay(100);
62if (bluetooth.available()) { // Si donne Bluetooth reue...
63     bluerec = bluetooth.read();
64    switch(bluerec)
65      { 
66      case   'A': servo = 0;  rot = 'L';  break;
67      case 'B': servo = 0;  rot = 'R';  break;
68       case 'C': servo = 1;  rot = 'L';  break;
69      case 'D': servo = 1;  rot   = 'R';  break;
70      case 'E': servo = 2;  rot = 'L';  break;
71      case 'F':   servo = 2;  rot = 'R';  break;
72      case 'G': servo = 3;  rot = 'L';  break;
73       case 'H': servo = 3;  rot = 'R';  break;
74      case 'I': servo = 4;  rot   = 'L';  break;
75      case 'J': servo = 4;  rot = 'R';  break;
76      case 'K':   servo = 5;  rot = 'L';  break;
77      case 'L': servo = 5;  rot = 'R';  break;
78       case 'M': if (epractmax[0] <= (epractmax[1])) {epractmax[0] = epractmax[0]   + 1; listpos(); } rot = ' '; break;  // increase eeprom index
79      case 'N':   if (epractmax[0] > 0)               {epractmax[0] = epractmax[0] - 1; listpos();   } rot = ' '; break;  // decrease eeprom index       
80      case 'Y': listpos();   rot = ' '; break;                                          // display robot position
81       case 'Z': servomove(0,0); rot = ' '; break;                                     //   move robot to home position 
82      case '1': servomove(1,0); rot = ' '; break;                                      // move robot to indicated position
83      case   '2': servomove(2,0); rot = ' '; break;                                     // move   robot to indicated position
84      case '3': servomove(3,0); rot = ' '; break;                                      // move robot to indicated position
85      case   '4': servomove(4,0); rot = ' '; break;                                     // move   robot to indicated position
86      case '6': scenario() ;rot = ' '; break;                                         //   move robot to indicated position
87      case '7': eeprom_read_test(); rot = '   '; break;                                 // display eeprom content
88      case   '8': servomove(epractmax[0],0); rot = ' '; break;                          // move   robot to home position 
89      case '9': writeprom(epractmax[0]); rot = ' '; break;                             // write robot position to eeprom
90      default:   rot = ' ';    break; 
91      } // end switch(bluerec)
92      while((bluerec   != 'S') && (rot != " "))
93         { 
94         if ((rot == "L") && (tabserpos[servo]   > tabanglim[0][servo]))   { tabserpos[servo] = tabserpos[servo] - 1; } // decrease   servo position and verify servo stroke
95         if ((rot == "R") && (tabserpos[servo]   < tabanglim[1][servo]))   { tabserpos[servo] = tabserpos[servo] + 1; } // increase   servo position and verify servo stroke 
96         servowrite(servo, tabserpos[servo]);   
97         delay(100);  // to allow slow motion
98         bluerec = bluetooth.read();   
99         } // end while(bluerec != 'S')
100} // end if (bluetooth.available()
101}   // end loop
102

// Main programm - Robot controler
// LCD =================================   
#include <LiquidCrystal_PCF8574.h>
#include <Wire.h>
LiquidCrystal_PCF8574   lcd(0x38); // I2C
int lcdpres = 0;                                // to put to   0 if LCD not present
// EEPROM ==============================
#define EEPROM_I2C_ADDRESS   0x50  // I2C
int eprdata[6];                  // data read from eeprom
int   epractmax[2];                // cursor position to/from eeprom
// BLUETOOTH ===========================
#include   "SoftwareSerial.h"
SoftwareSerial bluetooth(2, 3);  // RX, TX
int bluerec   ;                    // bluetooth reception variable
// SERVOS =======================================================
#include   <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pca= Adafruit_PWMServoDriver(0x40);   // I2C
#define nbPCAServo 6
int MIN_IMP [nbPCAServo] ={500, 500, 500, 500,   500, 500};
int MAX_IMP [nbPCAServo] ={2500, 2500, 2500, 2500, 2500, 2500};
int   MIN_ANG [nbPCAServo] ={0, 0, 0, 0, 0, 0} ;
int MAX_ANG [nbPCAServo] ={180,180,   180, 180, 180, 180 };               // servo angle capability
int tabanglim[2][6]   = {{0,43,65,0,10,80}, {180,177,180,140,175,116}};   // Limits the servo stroke.
int   servodelay(15);              // delay for servo to achieve rotation
int servo;                        // current servo number (from 0 to 5)
int homepos[6] =   {93, 124, 115, 118, 83, 80};   // robot home position (will be stored to eeprom   on setup
int tabserpos[6] = {90,90,90,90,90,90};  // to store current servo position/angle
String   rot;                              // current direction of rotation (L or R)
//   Other variables =====================
int prog[2][10] = {{2,5,6,3,7,8,4,9,11,0},   {0,1,0,0,1,0,0,1,0,0}};  // little scenario as an example 

//
// == SETUP   ============================================================
void setup() {
//   SERIAL init =========================
Serial.begin(9600);
delay(500);
//   LCD init ============================ 
Wire.begin();
Wire.beginTransmission(0x38);   
if (lcdpres == 1) {lcd.begin(16, 2); lcdw(0,0,0," "); lcdw(1,0,0," READY   TO WORK"); }
// BLUETOOTH init ====================== 
bluetooth.begin(9600);
//   SERVOINIT =================================
pca.begin();
pca.setPWMFreq(60);   // Analog servos run at ~60 Hz updates
delay(500);
// EEPROM init =======================
//   EEPROM structure : two first bytes contains the current pointer and the max of records
//   the a multiple of 6 bytes represending robot positions, one record represents 6   bytes, each for one servo
readeeprom(0,2); epractmax[0] = eprdata[0]; epractmax[1]   = eprdata[1]; // pos 0,1 of eeprom indexes
delay(100);
writeeeprom(2,homepos,6);   // write robot home position
delay(100);
servomove(0,0); // move robot to   home position 
// end EEPROM init
Serial.println(" READY TO WORK");
}
//   == LOOP ==============================================================
void loop()   {
delay(100);
if (bluetooth.available()) { // Si donne Bluetooth reue...
     bluerec = bluetooth.read();
    switch(bluerec)
      { 
      case   'A': servo = 0;  rot = 'L';  break;
      case 'B': servo = 0;  rot = 'R';  break;
       case 'C': servo = 1;  rot = 'L';  break;
      case 'D': servo = 1;  rot   = 'R';  break;
      case 'E': servo = 2;  rot = 'L';  break;
      case 'F':   servo = 2;  rot = 'R';  break;
      case 'G': servo = 3;  rot = 'L';  break;
       case 'H': servo = 3;  rot = 'R';  break;
      case 'I': servo = 4;  rot   = 'L';  break;
      case 'J': servo = 4;  rot = 'R';  break;
      case 'K':   servo = 5;  rot = 'L';  break;
      case 'L': servo = 5;  rot = 'R';  break;
       case 'M': if (epractmax[0] <= (epractmax[1])) {epractmax[0] = epractmax[0]   + 1; listpos(); } rot = ' '; break;  // increase eeprom index
      case 'N':   if (epractmax[0] > 0)               {epractmax[0] = epractmax[0] - 1; listpos();   } rot = ' '; break;  // decrease eeprom index       
      case 'Y': listpos();   rot = ' '; break;                                          // display robot position
       case 'Z': servomove(0,0); rot = ' '; break;                                     //   move robot to home position 
      case '1': servomove(1,0); rot = ' '; break;                                      // move robot to indicated position
      case   '2': servomove(2,0); rot = ' '; break;                                     // move   robot to indicated position
      case '3': servomove(3,0); rot = ' '; break;                                      // move robot to indicated position
      case   '4': servomove(4,0); rot = ' '; break;                                     // move   robot to indicated position
      case '6': scenario() ;rot = ' '; break;                                         //   move robot to indicated position
      case '7': eeprom_read_test(); rot = '   '; break;                                 // display eeprom content
      case   '8': servomove(epractmax[0],0); rot = ' '; break;                          // move   robot to home position 
      case '9': writeprom(epractmax[0]); rot = ' '; break;                             // write robot position to eeprom
      default:   rot = ' ';    break; 
      } // end switch(bluerec)
      while((bluerec   != 'S') && (rot != " "))
         { 
         if ((rot == "L") && (tabserpos[servo]   > tabanglim[0][servo]))   { tabserpos[servo] = tabserpos[servo] - 1; } // decrease   servo position and verify servo stroke
         if ((rot == "R") && (tabserpos[servo]   < tabanglim[1][servo]))   { tabserpos[servo] = tabserpos[servo] + 1; } // increase   servo position and verify servo stroke 
         servowrite(servo, tabserpos[servo]);   
         delay(100);  // to allow slow motion
         bluerec = bluetooth.read();   
         } // end while(bluerec != 'S')
} // end if (bluetooth.available()
}   // end loop

SERVO-functions

arduino

1// =================================== move robot to position
2void   servomove(int li,int sk){ // move the robot in the target position. 
3// Setting   of the function ====================
4//     li : number of stored sequence of   robot (example: sequence = 3 will point to address li*6+2 of eeprom)
5//     sk   : if sk = 0 the arm rise before the programmed move
6// End setting of the function   ================  
7delay(200);  // to be sure last move finished
8int bb =   75,cc = 110;
9int ind0=1, ind1=1, ind2, ind3, ind4, ind5 ;
10if (sk == 1) {ind1   = 0; ind2 = 0;}
11while (ind1+ind2 >0)
12  {
13  if (tabserpos[1] != bb) { ind1   = 1;
14    if (tabserpos[1] < bb) { tabserpos[1] = tabserpos[1]+1; } if (tabserpos[1]   > bb) { tabserpos[1] = tabserpos[1]-1;  } servowrite(1, tabserpos[1]); }
15    else   { ind1 = 0 ;} 
16  if (tabserpos[2] != cc) { ind2 = 1; 
17    if (tabserpos[2]   < cc) { tabserpos[2] = tabserpos[2]+1; } if (tabserpos[2] > cc) { tabserpos[2] =   tabserpos[2]-1;  } servowrite(2, tabserpos[2]); }
18    else { ind2 = 0 ;} 
19   } // end while (ind1+ind2 >0)
20
21if (li > epractmax[1]) { li = epractmax[1]   ; } 
22readeeprom((li*6 + 2),6) ; // in return the eprdata table contains the target   positions of the servos
23ind0 = 1;
24while (ind0+ind3+ind4+ind5 >0)
25  {
26   if (tabserpos[0] != eprdata[0]) { ind0 = 1; 
27    if (tabserpos[0] < eprdata[0])   { tabserpos[0] = tabserpos[0]+1;  } if (tabserpos[0] > eprdata[0]) { tabserpos[0]   = tabserpos[0]-1; } servowrite(0, tabserpos[0]) ; } 
28    else { ind0 = 0 ;}
29   if (tabserpos[3] != eprdata[3]) { ind3 = 1; 
30    if (tabserpos[3] > eprdata[3])   { tabserpos[3] = tabserpos[3]-1;  } if (tabserpos[3] < eprdata[3]) { tabserpos[3]   = tabserpos[3]+1; } servowrite(3, tabserpos[3]) ; } 
31    else { ind3 = 0 ;} 
32   if (tabserpos[4] != eprdata[4]) { ind4 = 1;  
33    if (tabserpos[4] < eprdata[4])   { tabserpos[4] = tabserpos[4]+1;  } if (tabserpos[4] > eprdata[4]) { tabserpos[4]   = tabserpos[4]-1; } servowrite(4, tabserpos[4]) ; } 
34    else { ind4 = 0 ;}
35   }
36ind1 = 1;
37while (ind1+ind2 >0)
38  {
39  if (tabserpos[1] != eprdata[1])   { ind1 = 1; 
40    if (tabserpos[1] < eprdata[1]) { tabserpos[1] = tabserpos[1]+1;   } if (tabserpos[1] > eprdata[1]) { tabserpos[1] = tabserpos[1]-1; } servowrite(1,   tabserpos[1]) ; } 
41    else { ind1 = 0 ;} 
42  if (tabserpos[2] != eprdata[2])   { ind2 = 1; 
43    if (tabserpos[2] < eprdata[2]) { tabserpos[2] = tabserpos[2]+1;   } if (tabserpos[2] > eprdata[2]) { tabserpos[2] = tabserpos[2]-1; } servowrite(2,   tabserpos[2]) ; } 
44    else { ind2 = 0 ;} 
45  }
46ind5 = 1;
47while (ind5   >0)
48  {
49  if (tabserpos[5] != eprdata[5]) { ind5 = 1; 
50    if (tabserpos[5]   < eprdata[5]) { tabserpos[5] = tabserpos[5]+1;  } if (tabserpos[5] > eprdata[5])   { tabserpos[5] = tabserpos[5]-1; } servowrite(5, tabserpos[5]) ; } 
51    else   { ind5 = 0 ;} 
52  }
53listpos();     
54}
55// ==================================
56void   servowrite(int sernum, int angl)  // move servo to new position
57{
58 int imp   = map(angl, MIN_ANG[sernum], MAX_ANG[sernum], MIN_IMP[sernum], MAX_IMP[sernum]);   // convert angle to impulse
59 pca.writeMicroseconds(sernum,imp); 
60 delay(servodelay);
61   tabserpos[sernum] = angl;
62}
63// =================================== list the   position of the servos
64void listpos(){
65 
66String  aff = String(tabserpos[0])   + " " + String(tabserpos[1]) + " " + String(tabserpos[2]) + " " + String(tabserpos[3]);
67String   aff1 = String(tabserpos[4]) + " " + String(tabserpos[5]) + " > " + String (epractmax[0])   + "-" + String (epractmax[1]);
68if (lcdpres == 1) {lcdw(2,0,0,""); lcdw(1,0,0,aff);   lcdw(1,1,0,aff1); }
69Serial.println(aff + " " + aff1);
70}
71void scenario   ()
72{
73  for (int i=0;i<10;i++) {servomove(prog[0][i],prog[1][i]); }
74}
75

// =================================== move robot to position
void   servomove(int li,int sk){ // move the robot in the target position. 
// Setting   of the function ====================
//     li : number of stored sequence of   robot (example: sequence = 3 will point to address li*6+2 of eeprom)
//     sk   : if sk = 0 the arm rise before the programmed move
// End setting of the function   ================  
delay(200);  // to be sure last move finished
int bb =   75,cc = 110;
int ind0=1, ind1=1, ind2, ind3, ind4, ind5 ;
if (sk == 1) {ind1   = 0; ind2 = 0;}
while (ind1+ind2 >0)
  {
  if (tabserpos[1] != bb) { ind1   = 1;
    if (tabserpos[1] < bb) { tabserpos[1] = tabserpos[1]+1; } if (tabserpos[1]   > bb) { tabserpos[1] = tabserpos[1]-1;  } servowrite(1, tabserpos[1]); }
    else   { ind1 = 0 ;} 
  if (tabserpos[2] != cc) { ind2 = 1; 
    if (tabserpos[2]   < cc) { tabserpos[2] = tabserpos[2]+1; } if (tabserpos[2] > cc) { tabserpos[2] =   tabserpos[2]-1;  } servowrite(2, tabserpos[2]); }
    else { ind2 = 0 ;} 
   } // end while (ind1+ind2 >0)

if (li > epractmax[1]) { li = epractmax[1]   ; } 
readeeprom((li*6 + 2),6) ; // in return the eprdata table contains the target   positions of the servos
ind0 = 1;
while (ind0+ind3+ind4+ind5 >0)
  {
   if (tabserpos[0] != eprdata[0]) { ind0 = 1; 
    if (tabserpos[0] < eprdata[0])   { tabserpos[0] = tabserpos[0]+1;  } if (tabserpos[0] > eprdata[0]) { tabserpos[0]   = tabserpos[0]-1; } servowrite(0, tabserpos[0]) ; } 
    else { ind0 = 0 ;}
   if (tabserpos[3] != eprdata[3]) { ind3 = 1; 
    if (tabserpos[3] > eprdata[3])   { tabserpos[3] = tabserpos[3]-1;  } if (tabserpos[3] < eprdata[3]) { tabserpos[3]   = tabserpos[3]+1; } servowrite(3, tabserpos[3]) ; } 
    else { ind3 = 0 ;} 
   if (tabserpos[4] != eprdata[4]) { ind4 = 1;  
    if (tabserpos[4] < eprdata[4])   { tabserpos[4] = tabserpos[4]+1;  } if (tabserpos[4] > eprdata[4]) { tabserpos[4]   = tabserpos[4]-1; } servowrite(4, tabserpos[4]) ; } 
    else { ind4 = 0 ;}
   }
ind1 = 1;
while (ind1+ind2 >0)
  {
  if (tabserpos[1] != eprdata[1])   { ind1 = 1; 
    if (tabserpos[1] < eprdata[1]) { tabserpos[1] = tabserpos[1]+1;   } if (tabserpos[1] > eprdata[1]) { tabserpos[1] = tabserpos[1]-1; } servowrite(1,   tabserpos[1]) ; } 
    else { ind1 = 0 ;} 
  if (tabserpos[2] != eprdata[2])   { ind2 = 1; 
    if (tabserpos[2] < eprdata[2]) { tabserpos[2] = tabserpos[2]+1;   } if (tabserpos[2] > eprdata[2]) { tabserpos[2] = tabserpos[2]-1; } servowrite(2,   tabserpos[2]) ; } 
    else { ind2 = 0 ;} 
  }
ind5 = 1;
while (ind5   >0)
  {
  if (tabserpos[5] != eprdata[5]) { ind5 = 1; 
    if (tabserpos[5]   < eprdata[5]) { tabserpos[5] = tabserpos[5]+1;  } if (tabserpos[5] > eprdata[5])   { tabserpos[5] = tabserpos[5]-1; } servowrite(5, tabserpos[5]) ; } 
    else   { ind5 = 0 ;} 
  }
listpos();     
}
// ==================================
void   servowrite(int sernum, int angl)  // move servo to new position
{
 int imp   = map(angl, MIN_ANG[sernum], MAX_ANG[sernum], MIN_IMP[sernum], MAX_IMP[sernum]);   // convert angle to impulse
 pca.writeMicroseconds(sernum,imp); 
 delay(servodelay);
   tabserpos[sernum] = angl;
}
// =================================== list the   position of the servos
void listpos(){
 
String  aff = String(tabserpos[0])   + " " + String(tabserpos[1]) + " " + String(tabserpos[2]) + " " + String(tabserpos[3]);
String   aff1 = String(tabserpos[4]) + " " + String(tabserpos[5]) + " > " + String (epractmax[0])   + "-" + String (epractmax[1]);
if (lcdpres == 1) {lcdw(2,0,0,""); lcdw(1,0,0,aff);   lcdw(1,1,0,aff1); }
Serial.println(aff + " " + aff1);
}
void scenario   ()
{
  for (int i=0;i<10;i++) {servomove(prog[0][i],prog[1][i]); }
}

SERVO-functions

arduino

1// =================================== move robot to position
2void  servomove(int li,int sk){ // move the robot in the target position. 
3// Setting  of the function ====================
4//     li : number of stored sequence of  robot (example: sequence = 3 will point to address li*6+2 of eeprom)
5//     sk  : if sk = 0 the arm rise before the programmed move
6// End setting of the function  ================  
7delay(200);  // to be sure last move finished
8int bb =  75,cc = 110;
9int ind0=1, ind1=1, ind2, ind3, ind4, ind5 ;
10if (sk == 1) {ind1  = 0; ind2 = 0;}
11while (ind1+ind2 >0)
12  {
13  if (tabserpos[1] != bb) { ind1  = 1;
14    if (tabserpos[1] < bb) { tabserpos[1] = tabserpos[1]+1; } if (tabserpos[1]  > bb) { tabserpos[1] = tabserpos[1]-1;  } servowrite(1, tabserpos[1]); }
15    else  { ind1 = 0 ;} 
16  if (tabserpos[2] != cc) { ind2 = 1; 
17    if (tabserpos[2]  < cc) { tabserpos[2] = tabserpos[2]+1; } if (tabserpos[2] > cc) { tabserpos[2] =  tabserpos[2]-1;  } servowrite(2, tabserpos[2]); }
18    else { ind2 = 0 ;} 
19  } // end while (ind1+ind2 >0)
20
21if (li > epractmax[1]) { li = epractmax[1]  ; } 
22readeeprom((li*6 + 2),6) ; // in return the eprdata table contains the target  positions of the servos
23ind0 = 1;
24while (ind0+ind3+ind4+ind5 >0)
25  {
26  if (tabserpos[0] != eprdata[0]) { ind0 = 1; 
27    if (tabserpos[0] < eprdata[0])  { tabserpos[0] = tabserpos[0]+1;  } if (tabserpos[0] > eprdata[0]) { tabserpos[0]  = tabserpos[0]-1; } servowrite(0, tabserpos[0]) ; } 
28    else { ind0 = 0 ;}
29  if (tabserpos[3] != eprdata[3]) { ind3 = 1; 
30    if (tabserpos[3] > eprdata[3])  { tabserpos[3] = tabserpos[3]-1;  } if (tabserpos[3] < eprdata[3]) { tabserpos[3]  = tabserpos[3]+1; } servowrite(3, tabserpos[3]) ; } 
31    else { ind3 = 0 ;} 
32  if (tabserpos[4] != eprdata[4]) { ind4 = 1;  
33    if (tabserpos[4] < eprdata[4])  { tabserpos[4] = tabserpos[4]+1;  } if (tabserpos[4] > eprdata[4]) { tabserpos[4]  = tabserpos[4]-1; } servowrite(4, tabserpos[4]) ; } 
34    else { ind4 = 0 ;}
35  }
36ind1 = 1;
37while (ind1+ind2 >0)
38  {
39  if (tabserpos[1] != eprdata[1])  { ind1 = 1; 
40    if (tabserpos[1] < eprdata[1]) { tabserpos[1] = tabserpos[1]+1;  } if (tabserpos[1] > eprdata[1]) { tabserpos[1] = tabserpos[1]-1; } servowrite(1,  tabserpos[1]) ; } 
41    else { ind1 = 0 ;} 
42  if (tabserpos[2] != eprdata[2])  { ind2 = 1; 
43    if (tabserpos[2] < eprdata[2]) { tabserpos[2] = tabserpos[2]+1;  } if (tabserpos[2] > eprdata[2]) { tabserpos[2] = tabserpos[2]-1; } servowrite(2,  tabserpos[2]) ; } 
44    else { ind2 = 0 ;} 
45  }
46ind5 = 1;
47while (ind5  >0)
48  {
49  if (tabserpos[5] != eprdata[5]) { ind5 = 1; 
50    if (tabserpos[5]  < eprdata[5]) { tabserpos[5] = tabserpos[5]+1;  } if (tabserpos[5] > eprdata[5])  { tabserpos[5] = tabserpos[5]-1; } servowrite(5, tabserpos[5]) ; } 
51    else  { ind5 = 0 ;} 
52  }
53listpos();     
54}
55// ==================================
56void  servowrite(int sernum, int angl)  // move servo to new position
57{
58 int imp  = map(angl, MIN_ANG[sernum], MAX_ANG[sernum], MIN_IMP[sernum], MAX_IMP[sernum]);  // convert angle to impulse
59 pca.writeMicroseconds(sernum,imp); 
60 delay(servodelay);
61  tabserpos[sernum] = angl;
62}
63// =================================== list the  position of the servos
64void listpos(){
65 
66String  aff = String(tabserpos[0])  + " " + String(tabserpos[1]) + " " + String(tabserpos[2]) + " " + String(tabserpos[3]);
67String  aff1 = String(tabserpos[4]) + " " + String(tabserpos[5]) + " > " + String (epractmax[0])  + "-" + String (epractmax[1]);
68if (lcdpres == 1) {lcdw(2,0,0,""); lcdw(1,0,0,aff);  lcdw(1,1,0,aff1); }
69Serial.println(aff + " " + aff1);
70}
71void scenario  ()
72{
73  for (int i=0;i<10;i++) {servomove(prog[0][i],prog[1][i]); }
74}
75

// =================================== move robot to position
void  servomove(int li,int sk){ // move the robot in the target position. 
// Setting  of the function ====================
//     li : number of stored sequence of  robot (example: sequence = 3 will point to address li*6+2 of eeprom)
//     sk  : if sk = 0 the arm rise before the programmed move
// End setting of the function  ================  
delay(200);  // to be sure last move finished
int bb =  75,cc = 110;
int ind0=1, ind1=1, ind2, ind3, ind4, ind5 ;
if (sk == 1) {ind1  = 0; ind2 = 0;}
while (ind1+ind2 >0)
  {
  if (tabserpos[1] != bb) { ind1  = 1;
    if (tabserpos[1] < bb) { tabserpos[1] = tabserpos[1]+1; } if (tabserpos[1]  > bb) { tabserpos[1] = tabserpos[1]-1;  } servowrite(1, tabserpos[1]); }
    else  { ind1 = 0 ;} 
  if (tabserpos[2] != cc) { ind2 = 1; 
    if (tabserpos[2]  < cc) { tabserpos[2] = tabserpos[2]+1; } if (tabserpos[2] > cc) { tabserpos[2] =  tabserpos[2]-1;  } servowrite(2, tabserpos[2]); }
    else { ind2 = 0 ;} 
  } // end while (ind1+ind2 >0)

if (li > epractmax[1]) { li = epractmax[1]  ; } 
readeeprom((li*6 + 2),6) ; // in return the eprdata table contains the target  positions of the servos
ind0 = 1;
while (ind0+ind3+ind4+ind5 >0)
  {
  if (tabserpos[0] != eprdata[0]) { ind0 = 1; 
    if (tabserpos[0] < eprdata[0])  { tabserpos[0] = tabserpos[0]+1;  } if (tabserpos[0] > eprdata[0]) { tabserpos[0]  = tabserpos[0]-1; } servowrite(0, tabserpos[0]) ; } 
    else { ind0 = 0 ;}
  if (tabserpos[3] != eprdata[3]) { ind3 = 1; 
    if (tabserpos[3] > eprdata[3])  { tabserpos[3] = tabserpos[3]-1;  } if (tabserpos[3] < eprdata[3]) { tabserpos[3]  = tabserpos[3]+1; } servowrite(3, tabserpos[3]) ; } 
    else { ind3 = 0 ;} 
  if (tabserpos[4] != eprdata[4]) { ind4 = 1;  
    if (tabserpos[4] < eprdata[4])  { tabserpos[4] = tabserpos[4]+1;  } if (tabserpos[4] > eprdata[4]) { tabserpos[4]  = tabserpos[4]-1; } servowrite(4, tabserpos[4]) ; } 
    else { ind4 = 0 ;}
  }
ind1 = 1;
while (ind1+ind2 >0)
  {
  if (tabserpos[1] != eprdata[1])  { ind1 = 1; 
    if (tabserpos[1] < eprdata[1]) { tabserpos[1] = tabserpos[1]+1;  } if (tabserpos[1] > eprdata[1]) { tabserpos[1] = tabserpos[1]-1; } servowrite(1,  tabserpos[1]) ; } 
    else { ind1 = 0 ;} 
  if (tabserpos[2] != eprdata[2])  { ind2 = 1; 
    if (tabserpos[2] < eprdata[2]) { tabserpos[2] = tabserpos[2]+1;  } if (tabserpos[2] > eprdata[2]) { tabserpos[2] = tabserpos[2]-1; } servowrite(2,  tabserpos[2]) ; } 
    else { ind2 = 0 ;} 
  }
ind5 = 1;
while (ind5  >0)
  {
  if (tabserpos[5] != eprdata[5]) { ind5 = 1; 
    if (tabserpos[5]  < eprdata[5]) { tabserpos[5] = tabserpos[5]+1;  } if (tabserpos[5] > eprdata[5])  { tabserpos[5] = tabserpos[5]-1; } servowrite(5, tabserpos[5]) ; } 
    else  { ind5 = 0 ;} 
  }
listpos();     
}
// ==================================
void  servowrite(int sernum, int angl)  // move servo to new position
{
 int imp  = map(angl, MIN_ANG[sernum], MAX_ANG[sernum], MIN_IMP[sernum], MAX_IMP[sernum]);  // convert angle to impulse
 pca.writeMicroseconds(sernum,imp); 
 delay(servodelay);
  tabserpos[sernum] = angl;
}
// =================================== list the  position of the servos
void listpos(){
 
String  aff = String(tabserpos[0])  + " " + String(tabserpos[1]) + " " + String(tabserpos[2]) + " " + String(tabserpos[3]);
String  aff1 = String(tabserpos[4]) + " " + String(tabserpos[5]) + " > " + String (epractmax[0])  + "-" + String (epractmax[1]);
if (lcdpres == 1) {lcdw(2,0,0,""); lcdw(1,0,0,aff);  lcdw(1,1,0,aff1); }
Serial.println(aff + " " + aff1);
}
void scenario  ()
{
  for (int i=0;i<10;i++) {servomove(prog[0][i],prog[1][i]); }
}

LCD-EEPROM-functions

arduino

1// function for LCD control ============================================
2void  lcdw (int lcdf, int lcdl, int lcdc, String lcdt) { 
3
4// Setting of the function  ====================
5//     lcdf : 0=init LCD for void setup/1=write txt/2=clear  display,
6//     lcdl : for lcdf=1 display line number 0-1,
7//     lcdc : for  lcdf=1 display column number 0-15,
8//     lcdt : for lcdf=1 string to diplay,
9//  End setting of the function ================
10
11// Function start =============================
12//  const static String lcds1 ="                ";
13  switch (lcdf) {
14    case  0: 
15      lcd.setBacklight(255);
16      lcd.home();
17      lcd.clear();      
18      delay(100);                      
19    break;
20    case 1: 
21      lcd.setBacklight(255);         
22      lcd.setCursor(lcdc, lcdl);                                 //  set start for write
23      lcd.print(lcdt);                                           //  write info
24    break;
25    case 2: 
26        lcd.clear();                                   //  clear LCD
27    break;    
28    }  
29/*  Other LCD functions which can be included  in this function to improve it
30    lcd.clear(); lcd.print("Cursor Blink");  lcd.noBlink(); lcd.blink(); lcd.noCursor(); lcd.noDisplay(); lcd.display(); lcd.setCursor(0-15,  0-1); 
31    lcd.scrollDisplayLeft(); lcd.scrollDisplayRight(); lcd.setBacklight(0-255);  */
32}
33// =================================== write robot postion to eeprom
34void  writeprom(int writeprom1){
35  int writeprom3[6];
36  writeprom3[0] = tabserpos[0];  writeprom3[1] = tabserpos[1]; writeprom3[2] = tabserpos[2]; 
37  writeprom3[3]  = tabserpos[3]; writeprom3[4] = tabserpos[4]; writeprom3[5] = tabserpos[5]; 
38  writeeeprom((writeprom1*6 + 2),writeprom3,6) ; // writes robot position to eeprom
39  if (epractmax[0] > epractmax[1]) { epractmax[1] = epractmax[0]; }
40  writeeeprom(0,epractmax,2);  // writes eeprom pointers to eeprom
41  if (lcdpres == 1) {
42    lcdw(2,0,0,"");  lcdw(1,0,0,"Write OK");   lcdw(1,1,0,String (epractmax[0]) + "-" + String (epractmax[1]));
43    }
44  Serial.println("Write OK  ");
45  listpos();
46}
47// ===================================  eeprom write function
48void writeeeprom(int data_addr, int stotab[], int len)
49{
50  Wire.beginTransmission(EEPROM_I2C_ADDRESS);
51  Wire.write((int)(data_addr >>8));
52  Wire.write((int)(data_addr & 0xFF));
53  for (int i=0;i!=len; i++) {  Wire.write(stotab[i]);}  // writes "len number" memory cells 
54  Wire.endTransmission();
55  delay(5);
56}
57//  =================================== eeprom read function
58byte readeeprom(int  data_addr, int len)
59{
60  int i=0;
61  Wire.beginTransmission(EEPROM_I2C_ADDRESS);
62  Wire.write((int)(data_addr >>8));
63  Wire.write((int)(data_addr & 0xFF));
64  Wire.endTransmission();
65  Wire.requestFrom(EEPROM_I2C_ADDRESS, len); // request  to return "len" memory cells 
66  delay(10);
67  while (Wire.available()){ eprdata[i]  = Wire.read(); i=i+1; }
68}
69// =================================== eeprom read  and disply on computer
70void eeprom_read_test() 
71{
72  String str;
73 int  j, k=0 ,l=0;
74 for(int i=2;i<100;i++)
75    {
76    j = (i-2)/6 ;
77    if  (k != j) { k = j; l = 0;}
78    readeeprom(i,1);  delay(10) ; l = l+1;
79    str  = "Position - Servo : " + String((i-2)/6) + " - " + String(l) + " value : "  + String(eprdata[0]); Serial.println(str) ;
80    }
81    Serial.println("End  list");
82}
83// =================================== Init 30 firts eeprom cells  to 0
84void reinitI2CByte() 
85{
86  int data_addr = 0;
87  Wire.beginTransmission(EEPROM_I2C_ADDRESS);
88  Wire.write((int)(data_addr >>8));
89  Wire.write((int)(data_addr & 0xFF));
90  for (int i=0;i < 30; i++) {  Wire.write(0);} // writes 0 to cells
91  Wire.endTransmission();
92  delay(5);
93}
94

// function for LCD control ============================================
void  lcdw (int lcdf, int lcdl, int lcdc, String lcdt) { 

// Setting of the function  ====================
//     lcdf : 0=init LCD for void setup/1=write txt/2=clear  display,
//     lcdl : for lcdf=1 display line number 0-1,
//     lcdc : for  lcdf=1 display column number 0-15,
//     lcdt : for lcdf=1 string to diplay,
//  End setting of the function ================

// Function start =============================
//  const static String lcds1 ="                ";
  switch (lcdf) {
    case  0: 
      lcd.setBacklight(255);
      lcd.home();
      lcd.clear();      
      delay(100);                      
    break;
    case 1: 
      lcd.setBacklight(255);         
      lcd.setCursor(lcdc, lcdl);                                 //  set start for write
      lcd.print(lcdt);                                           //  write info
    break;
    case 2: 
        lcd.clear();                                   //  clear LCD
    break;    
    }  
/*  Other LCD functions which can be included  in this function to improve it
    lcd.clear(); lcd.print("Cursor Blink");  lcd.noBlink(); lcd.blink(); lcd.noCursor(); lcd.noDisplay(); lcd.display(); lcd.setCursor(0-15,  0-1); 
    lcd.scrollDisplayLeft(); lcd.scrollDisplayRight(); lcd.setBacklight(0-255);  */
}
// =================================== write robot postion to eeprom
void  writeprom(int writeprom1){
  int writeprom3[6];
  writeprom3[0] = tabserpos[0];  writeprom3[1] = tabserpos[1]; writeprom3[2] = tabserpos[2]; 
  writeprom3[3]  = tabserpos[3]; writeprom3[4] = tabserpos[4]; writeprom3[5] = tabserpos[5]; 
  writeeeprom((writeprom1*6 + 2),writeprom3,6) ; // writes robot position to eeprom
  if (epractmax[0] > epractmax[1]) { epractmax[1] = epractmax[0]; }
  writeeeprom(0,epractmax,2);  // writes eeprom pointers to eeprom
  if (lcdpres == 1) {
    lcdw(2,0,0,"");  lcdw(1,0,0,"Write OK");   lcdw(1,1,0,String (epractmax[0]) + "-" + String (epractmax[1]));
    }
  Serial.println("Write OK  ");
  listpos();
}
// ===================================  eeprom write function
void writeeeprom(int data_addr, int stotab[], int len)
{
  Wire.beginTransmission(EEPROM_I2C_ADDRESS);
  Wire.write((int)(data_addr >>8));
  Wire.write((int)(data_addr & 0xFF));
  for (int i=0;i!=len; i++) {  Wire.write(stotab[i]);}  // writes "len number" memory cells 
  Wire.endTransmission();
  delay(5);
}
//  =================================== eeprom read function
byte readeeprom(int  data_addr, int len)
{
  int i=0;
  Wire.beginTransmission(EEPROM_I2C_ADDRESS);
  Wire.write((int)(data_addr >>8));
  Wire.write((int)(data_addr & 0xFF));
  Wire.endTransmission();
  Wire.requestFrom(EEPROM_I2C_ADDRESS, len); // request  to return "len" memory cells 
  delay(10);
  while (Wire.available()){ eprdata[i]  = Wire.read(); i=i+1; }
}
// =================================== eeprom read  and disply on computer
void eeprom_read_test() 
{
  String str;
 int  j, k=0 ,l=0;
 for(int i=2;i<100;i++)
    {
    j = (i-2)/6 ;
    if  (k != j) { k = j; l = 0;}
    readeeprom(i,1);  delay(10) ; l = l+1;
    str  = "Position - Servo : " + String((i-2)/6) + " - " + String(l) + " value : "  + String(eprdata[0]); Serial.println(str) ;
    }
    Serial.println("End  list");
}
// =================================== Init 30 firts eeprom cells  to 0
void reinitI2CByte() 
{
  int data_addr = 0;
  Wire.beginTransmission(EEPROM_I2C_ADDRESS);
  Wire.write((int)(data_addr >>8));
  Wire.write((int)(data_addr & 0xFF));
  for (int i=0;i < 30; i++) {  Wire.write(0);} // writes 0 to cells
  Wire.endTransmission();
  delay(5);
}

Robot-bluetooth-prog-pca

arduino

1// Main programm - Robot controler
2// LCD =================================  
3#include <LiquidCrystal_PCF8574.h>
4#include <Wire.h>
5LiquidCrystal_PCF8574  lcd(0x38); // I2C
6int lcdpres = 0;                                // to put to  0 if LCD not present
7// EEPROM ==============================
8#define EEPROM_I2C_ADDRESS  0x50  // I2C
9int eprdata[6];                  // data read from eeprom
10int  epractmax[2];                // cursor position to/from eeprom
11// BLUETOOTH ===========================
12#include  "SoftwareSerial.h"
13SoftwareSerial bluetooth(2, 3);  // RX, TX
14int bluerec  ;                    // bluetooth reception variable
15// SERVOS =======================================================
16#include  <Adafruit_PWMServoDriver.h>
17Adafruit_PWMServoDriver pca= Adafruit_PWMServoDriver(0x40);  // I2C
18#define nbPCAServo 6
19int MIN_IMP [nbPCAServo] ={500, 500, 500, 500,  500, 500};
20int MAX_IMP [nbPCAServo] ={2500, 2500, 2500, 2500, 2500, 2500};
21int  MIN_ANG [nbPCAServo] ={0, 0, 0, 0, 0, 0} ;
22int MAX_ANG [nbPCAServo] ={180,180,  180, 180, 180, 180 };               // servo angle capability
23int tabanglim[2][6]  = {{0,43,65,0,10,80}, {180,177,180,140,175,116}};   // Limits the servo stroke.
24int  servodelay(15);              // delay for servo to achieve rotation
25int servo;                       // current servo number (from 0 to 5)
26int homepos[6] =  {93, 124, 115, 118, 83, 80};   // robot home position (will be stored to eeprom  on setup
27int tabserpos[6] = {90,90,90,90,90,90};  // to store current servo position/angle
28String  rot;                              // current direction of rotation (L or R)
29//  Other variables =====================
30int prog[2][10] = {{2,5,6,3,7,8,4,9,11,0},  {0,1,0,0,1,0,0,1,0,0}};  // little scenario as an example 
31
32//
33// == SETUP  ============================================================
34void setup() {
35//  SERIAL init =========================
36Serial.begin(9600);
37delay(500);
38//  LCD init ============================ 
39Wire.begin();
40Wire.beginTransmission(0x38);  
41if (lcdpres == 1) {lcd.begin(16, 2); lcdw(0,0,0," "); lcdw(1,0,0," READY  TO WORK"); }
42// BLUETOOTH init ====================== 
43bluetooth.begin(9600);
44//  SERVOINIT =================================
45pca.begin();
46pca.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
47delay(500);
48// EEPROM init =======================
49//  EEPROM structure : two first bytes contains the current pointer and the max of records
50//  the a multiple of 6 bytes represending robot positions, one record represents 6  bytes, each for one servo
51readeeprom(0,2); epractmax[0] = eprdata[0]; epractmax[1]  = eprdata[1]; // pos 0,1 of eeprom indexes
52delay(100);
53writeeeprom(2,homepos,6);  // write robot home position
54delay(100);
55servomove(0,0); // move robot to  home position 
56// end EEPROM init
57Serial.println(" READY TO WORK");
58}
59//  == LOOP ==============================================================
60void loop()  {
61delay(100);
62if (bluetooth.available()) { // Si donne Bluetooth reue...
63    bluerec = bluetooth.read();
64    switch(bluerec)
65      { 
66      case  'A': servo = 0;  rot = 'L';  break;
67      case 'B': servo = 0;  rot = 'R';  break;
68      case 'C': servo = 1;  rot = 'L';  break;
69      case 'D': servo = 1;  rot  = 'R';  break;
70      case 'E': servo = 2;  rot = 'L';  break;
71      case 'F':  servo = 2;  rot = 'R';  break;
72      case 'G': servo = 3;  rot = 'L';  break;
73      case 'H': servo = 3;  rot = 'R';  break;
74      case 'I': servo = 4;  rot  = 'L';  break;
75      case 'J': servo = 4;  rot = 'R';  break;
76      case 'K':  servo = 5;  rot = 'L';  break;
77      case 'L': servo = 5;  rot = 'R';  break;
78      case 'M': if (epractmax[0] <= (epractmax[1])) {epractmax[0] = epractmax[0]  + 1; listpos(); } rot = ' '; break;  // increase eeprom index
79      case 'N':  if (epractmax[0] > 0)               {epractmax[0] = epractmax[0] - 1; listpos();  } rot = ' '; break;  // decrease eeprom index       
80      case 'Y': listpos();  rot = ' '; break;                                          // display robot position
81      case 'Z': servomove(0,0); rot = ' '; break;                                     //  move robot to home position 
82      case '1': servomove(1,0); rot = ' '; break;                                     // move robot to indicated position
83      case  '2': servomove(2,0); rot = ' '; break;                                     // move  robot to indicated position
84      case '3': servomove(3,0); rot = ' '; break;                                     // move robot to indicated position
85      case  '4': servomove(4,0); rot = ' '; break;                                     // move  robot to indicated position
86      case '6': scenario() ;rot = ' '; break;                                         //  move robot to indicated position
87      case '7': eeprom_read_test(); rot = '  '; break;                                 // display eeprom content
88      case  '8': servomove(epractmax[0],0); rot = ' '; break;                          // move  robot to home position 
89      case '9': writeprom(epractmax[0]); rot = ' '; break;                            // write robot position to eeprom
90      default:  rot = ' ';    break; 
91      } // end switch(bluerec)
92      while((bluerec  != 'S') && (rot != " "))
93         { 
94         if ((rot == "L") && (tabserpos[servo]  > tabanglim[0][servo]))   { tabserpos[servo] = tabserpos[servo] - 1; } // decrease  servo position and verify servo stroke
95         if ((rot == "R") && (tabserpos[servo]  < tabanglim[1][servo]))   { tabserpos[servo] = tabserpos[servo] + 1; } // increase  servo position and verify servo stroke 
96         servowrite(servo, tabserpos[servo]);  
97         delay(100);  // to allow slow motion
98         bluerec = bluetooth.read();  
99         } // end while(bluerec != 'S')
100} // end if (bluetooth.available()
101}  // end loop
102

// Main programm - Robot controler
// LCD =================================  
#include <LiquidCrystal_PCF8574.h>
#include <Wire.h>
LiquidCrystal_PCF8574  lcd(0x38); // I2C
int lcdpres = 0;                                // to put to  0 if LCD not present
// EEPROM ==============================
#define EEPROM_I2C_ADDRESS  0x50  // I2C
int eprdata[6];                  // data read from eeprom
int  epractmax[2];                // cursor position to/from eeprom
// BLUETOOTH ===========================
#include  "SoftwareSerial.h"
SoftwareSerial bluetooth(2, 3);  // RX, TX
int bluerec  ;                    // bluetooth reception variable
// SERVOS =======================================================
#include  <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pca= Adafruit_PWMServoDriver(0x40);  // I2C
#define nbPCAServo 6
int MIN_IMP [nbPCAServo] ={500, 500, 500, 500,  500, 500};
int MAX_IMP [nbPCAServo] ={2500, 2500, 2500, 2500, 2500, 2500};
int  MIN_ANG [nbPCAServo] ={0, 0, 0, 0, 0, 0} ;
int MAX_ANG [nbPCAServo] ={180,180,  180, 180, 180, 180 };               // servo angle capability
int tabanglim[2][6]  = {{0,43,65,0,10,80}, {180,177,180,140,175,116}};   // Limits the servo stroke.
int  servodelay(15);              // delay for servo to achieve rotation
int servo;                       // current servo number (from 0 to 5)
int homepos[6] =  {93, 124, 115, 118, 83, 80};   // robot home position (will be stored to eeprom  on setup
int tabserpos[6] = {90,90,90,90,90,90};  // to store current servo position/angle
String  rot;                              // current direction of rotation (L or R)
//  Other variables =====================
int prog[2][10] = {{2,5,6,3,7,8,4,9,11,0},  {0,1,0,0,1,0,0,1,0,0}};  // little scenario as an example 

//
// == SETUP  ============================================================
void setup() {
//  SERIAL init =========================
Serial.begin(9600);
delay(500);
//  LCD init ============================ 
Wire.begin();
Wire.beginTransmission(0x38);  
if (lcdpres == 1) {lcd.begin(16, 2); lcdw(0,0,0," "); lcdw(1,0,0," READY  TO WORK"); }
// BLUETOOTH init ====================== 
bluetooth.begin(9600);
//  SERVOINIT =================================
pca.begin();
pca.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
delay(500);
// EEPROM init =======================
//  EEPROM structure : two first bytes contains the current pointer and the max of records
//  the a multiple of 6 bytes represending robot positions, one record represents 6  bytes, each for one servo
readeeprom(0,2); epractmax[0] = eprdata[0]; epractmax[1]  = eprdata[1]; // pos 0,1 of eeprom indexes
delay(100);
writeeeprom(2,homepos,6);  // write robot home position
delay(100);
servomove(0,0); // move robot to  home position 
// end EEPROM init
Serial.println(" READY TO WORK");
}
//  == LOOP ==============================================================
void loop()  {
delay(100);
if (bluetooth.available()) { // Si donne Bluetooth reue...
    bluerec = bluetooth.read();
    switch(bluerec)
      { 
      case  'A': servo = 0;  rot = 'L';  break;
      case 'B': servo = 0;  rot = 'R';  break;
      case 'C': servo = 1;  rot = 'L';  break;
      case 'D': servo = 1;  rot  = 'R';  break;
      case 'E': servo = 2;  rot = 'L';  break;
      case 'F':  servo = 2;  rot = 'R';  break;
      case 'G': servo = 3;  rot = 'L';  break;
      case 'H': servo = 3;  rot = 'R';  break;
      case 'I': servo = 4;  rot  = 'L';  break;
      case 'J': servo = 4;  rot = 'R';  break;
      case 'K':  servo = 5;  rot = 'L';  break;
      case 'L': servo = 5;  rot = 'R';  break;
      case 'M': if (epractmax[0] <= (epractmax[1])) {epractmax[0] = epractmax[0]  + 1; listpos(); } rot = ' '; break;  // increase eeprom index
      case 'N':  if (epractmax[0] > 0)               {epractmax[0] = epractmax[0] - 1; listpos();  } rot = ' '; break;  // decrease eeprom index       
      case 'Y': listpos();  rot = ' '; break;                                          // display robot position
      case 'Z': servomove(0,0); rot = ' '; break;                                     //  move robot to home position 
      case '1': servomove(1,0); rot = ' '; break;                                     // move robot to indicated position
      case  '2': servomove(2,0); rot = ' '; break;                                     // move  robot to indicated position
      case '3': servomove(3,0); rot = ' '; break;                                     // move robot to indicated position
      case  '4': servomove(4,0); rot = ' '; break;                                     // move  robot to indicated position
      case '6': scenario() ;rot = ' '; break;                                         //  move robot to indicated position
      case '7': eeprom_read_test(); rot = '  '; break;                                 // display eeprom content
      case  '8': servomove(epractmax[0],0); rot = ' '; break;                          // move  robot to home position 
      case '9': writeprom(epractmax[0]); rot = ' '; break;                            // write robot position to eeprom
      default:  rot = ' ';    break; 
      } // end switch(bluerec)
      while((bluerec  != 'S') && (rot != " "))
         { 
         if ((rot == "L") && (tabserpos[servo]  > tabanglim[0][servo]))   { tabserpos[servo] = tabserpos[servo] - 1; } // decrease  servo position and verify servo stroke
         if ((rot == "R") && (tabserpos[servo]  < tabanglim[1][servo]))   { tabserpos[servo] = tabserpos[servo] + 1; } // increase  servo position and verify servo stroke 
         servowrite(servo, tabserpos[servo]);  
         delay(100);  // to allow slow motion
         bluerec = bluetooth.read();  
         } // end while(bluerec != 'S')
} // end if (bluetooth.available()
}  // end loop

Downloadable files

Wiring diagram

Overview of

Wiring diagram

Components

Wiring diagram

Overview of

Wiring diagram

Comments

Only logged in users can leave comments

danielgass

0 Followers

•

0 Projects

Intro