Yeah here is the code, sorry.
I just wanted to see how someone else would write the code. I figured that the problem would be communication just don't know where exactly.
#include<SoftwareSerial.h>
#include<Wire.h>
#include<Adafruit_PWMServoDriver.h> //Knižnice
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver (); //Adresa Servo Driver
SoftwareSerial mySerial(10,11); //Softwarova linka Arduina ,definovanie RX,TX
#define TRUE 1
#define FALSE 0
#define Base_Rot_L_GO 65 //Command na zapocatie rotacie zakladne vlavo
#define Base_Rot_L_STOP 97 //Command ma ukoncenie rotacie zakladne vlavo
#define Base_Rot_R_GO 66
#define Base_Rot_R_STOP 98
#define Base_U_GO 67
#define Base_U_STOP 99
#define Base_D_GO 68
#define Base_D_STOP 100
#define Elbow_U_GO 69
#define Elbow_U_STOP 101
#define Elbow_D_GO 70 //Definovanie znakov úkonov v decimalnom tvare
#define Elbow_D_STOP 102
#define Gripper_U_GO 71
#define Gripper_U_STOP 103
#define Gripper_D_GO 72
#define Gripper_D_STOP 104
#define Gripper_Rot_L_GO 73
#define Gripper_Rot_L_STOP 105
#define Gripper_Rot_R_GO 74
#define Gripper_Rot_R_STOP 106
#define Grip_GO 75
#define Grip_STOP 107
#define Release_GO 76
#define Release_STOP 108
int BaseRAngle=320; //Inicializacne uhly
int BaseEAngle=320;
int ElbowAngle=215;
int GripperEAngle=435;
int GripperRAngle=350;
int GripperOpen=120;
struct Servo{ //Vytvorenie struktury 'Servo'
int Pin;
int Angle;
int MovementUp;
int MovementDown;
int InitAngle;
int BoundMAX; //Atributy struktury
int BoundMIN;
int CommandUpStart;
int CommandUpStop;
int CommandDownStart;
int CommandDownStop;
void pin(){ //Funkcia pre pociatocne nastavenie ramena
pwm.setPWM(this->Pin,0,this->InitAngle); //Nastavi PWM na danom pine na inicializacny uhol serva
delay(1000); //Caka na dokoncenie pohybu
}
void moveUP(){ //Funkcia pre zvacsovanie uhlu
if(this->MovementUp == TRUE){
this->Angle++;
if(this->Angle >= this->BoundMAX){ //Ak nadobudne svoju maximalnu vychylku---zastav
this->Angle = this->BoundMAX;
}
}
}
void moveDOWN(){ //Funkcia pre zmensovanie uhlu
if(this->MovementDown == TRUE){
this->Angle--;
if(this->Angle <= this->BoundMIN){ //Ak nadobudne svoju minimalnu vychylku---zastav
this->Angle = this->BoundMIN;
}
}
}
void react(int Command){ //Rozpoznavac prikazov(commandov)
if(Command == this->CommandUpStart){ //Ak je prijaty prikaz na zvacsenie uhlu
this->MovementUp=TRUE; //registruj pohyb serva
}
else if(Command == this->CommandUpStop){ //Ak je prijaty prikaz na ukoncenie zvacsovania uhlu
this->MovementUp=FALSE; //registruj ukoncenie pohybu serva
}
else if(Command == this->CommandDownStart){
this->MovementDown=TRUE;
}
else if(Command == this->CommandDownStart){
this->MovementDown=FALSE;
}
}
void move(){ //Funkcia samotneho pohybu
if(this->MovementUp=TRUE){ //Ak je registrovane zvacsovanie uhlu
moveUP(); //zvacsuj uhol
}
else if(this->MovementDown=TRUE){
moveDOWN();
}
pwm.setPWM(this-Pin,0,this->Angle); //Vysli PWM signal
}
};
Servo* BaseServo1; //Definovanie instancii struktur pre jednotlive serva
Servo* BaseServo2;
Servo* ElbowServo;
Servo* GripperServo1;
Servo* GripperServo2;
Servo* GripperServo3;
void setup() {
Serial.begin(9600);
mySerial.begin(9600); //Zapnutie seriovych liniek
pwm.begin(); //Zapnutie vysielania PWM signalu
pwm.setPWMFreq(60); //Nastavenie frekvencie PWM signalu (analogove serva okolo 60Hz)
BaseServo1 = new Servo{0,BaseRAngle,FALSE,FALSE,BaseRAngle,600,100,Base_Rot_L_GO,Base_Rot_L_STOP,Base_Rot_R_GO,Base_Rot_R_STOP}; //Specifikovanie premennych jednotlivych instancii pre konkretne serva
BaseServo1->pin();
BaseServo2 = new Servo{1,BaseEAngle,FALSE,FALSE,BaseEAngle,600,100,Base_U_GO,Base_U_STOP,Base_D_GO,Base_D_STOP}; //NAZOV = JEDNA SA O NOVU INSTANCIU STRUKTURY SERVO{PIN,POCIATOCNY UHOL, POCIATOCNY POHYB,,INICIALIZACNY UHOL,MAX,MIN,PRIKAZY}
BaseServo2->pin(); //Vykonaj funkciu pin--pohni sa do svojej inicializacnej polohy
ElbowServo = new Servo{2,ElbowAngle,FALSE,FALSE,ElbowAngle,560,100,Elbow_U_GO,Elbow_U_STOP,Elbow_D_GO,Elbow_D_STOP};
ElbowServo->pin();
GripperServo1 = new Servo{3,GripperEAngle,FALSE,FALSE,GripperEAngle,540,180,Gripper_U_GO,Gripper_U_STOP,Gripper_D_GO,Gripper_D_STOP};
GripperServo1->pin();
GripperServo2 = new Servo{4,GripperRAngle,FALSE,FALSE,GripperRAngle,530,170,Gripper_Rot_L_GO,Gripper_Rot_L_STOP,Gripper_Rot_R_GO,Gripper_Rot_R_STOP};
GripperServo2->pin();
GripperServo3 = new Servo{5,GripperOpen,FALSE,FALSE,GripperOpen,510,120,Grip_GO,Grip_STOP,Release_GO,Release_STOP};
GripperServo3->pin();
Serial.println("Connected");
}
void loop() {
int Command = mySerial.read();
delay(10);
//Serial.println("Connected");
if(mySerial.available()){
Serial.println(mySerial.read());
}
BaseServo1->react(Command); //Ked prijmes prikaz pre zakladove servo, reaguj podla funkcie react
BaseServo1->move(); //pohni servom na zaklade typu prikazu
BaseServo2->react(Command);
BaseServo2->move();
ElbowServo->react(Command);
ElbowServo->move();
GripperServo1->react(Command);
GripperServo1->move();
GripperServo2->react(Command);
GripperServo2->move();
GripperServo3->react(Command);
GripperServo3->move();
}