Need help on programming servomotor!

Hello!
I need your help! I have a project and I have some troubles for programming it. Here’s my project: I need to program a motor so the motor will turn Right or left according to the distance that 2 captors receive (ultrasound captors). I already have program the captors but i have troubles to match those with my motor. I want the motor to turn left when the distance calculated with the captor 1 is bigger than the distance of capteur 2 and turns left when the distance of the captor 2 is bigger than captor 1. Here is my program:

const int capteur1 = 25; // capteur attaché au pin 26
const int capteur2 = 24; // capteur attaché au pin 27
const int capteur3 = 23; // capteur attaché au pin 28
long pulsion1, cm1;
long pulsion2, cm2;
long pulsion3, cm3;
#include <Servo.h>
*Servo servomoteur; *

void setup () {

  • Serial.begin(38400);*

  • servomoteur.attach(26); // servomoteur attaché au pin 29*

  • pinMode(capteur1, INPUT);*

  • pinMode(capteur2, INPUT);*

  • pinMode(capteur3, INPUT);*
    }
    int mode = 0 ;
    void loop () {

  • pulsion1 = pulseIn (capteur1, HIGH);*
    _ cm1 = pulsion1/147 * 2.54;_

  • Serial.print("capteur 1 : ");*

  • Serial.print (cm1);*

  • Serial.println();*

  • delay(10);*

  • pulsion2 = pulseIn (capteur2, HIGH);*
    _ cm2 = pulsion1/147 * 2.54;_

  • Serial.print("capteur 2 : ");*

  • Serial.print (cm2);*

  • Serial.println();*

  • delay(100);*

  • pulsion3 = pulseIn (capteur3, HIGH);*
    _ cm3 = pulsion1/147 * 2.54;_

  • Serial.print("capteur 3 : ");*

  • Serial.print (cm3);*

  • Serial.println();*

  • delay(100);*

if (mode == 0) {

  • if (cm1 <= 50) {*

  • servomoteur.write(135);*

  • delay (100);*

  • }*

  • if (cm1 > 50 && cm1 <= 150) {*

  • servomoteur.write (120);*

  • delay (100);*

  • }*

  • if (cm1 > 150 && cm1 <= 250) {*

  • servomoteur.write(100);*

  • delay (100);*

  • }*

  • if (cm1 > 250)*

  • servomoteur.write (90);*

  • delay (100);*

  • }*

  • if (mode == 1) {*

  • if (cm2 <= 50) {*

  • servomoteur.write(45);*

  • delay (100);*

  • }*

  • if (cm2 > 50 && cm1 <= 150) {*

  • servomoteur.write (60);*

  • delay (100);*

  • }*

  • if (cm2 > 150 && cm1 <= 250) {*

  • servomoteur.write(80);*

  • delay (100);*

  • }*

  • if (cm2 > 250)*

  • servomoteur.write (90);*

  • delay (100);*
    }
    if (cm2 > cm1) {

  • mode = 0 ;*
    }

if (cm1 > cm2) {

  • mode = 1 ;*
    }

}

Dont pret attention to the captor 3! Can someone tell me if there’s a error? I just need you to verify the servomotor’s lines. Does the angle that i wrote match with the left and right rotation? I cannot verify it because i dont have the material to test it now (those are in the lab at school). Cm1 and cm3 are the distances capted by my captors!

Thank for your help!

PS: I’m sorry if my english is bad! I’m from France! :slight_smile:

If cm1 == cm2, what is mode set to?

Some of your conditions will not work because 2 of them can be true at the same time

    if (cm2 > 50 && cm1 <= 150) 
    {
      servomoteur.write (60);
      delay (100);
    }
    
    if (cm2 > 150 && cm1 <= 250) 
    {
      servomoteur.write(80);
      delay (100);
    }

What will happen if cm2 = 160 and cm1 = 100 ?

well if cm2= 160 and cm1 = 100, because 160>100, it will activate the mode 2, and the servo will normally turn right? no? And yes, I ve forgot to introduce the cm1 == cm2 ...

What mode 2 would that be ? I can only see mode 0 and mode 1 in the program.

My point was that cm2= 160 and cm1 = 100 both of these tests will be true

if (cm2 > 50 && cm1 <= 150) 
    {
      servomoteur.write (60);
      delay (100);
    }
    
    if (cm2 > 150 && cm1 <= 250) 
    {
      servomoteur.write(80);
      delay (100);
    }

and the program will execute both servo commands with a delay of 0.1 seconds between them. Is that what you want it to do ?

I would think it would be as simple as,
if(cm1 > cm2)
{/* LEFT */ }

else if(cm1 < cm2)
{/* RIGHT */}

else
{/* CENTER */}

And also what you can do, is take the difference of the two readings, map them and use that value to tell the servos how fast to go.

mmmh, but i need to configure the angle in function of the distance… and i dont understan the mode 1 and mode 2 thing… why can i not use the mode 0? is that the same?
Can you take my code and do the modifications in it, and make the transformations bolt? Im sorry, im very bad at writing code :S

I'm surprised no one caught that the equations for cm2 & 3, are the exact same as cm1, so basically cm1,2,3 will always equal each other.

I’ve wrote an other code, way more complicated than the first one. Here’s my goal: With two captors each connected to a servomotor, I want to make those servomotor rotate in function of the distance capted by the captor. For example, for a distance between 150 and 300 cm, i want the motor to turn 120 degrees and stay at the position until the distance reaches 300 (the motor turn back at central position) or reaches <150 (the motor turns 150 degrees). Each captors controls one motors. There is also a third captor; if the distance goes under 50cm, a piezzo (f=2900Hz) will me activated. Here’s my code:

const int capteur1 = 26; // connecté le capteur 1 (celui de droite) sur le pin 26
const int capteur2 = 25; // connecté le capteur 2 (celui de gauche) sur le pin 25
const int capteur3 = 24; // connecté le capteur 3 (celui du centre) sur le pin 24
long pulsion1, cm1;
long pulsion2, cm2;
long pulsion3, cm3;
#include <Servo.h> // librairie servomoteur
*Servo servomoteur1; *
*Servo servomoteur2; *
int piezzo = 7; // connecté le piezzo sur le pin 7
void setup () {

* Serial.begin(38400);*
* servomoteur1.attach(27);*
* servomoteur2.attach(27);*
* pinMode(capteur1, INPUT);*
* pinMode(capteur2, INPUT);*
* pinMode(capteur3, INPUT);*
* pinMode (piezzo, OUTPUT);*
}
void loop () {
* pulsion1 = pulseIn (capteur1, HIGH);*
_ cm1 = pulsion1/147 * 2.54;_
* Serial.print("capteur 1 : ");*
* Serial.print (cm1);*
* Serial.println();*
* delay(10);*

* if (cm1 <= 150) {*
* servomoteur1.write(150);*
* delay (100);*
* }*
* if (cm1 > 150 && cm1 <= 300) {*
* servomoteur1.write (120);*
* delay (100);*
* }*
* if (cm1 > 300) {*
* servomoteur1.write (90);*
* delay (100);*
* }*

* pulsion2 = pulseIn (capteur2, HIGH);*
_ cm2 = pulsion2/147 * 2.54;_
* Serial.print("capteur 2 : ");*
* Serial.print (cm2);*
* Serial.println();*
* delay(10);*

* if (cm2 <= 150) {*
* servomoteur2.write(150);*
* delay (100);*
* }*
* if (cm2 > 150 && cm2 <= 300) {*
* servomoteur2.write (120);*
* delay (100);*
* }*
* if (cm2 > 300){*
* servomoteur2.write (90);*
* delay (100);*
* }*

* pulsion3 = pulseIn (capteur3, HIGH);*
_ cm3 = pulsion3/147 * 2.54;_
* Serial.print("capteur 3 : ");*
* Serial.print (cm3);*
* Serial.println();*
* delay(10);*

* if (cm3 < 50) {*
* digitalWrite (piezzo, HIGH);*
* delay (1);*
* digitalWrite (piezzo, LOW);*
* delay (1);*

* }*

If someone sees a error, please tell me! I really need it to work! (do you know if my method to calculate the distance is accured? Do I have to use the NewPing library? Because it seems that the captors arent very trustful: they change the distance sometime, going from 20cm to 70 and then back to 20cm… i really dont know where the problem is…

Thank for your help! :slight_smile:

Try this and play with the values I said at the bottom of the code. Please know that I can’t test this, so don’t get mad if it doesn’t do exactly what you want.

const int capteur1 = 25;  // capteur attaché au pin 26
const int capteur2 = 24; // capteur attaché au pin 27
const int capteur3 = 23;  // capteur attaché au pin 28

int diff, value;
long pulsion1, cm1;
long pulsion2, cm2;
long pulsion3, cm3;
#include <Servo.h> 
Servo servomoteur;     


void setup () {

  Serial.begin(38400);
  servomoteur.attach(26); // servomoteur attaché au pin 29
  pinMode(capteur1, INPUT);
  pinMode(capteur2, INPUT);
  pinMode(capteur3, INPUT);
}

void loop () {
  pulsion1 = pulseIn (capteur1, HIGH);
  cm1 = pulsion1/147 * 2.54;
  Serial.print("capteur 1 : ");
  Serial.print (cm1);
  Serial.println();
  delay(10);

  pulsion2 = pulseIn (capteur2, HIGH);
  cm2 = pulsion2/147 * 2.54;
  Serial.print("capteur 2 : ");
  Serial.print (cm2);
  Serial.println();
  delay(10);

  pulsion3 = pulseIn (capteur3, HIGH);
  cm3 = pulsion3/147 * 2.54;
  Serial.print("capteur 3 : ");
  Serial.print (cm3);
  Serial.println();
  delay(10);

  if(cm1 > cm2)/* LEFT */ 
  { 
    diff = cm1 - cm2;
  }

  else if(cm1 < cm2) /* RIGHT */ 
  {
    diff = cm1 - cm2;  
  }

  else /* CENTER */
  {
    diff = 0;
  }

  value = map(diff, -150, 150, 0, 180); //map(diff, X1, X2, 0, 180); Play with X1 and X2 to get the correct angles.
  servomoteur.write(value);
}