Send angles increase

Hello,

I can’t see which is my mistake. Anybody can help me?

With this program I’m trying to make a robotic arm, q1 and q2 are the angle references (final angle position) of the base and the elbow respectively, and qB and qE are the angels sent to the servo. I want qB and qE to increase/decrease from 1 to 1 until its value reaches the respective angle reference.

I can’t see which is my mistake but qB and qE never get more than value 2.

Here is the loop program where I’m trying to do what I’ve explained. I aso attach an image of the serial printer.

Take a look and let me know if you see something that helps me or solves my problem. Thanks in advance!

void loop() {

/*Initialize variables*/
float xts=0;
float yts=0;
q1=0;
q2=0;
int qB=0;
int qE=0;
servoMotor(qB,qE);

/*Principal program*/
principal:
       
        /*~~~~~~~~~~  Obtain Angles ~~~~~~~~~~*/
/*_____________________________________________________
=======       Read Touch Screen coords       ========*/
  xts= readX();
  yts= readY();

       /*--------------------------------------------------- 
         Touchscreen coords origen down left corner
        -------------------------------------------------*/
    
  float x= map(xts,xminread,xmaxread,10,width);      /*mapping TS coords to the paper dimensions*/
  float y= map(yts,ymaxread,yminread,10,height);
  
  WriteTouchScreen(x,y);
  
/*_____________________________________________________
=======       TS coords to Arm coords       =========*/
  float xArm = x-dPaperX;
  float yArm = y+dPaperY;

/*_____________________________________________________
==============       Arms Angles       ==============*/

    /*If there's no input from TS the position the angle keeps its last value*/
  if ((y>230)|(x<0)){
    q1=q1;
    q2=q2;
  }else{
      q2=getAngleQ2(xArm,yArm); 
      q1=getAngleQ1(xArm,yArm); 
  }
  
  PrintAngle(q1,q2,qB,qE);

       /*~~~~~~~~~~~~  Move Motors ~~~~~~~~~~~~*/
servoMotor (qB, qE);


  if (qB>q1){
    goto dec;
  }
  else{
    if (qB<q1){
      goto inc;
    }
  } 

ELBOW:
    if (qE>q2){
    goto decE;
  }
  else{
    if (qE<q2){
      goto incE;
    }
  } 
   
goto principal;


dec:
  qB=qB-1;
  goto ELBOW;


inc:
  qB=qB+1;
  goto ELBOW;


decE:
  qE=qE-1;
  goto principal;


incE:
  qE=qE+1;
  goto principal;
}

Please edit your post to add code tags ("</>") button on editor.

See also the “How to use this forum” post.

In a language with structures available, why are you using goto?

vinceherman:
In a language with structures available, why are you using goto?

Very good.

OP you probably just need two functions ‘Base’ and ‘Elbow’, but you do so little why do you even bother just expand the code.

This;

if (qB>q1)
{
    goto dec;
}
else
{
    if (qB<q1)
    {
      goto inc;
    }
}

could just be written as this;

if (qB>q1)
{
    qB=qB-1;
}
else
{
    if (qB<q1)
    {
      qB=qB+1;
    }
}

Stick more print statements in the code as well to find out why it is not doing what you expect.

I tried before and it still doesn’t work… See below.

// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {

/*Initialize variables*/
float xts=0;
float yts=0;
q1=0;
q2=0;
int qB=0;
int qE=0;
servoMotor(qB,qE);

/*Principal program*/
principal:
       
        /*~~~~~~~~~~  Obtain Angles ~~~~~~~~~~*/
/*_____________________________________________________
=======       Read Touch Screen coords       ========*/
  xts= readX();
  yts= readY();

       /*--------------------------------------------------- 
         Touchscreen coords origen down left corner
        -------------------------------------------------*/
    
  float x= map(xts,xminread,xmaxread,10,width);      /*mapping TS coords to the paper dimensions*/
  float y= map(yts,ymaxread,yminread,10,height);
  
  WriteTouchScreen(x,y);
  
/*_____________________________________________________
=======       TS coords to Arm coords       =========*/
  float xArm = x-dPaperX;
  float yArm = y+dPaperY;

/*_____________________________________________________
==============       Arms Angles       ==============*/

    /*If there's no input from TS the position the angle keeps its last value*/
  if ((y>230)|(x<0)){
    q1=q1;
    q2=q2;
  }else{
      q2=getAngleQ2(xArm,yArm); 
      q1=getAngleQ1(xArm,yArm); 
  }
  
  PrintAngle(q1,q2,qB,qE);

       /*~~~~~~~~~~~~  Move Motors ~~~~~~~~~~~~*/
servoMotor (qB, qE);


  if (qB>q1){
      qB=qB-1;
  }
  else{
    if (qB<q1){
      qB=qB+1;
    }
  } 


    if (qE>q2){
      qE=qE-1;
  }
  else{
    if (qE<q2){
      qE=qE+1;
    }
  } 
   
goto principal;

}

Pardal94:
I tried before and it still doesn't work....... See below.
...

You still have a goto and you don;t have any setup function.

Put all the initialisation that you want to happen only once within a setup function.
Then put onl the code that you want to repeat within the loop.
Then sprinkle liberally with print statements so that you can see exactly where your program is going wrong.

ardly:
You still have a goto and you don;t have any setup function.

Put all the initialisation that you want to happen only once within a setup function.
Then put onl the code that you want to repeat within the loop.
Then sprinkle liberally with print statements so that you can see exactly where your program is going wrong.

I just tried it and still doesn’t work.

I thought that the problem will be in the loop but maybe there’s something out of the loop that I can’t notice. I attach the full program:

// ================================================================
// ===                    LIBRARY DECLARE                       ===
// ================================================================

#include <Servo.h>
#include <math.h> 
#include <SPI.h>
#include <TFT.h>          //Touch screen library



// ================================================================
// ===                    PINS & CONSTANTS                      ===
// ================================================================

/*_____________________________________________________
===========       Size paper (mm)       =============*/

#define width 287
#define height 200                    

/*_____________________________________________________
=========       Configuration Robot       ===========*/

#define L1 130          //Length of the links (mm)
#define L2 150 
#define theta 20        // Angle offset for base motor

#define dPaperY 60      //Distance Joint0 to the paper (y axis)
#define dPaperX 148     //Distance Joint0 to the paper (x axis)


/*_____________________________________________________
=========         Declare variables        ===========*/
float Px=0,Py=0;     //coordinates of the a point
float q1,q2;        //angles of the joints
int qB,qE;
float xts;
float yts;


/*Reset point
--------------------------------------------------------*//* 
#define Ix 160.0
#define Iy 170.0*/      //¿?



/*_____________________________________________________
=============       Touchscreen       ===============*/

#define y1 A0
#define x2 A1
#define y2 A2
#define x1 A3


#define xminread 60
#define xmaxread 950
#define yminread 150
#define ymaxread 900

/*_____________________________________________________
=========       Configuration Servos       ==========*/
Servo base;
Servo elbow;
Servo pen;


// ================================================================
// ===                          SETUP                           ===
// ================================================================

 void setup() {
  Serial.begin(9600);
  base.attach(9);       //Base servo controlled by pin 9
  elbow.attach(10);     //Elbow servo controlled by pin 10
  pen.attach(11);       //Pen servo controlled by pin 11

  Serial.println("Ready!");
  Serial.println(" ");
  
  xts=0;
  yts=0;
  q1=0;
  q2=0;
  qB=0;
  qE=0;
  
  
}


// ================================================================
// ===                   READ TOUCHSCREEN                       ===
// ================================================================

  float readY(){
  float xr=0;
  pinMode(y1,INPUT);
  pinMode(x2,OUTPUT);
  pinMode(y2,INPUT);
  pinMode(x1,OUTPUT);

  digitalWrite(x2,LOW);
  digitalWrite(x1,HIGH);
  delay(5);
  xr = 1023 - analogRead(A0);
  return xr;
  //return analogRead(y1);
  
}

  float readX(){
  float yr=0;
  pinMode(y1,OUTPUT);
  pinMode(x2,INPUT);
  pinMode(y2,OUTPUT);
  pinMode(x1,INPUT);

  digitalWrite(y1,LOW);
  digitalWrite(y2,HIGH);
  delay(5);
  yr = 1023 - analogRead(A1);
  return yr;
  //return analogRead(x2);    
}


// ================================================================
// ===                     GET ANGLES                           ===
// ================================================================
/*_____________________________________________________
================       Angle Q2      =================*/     
float getAngleQ2(float x,float y){  
  float H = sqrt(x*x+y*y);   // hipotenusa
  float q2=acos(((float)L1*(float)L1+(float)L2*(float)L2-H*H)/(2*(float)L1*(float)L2)); 
    
  return q2;
}

/*_____________________________________________________
================       Angle Q1      =================*/
float getAngleQ1(int x,int y){  

  float H = sqrt(x*x+y*y);   // hipotenusa
  float alpha = atan((float)y/(float)x); //Serial.println(beta)
  float beta = acos(((float)L1*(float)L1-(float)L2*(float)L2+H*H)/(2*(float)L1*H));
  float q1 = alpha + beta;
  if (x<0){
  //q1=PI-q1;  REVISAR!
  }

  return q1;
}


// ================================================================
// ===                        MOVE MOTOR                        ===
// ================================================================

void servoMotor ( int qB, int qE){
  qB=map(qB, 0, 200, 200, 20); //Adjust Base angle
  base.write(qB);   // send angle to base
  delay(100);
  elbow.write(qE);  // send angle to elbow
  delay(100);
}


// ================================================================
// ===                  DISPLAY INFORMATION                     ===
// ================================================================

void PrintAngle (float ang1, float ang2, int base, int elbow){
  Serial.print("q1: ");Serial.print (base); Serial.print("º  ");
  Serial.print (ang1*180/PI); Serial.println("º");
  Serial.print("q2: ");Serial.print (elbow); Serial.print("º  ");
  Serial.print (ang2*180/PI); Serial.println("º");
  Serial.println("-----------------------------");
  delay(256);
}

void WriteTouchScreen (float x, float y){
  Serial.print("x: ");Serial.print(x);Serial.print("    ");
  Serial.print("y: ");Serial.println(y);
 // Serial.println("--------------------");
 

}




// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {

/*Principal program*/

     
        /*~~~~~~~~~~  Obtain Angles ~~~~~~~~~~*/
/*_____________________________________________________
=======       Read Touch Screen coords       ========*/
  xts= readX();
  yts= readY();

       /*--------------------------------------------------- 
         Touchscreen coords origen down left corner
        -------------------------------------------------*/
    
  float x= map(xts,xminread,xmaxread,10,width);      /*mapping TS coords to the paper dimensions*/
  float y= map(yts,ymaxread,yminread,10,height);
  
  WriteTouchScreen(x,y);
  
/*_____________________________________________________
=======       TS coords to Arm coords       =========*/
  float xArm = x-dPaperX;
  float yArm = y+dPaperY;

/*_____________________________________________________
==============       Arms Angles       ==============*/

    /*If there's no input from TS the position the angle keeps its last value*/
  if ((y>230)|(x<0)){
    q1=q1;
    q2=q2;
  }else{
      q2=getAngleQ2(xArm,yArm); 
      q1=getAngleQ1(xArm,yArm); 
  }
  
  PrintAngle(q1,q2,qB,qE);

       /*~~~~~~~~~~~~  Move Motors ~~~~~~~~~~~~*/
servoMotor (qB, qE);

  if (qB>q1){
      qB=qB-1;
  }
  else{
    if (qB<q1){
      qB=qB+1;
    }
  } 


    if (qE>q2){
      qE=qE-1;
  }
  else{
    if (qE<q2){
      qE=qE+1;
    }
  } 
   

}

PROBLEM SOLVED!

The q1 and q2 were in rad while the qE and qB were in degrees.

Thanks!