Help with certain parts of the meArm Code (for Joystick Control)

Projects Programming
1

Good Morning:

I'm giving a little robotics workshop to a group of teenagers in my city in Texas and I'm guiding them through the process of building the meArm. I've assembled it and programmed it 'manually' before to perform certain movements, but now we're going to use Joysticks to control it. We bought the joysticks that come in separately and hook them up to our proto-board. We're using the code below but I'm having problems interpreting certain lines of code, particularly the move_joy() function, perhaps of my lack of experience with Joystick programming. Anyways, I've tried for the last week to try to interpret the code the best I can, doing online research and reading an arduino book that I have, but this function keeps giving me trouble. I would like your help to verify that what I'm putting on the comments lines is accurate and also to try and figure out the lines in which I don't have comments. Your help will be greatly appreciated and promote the proper dissemination or Arduino Knowledge, thanks

Here's the full code...

#include <Servo.h> 
#include <SoftwareSerial.h>

SoftwareSerial mySerial(12, 13); // RX, TX
const int SERVOS = 4;
int PIN[SERVOS], value[SERVOS], currentAngle[SERVOS], MIN[SERVOS], MAX[SERVOS], INITANGLE[SERVOS];
Servo myservo[SERVOS];
int afrom[] = {90, 35, 135}, ato[] = {20, 140, 110}, amiddle[] = {20, 35, 135}, afinal[] = {179,75,95}, aafterfinal[] = {179,35,135};
int DELAYTIME = 200;
int servo_moving[SERVOS] = {0,0,0,0};
boolean stringComplete = false;
int bt_servo = 0;
int bt_move = 0;
int idle = 0;


void setup() {
  Serial.begin(9600);
  pinMode(3, OUTPUT);  
  digitalWrite(3, HIGH); 
  mySerial.begin(9600);
  
  init_Pins();
  
  //auto_mode();
}

void loop() {
  move_bt();
  move_joy();  
}

void init_Pins(){
  PIN[0] = 11;
  MIN[0] = 0;
  MAX[0] = 179;
  INITANGLE[0] = 90;
  PIN[1] = 10;
  MIN[1] = 35;
  MAX[1] = 179;
  INITANGLE[1] = 90;
  PIN[2] = 9;
  MIN[2] = 90;
  MAX[2] = 179;
  INITANGLE[2] = 155;
  PIN[3] = 5;
  MIN[3] = 0;
  MAX[3] = 179;
  INITANGLE[3] = 25;
  
  for (int i = 0; i < SERVOS; i++){
    myservo[i].attach(PIN[i]);
    myservo[i].write(INITANGLE[i]);
    value[i] = 0;
    idle = 0;
  }  
}

void move_bt(){ 
  checkSoftSerial();

  for (int i = 0; i < SERVOS; i++){
    currentAngle[i] = myservo[i].read();
    
    if (servo_moving[i] != 0){
      currentAngle[i] += servo_moving[i];
      currentAngle[i] = currentAngle[i] > MAX[i] ? --currentAngle[i] : currentAngle[i];
      currentAngle[i] = currentAngle[i] < MIN[i] ? ++currentAngle[i] : currentAngle[i];
      myservo[i].write(currentAngle[i]);
      delay(20);
    }
  }
}

void checkSoftSerial() {
  String str = "";
  
  if (mySerial.available()){
    for (int i = 0 ; i < 2; i++){
      str += (char)mySerial.read(); 
    }
    // the servo to move
    int value = str.toInt();
    bt_servo = value / 10;
    
    // the direction to move
    int angle = value % 10;
    if (angle == 2) bt_move = 1;
    else if (angle == 1) bt_move = -1;
    else bt_move = 0;
    
    servo_moving[bt_servo] = bt_move;
  }
}

void move_joy(){
  for (int i = 0; i < SERVOS; i++){
    value[i] = analogRead(i);
    currentAngle[i] = myservo[i].read();
    
       
    if (value[i] > 612) {
      idle = 0;      
      if (currentAngle[i] > MIN[i]) --currentAngle[i]; 
    } else if (value[i] < 412) {
      idle = 0;
      if (currentAngle[i] < MAX[i]) ++currentAngle[i]; 
    } else {
      ++idle;
    }
    
    if (idle == 100){
      myservo[i].detach();
    }
  }      
    
  for (int i = 0 ; i < SERVOS; i++){
    if (!myservo[i].attached()) myservo[i].attach(PIN[i]);
    myservo[i].write(currentAngle[i]);
  }  

  delay(20);
}

void auto_mode(){
  for (int i = 0; i < 2; i++){
    closeclaw(false);
    armfromto(afrom, ato);
    closeclaw(true);
    delay(DELAYTIME); 
    armfromto(ato, amiddle);
    delay(DELAYTIME);   
    armfromto(amiddle, afinal);
    closeclaw(false);
    delay(DELAYTIME);
    armfromto(afinal, aafterfinal);
    delay(DELAYTIME);
    armfromto(aafterfinal, afrom);
    delay(DELAYTIME);
  } 
}

void armfromto(int *arrf, int *arrt){
  int lp[3], seg = 3, sign;
  
  for (int i = 0; i < 3; i++){    
    lp[i] = abs((arrt[i] - arrf[i])/seg);
  }
  
  //delay(DALAYTIME);
  for (int i = 0; i < 3; i++){
    sign = arrt[i] - arrf[i] > 0 ? 1 : -1;
    for (int j = 0; j < lp[i]; j++){
      myservo[i].write(arrf[i]+j*seg*sign);
      delay(20);
    }
    delay(DELAYTIME);
  }
}

void closeclaw(boolean op){
  if (op){
    myservo[3].write(5);
  } else {
    myservo[3].write(30);
  }
}

But where I'm stuck is on the move_joy() function

void move_joy(){			         //Declares the ‘move-joy()’ function
  for (int i = 0; i < SERVOS; i++){	         //Creates the ‘for’ loop, counting 1 servo at the time to perform the following instructions
    value[i] = analogRead(i);	         //Reads analog feed from the Joystick and stores it into the Variable ‘Value’ for each servo
    currentAngle[i] = myservo[i].read();   //Reads the angle of each servo and stores it in the Variable ‘CurrentAngle’
    
       
    if (value[i] > 612) {		        //If the Joystick is over 612…
      idle = 0;      
      if (currentAngle[i] > MIN[i]) --currentAngle[i]; //If the angle of a given Servo is larger than its MIN limit, it will decrease it by 1 in each loop
    } else if (value[i] < 412) {	     //If the Joystick is under 412…
      idle = 0;
      if (currentAngle[i] < MAX[i]) ++currentAngle[i]; //If the angle of a given Servo is smaller than its MAX limit, it will increase it by 1 in each loop
    } else {
      ++idle;
    }
    
    if (idle == 100){
      myservo[i].detach();
    }
  }

I would like to know why the code moves only between 412 and 612, and also if idle means really stand still and why does it detaches the servos at 100. Again, any help will be greatly appreciated, have a great day! :slight_smile:

2

I would like to know why the code moves only between 412 and 612

It doesn't. It only does something when the joystick value is below 412 or above 612. The reading from the joystick when it is not being held should be approximately, so the code requires that the stick be moved a noticeable amount, before it starts moving the servo.

3

For improved readability, I'd change this:

    if (value[i] > 612) {
      idle = 0;
      if (currentAngle[i] > MIN[i]) --currentAngle[i];
    } else if (value[i] < 412) {
      idle = 0;
      if (currentAngle[i] < MAX[i]) ++currentAngle[i];
    } else {
      ++idle;
    }

To this:

if (value[i] > 612) {
      idle = 0;
      if (currentAngle[i] > MIN[i]) {
        currentAngle[i]--;
      }
    } else if (value[i] < 412) {
      idle = 0;
      if (currentAngle[i] < MAX[i]) {
        currentAngle[i]++;
      }
    } else {
      idle++;
    }

The code blocks defined by the extra { / } pairs make the conditional flow more clear.

And, since you're not using the pre-delta value in your '++var;' and '--var;' statements, I think 'var++;' and 'var--;' are more readable. YMMV.

4

Thanks for the reply PaulS, that makes sense. So just to be clear, if the Joystick lever is pushed (for example) to the right, then the program interprets that as a value greater than 612, therefore sending the signal to the servo to move one way, and if the Joystick is moved to the left, then the program will interpret that as a value less than 412 and will move the servo in the opposite direction, and if it remains between 412 and 612 it should remain idle, is this right? :slight_smile:

Also, why when idle == 100 it detaches the servo?

if (idle == 100){
myservo*.detach();*

  • }*
  • } *
5

gfvalvo:
For improved readability, I'd change this:

    if (value[i] > 612) {

idle = 0;
     if (currentAngle[i] > MIN[i]) --currentAngle[i];
   } else if (value[i] < 412) {
     idle = 0;
     if (currentAngle[i] < MAX[i]) ++currentAngle[i];
   } else {
     ++idle;
   }




To this:

if (value[i] > 612) {
     idle = 0;
     if (currentAngle[i] > MIN[i]) {
       currentAngle[i]--;
     }
   } else if (value[i] < 412) {
     idle = 0;
     if (currentAngle[i] < MAX[i]) {
       currentAngle[i]++;
     }
   } else {
     idle++;
   }




The code blocks defined by the extra { / } pairs make the conditional flow more clear.

and I in turn would change it to

if (value[i] > 612)
{
  idle = 0;
  if (currentAngle[i] > MIN[i])
  {
    currentAngle[i]--;
  }
}
else if (value[i] < 412)
{
  idle = 0;
  if (currentAngle[i] < MAX[i])
  {
    currentAngle[i]++;
  }
}
else
{
  idle++;
}

to make the code blocks even more obvious.

I would also replace the magic numbers with suitably named constants to make the code even more readable.

6

gfvalvo and UKHeliBob, thank you very much for the useful advice, I will apply these corrections to my code! :slight_smile:

In the mean time, could any of you guys clarify what is the following set of instructions doing?

if (idle == 100){
myservo*.detach();*

  • }*
    Thanks guys!
7

In the mean time, could any of you guys clarify what is the following set of instructions doing?

if (idle == 100){
myservo*.detach();*

  • }*
    [/quote]
    You mean
    * *if (idle == 100){      myservo[i].detach();    }* *
    I can tell you what it does, but not why
    Note that the code looks different if you don't use code tags.
8

What it appears is happening is that idle is incremented whenever the function is called, and the joystick is centered. After some number of times, with no movement of the joystick the servo is detached.

Why the code was written that way is a mystery.

9

PaulS, do you think it detaches the servo after 100 as a safety measure to avoid the sketch running eternally or something like that? :slight_smile:

10

Alan777:
PaulS, do you think it detaches the servo after 100 as a safety measure to avoid the sketch running eternally or something like that? :slight_smile:

The sketch will run forever, regardless of whether the servo is detached, or not.

I don't know if detaching the servo saves energy, or not. I rather doubt it, since the motor in the servo and the battery have no idea whether the Arduino has attached the servo or detached it.

11

A robot arm must hold its commanded position so detaching the servos is not a good idea. There may be enough friction for the robot to hold position with the servos detached but I would not rely on it.