Maintenance: Project Hub will be unavailable on Wednesday 25 (9AM to 6PM CET) while we deploy critical improvements

Robot Arm Automation

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

Jan 7, 2022

•

42607 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 Rev3

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

Trademarks & CopyrightsWhistleblowingDigital Services ActTerms of ServicePrivacy PolicySecurityCookie Settings

Report content