Humanoid robotic Arm With Servo Control

Although I am not new to Arduino. I am new to writing code for Arduino. I have been using an Arduino in my DIY CNC machine for quite a while now. And now I’ve gone and made a humanoid robotic arm for which I need help writing the code. Here’s what I got so far.
Please take a look at the code and tell me whats wrong with it.
or how i can make it better.

/* 
/* 
 Controlling servo position using potentiometers (variable resistor) 
 by Michal Rinott <http://people.interaction-ivrea.it/m.rinott> 
 modified by Bartolobot on 19 Apr 2016 added 4 continuous rotation  servos and a second
 control method with buttons
 modified on 8 Nov 2013
 by Scott Fitzgerald
 http://arduino.cc/en/Tutorial/Knob
*/

#include <Servo.h>

Servo myservo;  // create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;

const int buttonPin = 2;    // the number of the pushbutton pin
const int buttonPin2 = 4;     
const int buttonPin3 = 7;     
const int buttonPin4 = 8;    
const int buttonPin5 = 13;
const byte Relay1 = 6;     // Relay pin for pneumatic valve

int buttonState = 0;       // variable for reading the pushbutton status        
int buttonState2 = 0;
int buttonState3 = 0;
int buttonState4 = 0;
int buttonState5 = 0;
int potpin = 0;           // analog pin used to connect the potentiometer
int val;                  // variable to read the value from the analog pin
int potpin1 = 1;
int val1;
int potpin2 = 2;
int val2;
int potpin3 = 3;
int val3;   

void setup()
{
  myservo.attach(9);      // attaches the servo on pin 9 to the servo object
  myservo2.attach(10);
  myservo3.attach(11);
  myservo4.attach(12);

  
  pinMode(buttonPin, INPUT);    // initialize the pushbutton pin as an input:
  pinMode(buttonPin2, INPUT);
  pinMode(buttonPin3, INPUT);
  pinMode(buttonPin4, INPUT);
  pinMode(buttonPin5, INPUT);
  pinMode(Relay1, OUTPUT);      // initialize the pin as an output:
 
}

void loop() { 
  
  val = analogRead(potpin);       //reads the value of the potentiometer (value between 0 and 1023) 
  val = map(val, 0, 1023, 0, 180); //scale it to use with the servo(value between 0and180) 
  val1 = analogRead(potpin1);
  val1 = map(val1, 0, 1023, 0, 180);
  val2 = analogRead(potpin2);
  val2 = map(val2, 0, 1023, 0, 180);
  val3 = analogRead(potpin3);
  val3 = map(val3, 0, 1023, 0, 180); 
  
  myservo.write(val);          // sets the servo speed according to the scaled value 
  myservo2.write(val1);
  myservo3.write(val2);
  myservo4.write(val3);

  buttonState = digitalRead(buttonPin); // read the state of the pushbutton value:
  buttonState2 = digitalRead(buttonPin2);
  buttonState3 = digitalRead(buttonPin3);
  buttonState4 = digitalRead(buttonPin4);
  buttonState5 = digitalRead(buttonPin5);
  
 if (buttonState == HIGH) {  //  move myservo
   myservo.write(val);}  // sets the servo speed according to the scaled value 
  else {myservo.write(90);}  //  stop the servo.
    
 if (buttonState2 == HIGH) {
    myservo2.write(val1);}
  else {myservo2.write(90);}
   
 if (buttonState3 == HIGH) {
    myservo3.write(val2);}
  else {myservo3.write(90);}

 if (buttonState4 == HIGH) {
   myservo4.write(val3);}
  else {myservo4.write(90);}

  if (buttonState5 == HIGH) {   //move all servos with one button
    myservo.write(val);
     delay(250);               // waits for the servo to get there
    myservo2.write(val);
     delay(500);
    myservo3.write(val);
     delay(820);
    myservo4.write(val);
     delay(1200);}
  else {(val = 90);} 
       delay(2500);
       
   digitalWrite(Relay1, HIGH); 
     delay(250); 
   digitalWrite(Relay1, LOW);
     delay(2000);  
}

Knob_switch_for_4_servos.ino (3.2 KB)

here's a link to a video on youtube

Please post your code here to make it easier to provide help.

read this before posting a programming question

Thanks for posting the code

Please take a look at the code and tell me whats wrong with it.
or how i can make it better.

Does the code do what you want ? If not what does it do ?

Please take a look at the code and tell me whats wrong with it.

You haven't mentioned what the actual problem is. (As Bob also just mentioned.)

In the other thread that you posted in, you said:-

i have a very similar project but i have 4 servos which i made continuous rotation and five switches one for every servo and one more for all together.
i will be adding more servos and switches in the future .
But for now i need to figure out the code for what i got now.......I have already started my own thread ( Humanoid robotic Arm With Servo Control) but not getting any help yet. Although they are very similar issues I understand what your saying.

In this thread you've made no mention that your servos have been modified for continuous rotation. All very confusing.

So, have your servos been modified or not?

So, have your servos been modified or not?

I just asked that in the other thread !

UKHeliBob:
I just asked that in the other thread !

Haha. Great minds.....
All very confusing, isn't it?

Hay guys thanks for answering my thread.

UKHeliBob and Old Steve,

Yes, the servos have been modified. i did this myself by centering and disabling (or Detaching ) the internal pot. Reason being that I needed more travel on my finger tendons than a 180' servo and horn could provide. also the torque Increase by essentially making them into small winches is what I was after.

The problem with the code is that it doesn't stop the servos when I want.
I would like them to stop at full extension and at full contraction of the fingers and any where in between.
Instead they will go beyond full extension and full contraction causing too much stress on the servos and tendons.In the case of going passed full extension the tendons will just start wrapping around the winch bobbin in the wrong direction (causing contraction) which is not as bad as going passed full contraction , but still undesired.
I cant have them wrap around in the wrong direction because they will come in contact with another servo(winch). not to mention that it also reverses the servos dir.
I need them to stop at full extt. & contraction
I know this takes roughly around 1500 milliseconds at full speed ("0") in one direction and ("180") in the other.The speed is currently adjusted by the 4 pots I would like to eliminate them.

Just have a button for each finger that switches direction after each push and maybe a timer to keep track of how long any given servo has traveled in one direction and keep the speed at full(0 or 180).
not sure if this is possible.

also would like some additional buttons for preprogrammed combined movements.

If you need to 'position' them, you really do need to have that pot connected, or else provide some other method of position feedback so your firmware knows when to say 'stop'. Is there any way you can incorporate limit switches of some kind?

Trying to use timing will never work reliably, as you're discovering.

Have you checked out "sail winch servos"? They're not cheap, but would be more suited to your purpose - multiple turns plus positioning capability. I've never used them, so can't provide much info, but I'm sure a Google search will turn up plenty.

Here's an example, but make sure you're sitting down before looking at the price:-
HS-785HB 3.5 Rotations
With luck you might be able to find lower cost versions. This was just the first I spotted.

Edit: Here's a cheaper one:-
Sail Winch Servo 13kg / 0.7sec (360deg) / 55g
Read the comments underneath it.
e.g:-

......and it will do 7 turns when driven by pulse widths of 900 to 2100 msec

also the torque Increase by essentially making them into small winches is what I was after.

Presumably the increased torque has come about because you are no longer using the long servo arms. The amount of torque available is a function of the amount of movement generated by a certain servo input so longer servo arms will produce more movement but less torque and vice versa.

You could increase the amount of movement of standard servos by gearing the output or by using levers but that would reduce the torque. It sounds like you are between a rock and a hard place unless you can fit a feedback mechanism to your modified "servos" in order to determine their position.

As has been suggested, the right tool for the job does sound like sail winch servos or you could change things entirely and use stepper motors over which you could have more control of position without the need for feedback.

Guys the code! Thats what I need help with!

I got the hardware dept. coverd.

As has been suggested, the right tool for the job does sound like sail winch servos or you could change things entirely and use stepper motors over which you could have more control of position without the need for feedback.

I dont want to make any big hardware changes.

Is there any way you can incorporate limit switches of some kind?

I have thought of adding limit switches.
its just too many switches (two for every finger, four for the thumb)
I will be adding feed back of some sort
considering optical sensors.

Trying to use timing will never work reliably, as you're discovering.

I can re-calibrate the servos easy enough. so not too concerned with strict reliability
and no I haven't found this to be true!
because I have not successfully implemented any timing in my CODE.
and would appreciate testing this myself.

I would like to implement into the CODE the features I laid out in my last post.

I know this takes roughly around 1500 milliseconds at full speed ("0") in one direction and ("180") in the other.The speed is currently adjusted by the 4 pots I would like to eliminate them.

Just have a button for each finger that switches direction after each push and maybe a timer to keep track of how long any given servo has traveled in one direction and keep the speed at full(0 or 180).
not sure if this is possible.

would like some additional buttons for preprogrammed combined movements.

I have no idea at this point how to do this.
can someone help me do this in my CODE!
so I can test it!
then I can go from there.

Thanks in advance.

could you use a resistor? something small like 1~10 ohms between ground and the servo when a limit is reached the current increases and the voltage drop across the resistor increases. now you can use an analog input on the Arduino to test for an increase in voltage drop to detect limits being reached. this will also detect when the hand grabbing something or other problems occur preventing damage.

Thanks for the Idea!
zhomeslice

That sounds like a good idea I am willing to try.
but dont know how to write the code to do this.

also when you say

1~10 ohms between ground and the servo

you mean the data pin on the servo or the positive pin?

now you can use an analog input on the Arduino to test for an increase in voltage drop to detect limits being reached

how exactly do i do this?

I hope this gives you an idea what to start with:

int sensorPin = A0;    // select the input pin for the potentiometer
int sensorValue = 0;  // variable to store the value coming from the sensor
int HighLimit = 200; // This is a guess increase/decrease it until the survo opperates normally 
void setup() {

  Serial.begin(9600);
}

void loop() {
  // read the value from the sensor:
  sensorValue = analogRead(sensorPin);
  // turn the ledPin on
  Serial.println(sensorValue);
  if (sensorValue > HighLimit) {
    // Place your code here to stop the servo
    Serial.println("Stop Too High");
  }
}

bartolobot:
Guys the code! Thats what I need help with!

I got the hardware dept. coverd....
CODE...CODE...CODE

No you don't. And yelling CODE at us over and over isn't going to get you anywhere.
(How to lose friends and alienate people.)

You want CODE - learn how to write it like everyone else does. If you have problems while writing your CODE, then post it and ask for help. Don't just ask for CODE.

Good luck with it.

There are still two threads running on the same subject which is not helping things when providing answers.

Thank you for that bit of code and idea.

zhomeslice

you have given me the most help. so far
every one else seems to jump off in other directions.

I'm going to try this right now and I will report back with some results.

thanks again.

As zhomeslice suggested I have been trying to this

could you use a resistor? something small like 1~10 ohms between ground and the servo when a limit is reached the current increases and the voltage drop across the resistor increases. now you can use an analog input on the Arduino to test for an increase in voltage drop to detect limits being reached. this will also detect when the hand grabbing something or other problems occur preventing damage.

But i guess i don't understand exactly where or how to wire the resister.

could you use a resistor? something small like 1~10 ohms

also I don't know how to write the code to do this

now you can use an analog input on the Arduino to test for an increase in voltage drop to detect limits being reached.

another thing from zhomeslice's next post.
Is that I dont see how any of this corresponds to the code he suggested.

int sensorPin = A0; // select the input pin for the potentiometer
int sensorValue = 0; // variable to store the value coming from the sensor
int HighLimit = 200; // This is a guess increase/decrease it until the survo opperates normally
void setup() {

Serial.begin(9600);
}

void loop() {
// read the value from the sensor:
sensorValue = analogRead(sensorPin);
// turn the ledPin on
Serial.println(sensorValue);
if (sensorValue > HighLimit) {
// Place your code here to stop the servo
Serial.println("Stop Too High");
}
}

If anyone cares to elaborate on this I would appreciate the help.

There are still two threads running on the same subject which is not helping things when providing answers.

Was not aware of the other thread.
can you help me find it?

Was not aware of the other thread.

You have posted to it