Improve servo control with force sensors

I have a servo motor and two force sensors that can provide values from 0 to 1023.
I would like to make the servo spin clockwise when I press the first force sensor and to spin counterclockwise when I press the second sensor.

I use map() to convert the sensor value into the servo range and it seems to work.

The problem is that the servo motion is not smooth enough and it seems jerky.
I would like to make the servo slowly increase its position as long as I keep the pression on the force sensor while now it only moves based on the amount of pression.

In addition, when I press the other sensor, the servo considers the new position starting from the center position and not its current position.

How can I solve this problem? Do you have any suggestions?

Thank you!

#include <SoftwareSerial.h>

SoftwareSerial arm(11, 10); // RX, TX
int range = 500;
int time = 2000;
int t_delay = 80;
int reading = 0; 
int prev_reading = 0; 
int value = 0;
int reading_up = 0;
 
void setup() {
   Serial.begin(9600);
   arm.begin(9600);
   Serial.println("Laparoscopic Arm - Console");
}


void loop(void) {
 
  reading = analogRead(A1); //attached to analog 0
  reading_up = analogRead(A0);

  Serial.print("Sensor value down = ");
  Serial.println(reading);
  Serial.print("Sensor value up = ");
  Serial.println(reading_up);
  
  if (reading > 5 && reading_up < 5){
  value = map(reading, 6, 1023, 1480, 2200);
  Serial.print("Sono in reading_down: ");
  Serial.println(reading);
  if (value > prev_reading){
  move1(18, value, range, time);
  prev_reading = value;
  }
  else {
  move1(18, prev_reading, range, time);
  }
  } else if (reading < 5 && reading_up > 5){

  Serial.print("Sono in reading_up: ");
  Serial.println(reading_up);
  value = map(reading_up, 6, 1023, 1480, 1000);
  
  if (value < prev_reading){
  move1(18, value, range, time);
  prev_reading = value;
  }
  else {
  move1(18, prev_reading, range, time);
  }
  
    
  }
 
 
  delay(1000);
}

void move1(int servo, int position, int micro, int timer) {
   arm.print("#");
   arm.print(servo);
   arm.print(" P");
   arm.print(position);
   arm.print(" S");
   arm.print(micro);
   arm.print(" T");
   arm.println(timer);
   Serial.print("#");
   Serial.print(servo);
   Serial.print(" position:");
   Serial.println(position);
   delay(timer);
}

Speed up your Serial baud rate.

"In addition, when I press the other sensor, the servo considers the new position starting from the center position and not its current position."

Are you computing the new position from the current position and sending that to the servo code?

Paul

Thank you for your reply, Paul.

Unfortunately, I'm not able to figure out how to calculate the new position starting from the servo current position :confused:

You are obviously not using a standard hobby-type servo but some device which is driven via SoftwareSerial. What is it? Please provide datasheet / link describing it and what controls it takes.

Steve

slipstick:
You are obviously not using a standard hobby-type servo but some device which is driven via SoftwareSerial. What is it? Please provide datasheet / link describing it and what controls it takes.

Steve

I'm using the SSC-32U USB Servo Controller made by Lynxmotion and it is connected via RS232 to Arduino.
However, the servo control is the same, it just needs some addition commands, but the concept is the same.

You might drop back and use a pot instead of the “force sensor” (assuming it is a resistance type) to see if there is any difference in performance. The hobby grade force sensors are probably not accurate enough for close control.

But even if they are not accurate, i could use them to move the servo until they are pressed.
It would be ok for me but I don't know how to do that by code unfortunately.

It sounds like the servo is doing exactly what your code is telling it to do.
Move to a position based on how hard you are pressing your force sensor.

But it also sounds like this is not the behavior you want. Instead, you want the servo to start at some position and stay there if nothing is pushed.
While one sensor is pushed, the servo should slowly move one direction.
While the other sensor is pushed, the servo should slowly move the other direction.

Is this what you want?

I would declare a variable, maybe called Position, and set it to your midrange value of 1480.

In your loop, If reading > 5, increment Position.
Test Position. If it is over your max value of 2200, set Position to 2200.
Else if reading_up > 5, decrement Position.
Test Position. If it is less than your minimum position of 1000, set Position to 1000.
Then execute your move command to Position.

vinceherman:
It sounds like the servo is doing exactly what your code is telling it to do.
Move to a position based on how hard you are pressing your force sensor.

But it also sounds like this is not the behavior you want. Instead, you want the servo to start at some position and stay there if nothing is pushed.
While one sensor is pushed, the servo should slowly move one direction.
While the other sensor is pushed, the servo should slowly move the other direction.

Is this what you want?

I would declare a variable, maybe called Position, and set it to your midrange value of 1480.

In your loop, If reading > 5, increment Position.
Test Position. If it is over your max value of 2200, set Position to 2200.
Else if reading_up > 5, decrement Position.
Test Position. If it is less than your minimum position of 1000, set Position to 1000.
Then execute your move command to Position.

Yes it is exactly what I would like to achieve!
So should I avoid to use the map function in this case since I just need to increase/decrease the value?

Should I use a for() cycle to increase the servo position instead of the map() function?

marcusbarnet:
Should I use a for() cycle to increase the servo position instead of the map() function?

No for loop or map needed if you construct the loop() to be something like this:

  • analogread first sensor.
  • analogread second sensor.
  • increment or decrement.
  • move()
  • delay()
    bigger values in the delay function will make the servo movement slower. After you get this working, consider adjusting the value of the delay based on the analogread value, so that the harder you push, the faster the servo moves. (this would be a good use of the map function)

The problem is: how can I get the current servo position? I think I can't retrieve the servo position since I can only set it.

I'm really confused about it :frowning:

"The problem is: how can I get the current servo position? I think I can't retrieve the servo position since I can only set it."

Well, if things are working as expected, the servo is at the position you last sent it to. Save that as a variable if you need to access it later. If the servo is not at the position you sent it to, you have other issues.

I tried to write a code in order to increase or decrease the servo position.

I set the initial servo position as 1250 and then I increased or decreased this value depending on the sensor pressed by the user.

The problem is that when the servo reaches its maximum value, it starts to vibrates and it is not able to keep its position and the same happens when I decrease its position.

The servo works fine till it reaches its upper and lower position, then it starts to vibrate.

This is the code:

#include <SoftwareSerial.h>

SoftwareSerial arm(11, 10); // RX, TX

int range = 500;
int time = 2000;
int t_delay = 80;
int reading = 0; 
int prev_reading = 0; 
int value = 0;
int reading_up = 0;
int startup = 0;

int servoPosition = 1250;
int servoIncrement = 250;
 
void setup() {
   Serial.begin(9600);
   arm.begin(9600);
   Serial.println(" ");
   move1(18, 1250, range, time);
}


void loop(void) {

  
  reading = analogRead(A1); //attached to analog 0
  reading_up = analogRead(A0);

  Serial.print("Sensor value down = ");
  Serial.println(reading);
  Serial.print("Sensor value up = ");
  Serial.println(reading_up);
  
  if (reading > 5 && reading_up < 5){
  value = reading;
  Serial.print("Sono in reading_down: ");
  Serial.println(reading);
  Serial.print("servoPosition: ");
  Serial.println(servoPosition);
  if (servoPosition > 1750){
    servoPosition = 2000;
    } else {
  servoPosition += servoIncrement;
    }
  move1(18, servoPosition, range, time);

  } else if (reading < 5 && reading_up > 5){

  Serial.print("Sono in reading_up: ");
  Serial.println(reading_up);
  value = reading_up;  
  Serial.print("servoPosition: ");
  Serial.println(servoPosition);
   if (servoPosition < 500){
    servoPosition = 350;
    } else {
  servoPosition -= servoIncrement;
    }
  move1(18, servoPosition, range, time);

    
  }
  
  delay(50);
}

void move1(int servo, int position, int micro, int timer) {
   arm.print("#");
   arm.print(servo);
   arm.print(" P");
   arm.print(position);
   arm.print(" S");
   arm.print(micro);
   arm.print(" T");
   arm.println(timer);
   Serial.print("#");
   Serial.print(servo);
   Serial.print(" position:");
   Serial.println(position);
   delay(timer);
}

and this is the console output:

20:28:25.333 -> #18 position:1250
20:28:27.325 -> Sensor value down = 557
20:28:27.360 -> Sensor value up = 0
20:28:27.395 -> Sono in reading_down: 557
20:28:27.395 -> servoPosition: 1250
20:28:27.428 -> #18 position:1500
20:28:29.419 -> Sensor value down = 530
20:28:29.454 -> Sensor value up = 0
20:28:29.487 -> Sono in reading_down: 530
20:28:29.521 -> servoPosition: 1500
20:28:29.521 -> #18 position:1750
20:28:31.525 -> Sensor value down = 180
20:28:31.559 -> Sensor value up = 0
20:28:31.594 -> Sono in reading_down: 180
20:28:31.628 -> servoPosition: 1750
20:28:31.628 -> #18 position:2000
20:28:33.648 -> Sensor value down = 354
20:28:33.682 -> Sensor value up = 0
20:28:33.682 -> Sono in reading_down: 354
20:28:33.717 -> servoPosition: 2000
20:28:33.751 -> #18 position:2000
20:28:35.743 -> Sensor value down = 0
20:28:35.776 -> Sensor value up = 887
20:28:35.811 -> Sono in reading_up: 887
20:28:35.811 -> servoPosition: 2000
20:28:35.845 -> #18 position:1750
20:28:37.865 -> Sensor value down = 0
20:28:37.865 -> Sensor value up = 944
20:28:37.899 -> Sono in reading_up: 944
20:28:37.934 -> servoPosition: 1750
20:28:37.934 -> #18 position:1500
20:28:39.956 -> Sensor value down = 0
20:28:39.989 -> Sensor value up = 898
20:28:39.989 -> Sono in reading_up: 898
20:28:40.024 -> servoPosition: 1500
20:28:40.057 -> #18 position:1250
20:28:42.050 -> Sensor value down = 0
20:28:42.083 -> Sensor value up = 863
20:28:42.117 -> Sono in reading_up: 863
20:28:42.150 -> servoPosition: 1250
20:28:42.150 -> #18 position:1000
20:28:44.175 -> Sensor value down = 0
20:28:44.175 -> Sensor value up = 826
20:28:44.209 -> Sono in reading_up: 826
20:28:44.242 -> servoPosition: 1000
20:28:44.277 -> #18 position:750
20:28:46.275 -> Sensor value down = 0
20:28:46.275 -> Sensor value up = 845
20:28:46.308 -> Sono in reading_up: 845
20:28:46.343 -> servoPosition: 750
20:28:46.376 -> #18 position:500
20:28:48.369 -> Sensor value down = 0
20:28:48.403 -> Sensor value up = 891
20:28:48.403 -> Sono in reading_up: 891
20:28:48.437 -> servoPosition: 500
20:28:48.471 -> #18 position:250

“The problem is that when the servo reaches its maximum value, it starts to vibrates and it is not able to keep its position and the same happens when I decrease its position.”

The simple solution is to determine the command value where the servo starts to have issues, and don’t allow commands beyond this value. An example might be if greater than a 2455 ms command causes problems limit commands to that value or less. Do a simple command test before sending the command, something like if command is greater than 2455, then the command is 2455. Below is some simple servo test code that you can find the command values where the servo starts to have issues.

// zoomkat 7-30-10 serial servo test
// type servo position 0 to 180 in serial monitor
// for writeMicroseconds, use a value like 1500
// Powering a servo from the arduino usually *DOES NOT WORK*.

String readString;
#include <Servo.h> 
Servo myservo;  // create servo object to control a servo 

void setup() {
  Serial.begin(9600);
  myservo.attach(9);
}

void loop() {

  while (Serial.available()) {

    if (Serial.available() >0) {
      char c = Serial.read();  //gets one byte from serial buffer
      readString += c; //makes the string readString
      delay(3);
    } 
  }

  if (readString.length() >0) {
    Serial.println(readString);
    int n = readString.toInt();
    Serial.println(n);
    myservo.writeMicroseconds(n);
    //myservo.write(n);
    readString="";
  } 
}

I did some test with your code (I had to modify it a little bit to make it work with my ssc32 board) and I think the range values are 1900 -> 600 and the center position should be 1250.

If I try to give higher or lower values, then the servo doesn't behave properly.

Do I only need to change upper and lower values to my previous code or is there any other programming problem?

May be is it related to the int servoIncrement = 250; value?

I was thinking to change the code with something like:

 if (servoPosition > 1901){
    servoPosition = 1900;
    } else {
  servoPosition += servoIncrement;
    }
  move1(18, servoPosition, range, time);

But if I starts from 1250, I have:

1250 + 250 = 1500
1500 + 250 = 1750
1750 + 250 = 2000

Is my code OK in that case?

"Is my code OK in that case?"

What does your testing show. I would put the value test just before writing to the servo controller. You don't want to add an increment after the test is performed. Add the increment, then perform the test. Make a simple test like "if position is > 2455, then position is 2455", which limits the servo command to 2455 or less.