Control dc motor to specific point smoothly

Projects Programming
1

Hey Everyone,

This is the problem (sorry for bad English):

Dc-motor goes to point (400 mm) that ultrasonic sensor measures.

Dc-motor has to slow down at 300mm and 500mm before reaching the 400 mm.
If Dc-motor goes too far, for example (450mm). Dc-motor changes it's direction and goes smoothly to 400 mm (not at full speed what it does).

the main problem is that it works in one way, slows down smoothly from 500 mm to 400mm but not from 300mm to 400mm.

So what did I do wrong?

You can see what it looks like in 123d.circuits link:
Thank you for your help!

Stan

int ping = 12; //connection ultrasonic distance sensor
int knop = 11; //button to activate sensor
long x[] = {0, 400, 100, 600, 150, 800}; //mm //distances to stop motor eventually
long puls, afstand, afstandcal;
 
//h-brug
int enablepin1 = 4; //Pin h-bridge
int motor1links = 3; //dc-motor left
int motor1rechts = 2; //dc-motor right
 
//set van waar die moet afremmen //set where the motor starts to slow down
int set = 100;
 
//error tussen gemeten en bepaalde waarde 
long error = 0;
 
//voltage to change speed of the motor
long motorsnelheid;
 
 
void setup() {
  Serial.begin(9600);
  pinMode(knop, INPUT); 
  digitalWrite(knop, LOW); //button to activate ultrasonic sensor
 
  //h-brug
  pinMode(enablepin1, OUTPUT);
  pinMode(motor1links, OUTPUT);
  pinMode(motor1rechts, OUTPUT);
 
 
}
 
void loop() {
  Serial.println("baas");
  if (digitalRead(knop) == HIGH) {
    Serial.println("kaas");
    for (int i = 1; i < 6; i++) {
      while (error != x[i]) {
        //motor1 aan
        digitalWrite(enablepin1, HIGH);
        for (int j = 0; j < 10; j++) {
          pinMode(ping, OUTPUT);
          digitalWrite(ping, HIGH);
          delayMicroseconds(5);
          digitalWrite(ping, LOW);
          //measures signal getting back
          pinMode(ping, INPUT);
          puls = pulseIn(ping, HIGH);
          afstand = microsecondenaarmilimeters(puls);
          Serial.print("10 waarden ");
          Serial.println(afstand);
 
          afstandcal += afstand;
        }
        afstand = afstandcal / 10;
        delay(500);
        Serial.print("  gemiddelde "); //average speed over 10 measurements
        Serial.println(afstand);
        afstandcal = 0;
 
        error = x[i] - afstand;
        //omrekenen naar motorsnelheid
        motorsnelheid = map(error, -set, set, 255, -255);
 
      
        if (afstand < error - set) {
          analogWrite(motor1rechts, 0);
          analogWrite(motor1links, 255);
        }
        if (afstand > error + set) {
          analogWrite(motor1rechts, 255);
          analogWrite(motor1links, 0);
        }

        if (error < 0 && error < set) {
          analogWrite(motor1rechts, 0);
          analogWrite(motor1links, abs(motorsnelheid));
       
        }
        if (error > 0 && error > -set) {
          analogWrite(motor1rechts, abs(motorsnelheid));
          analogWrite(motor1links, 0);
        }
      }
 
    }
  }
}
 
long microsecondenaarmilimeters(long microsecondes) {
  return microsecondes * (0.343 / 2);}
2 
        motorsnelheid = map(error, -set, set, 255, -255);

Remember that map() does not constrain the output value to the range specified. That may be producing unexpected results. Try this:        motorsnelheid = map(constrain(error, -set, set), -set, set, 255, -255);

        if (afstand < error - set) {
          analogWrite(motor1rechts, 0);
          analogWrite(motor1links, 255);
        }
        if (afstand > error + set) {
          analogWrite(motor1rechts, 255);
          analogWrite(motor1links, 0);
        }

        if (error < 0 && error < set) {
          analogWrite(motor1rechts, 0);
          analogWrite(motor1links, abs(motorsnelheid));
       
        }
        if (error > 0 && error > -set) {
          analogWrite(motor1rechts, abs(motorsnelheid));
          analogWrite(motor1links, 0);
        }
      }

I think you can replace all of this with:

        if (motorsnelheid >= 0) {
          analogWrite(motor1rechts, 0);
          analogWrite(motor1links, motorsnelheid);
        } else {
          analogWrite(motor1rechts, -motorsnelheid);
          analogWrite(motor1links, 0);
       }
3

Stan forgot to specify that the cart when it reaches point x[i ] (first point is 400 mm) it has to pick up a package and when it reaches point x[i+1] (in this example 100 mm) it has to drop the package (it is basically an overhead crane). In the attached image is displayed what needs to happen to the motor speed at specific distances (a negative motor speed indicates that the motor spins in the opposite direction). The motor that is used is a Makeblock Model MB-81320 TT GEARED MOTOR DC6V/200RPM to control the direction of the motor a L293DNE H-bridge is used the motor will be connected to a 9 V battery.

motorspeed.jpg
motorspeed.jpg1047×443 16 KB

4

That's what I figured given the current sketch. The combination of constrain() and map() should get the desired speed curve.

void loop() {
  for (int i = 1; i < 6; i++) {
    GoToPosition(x[i]);
    // Add grabber stuff here?
  }
}

void GoToPosition(int position) {
  int error;
  while ((error  = position - sensor.getDistanceCM()) != 0) {
     int motorSpeed = map(constrain(error, -set, set), -set, set, 255, -255);
     if (motorSpeed >= 0)
      MotorForward(motorSpeed);
    else
      MotorBackward(-motorSpeed);
  }
}

void MotorForward(int speed) {
  analogWrite(motor1rechts, 0);
  analogWrite(motor1links, speed);
 }

void MotorBackward(int speed) {
  analogWrite(motor1rechts, speed);
  analogWrite(motor1links, 0);
  }
5

Note: You could get into a condition where the motor isn't getting enough power to move because it is very close to the desired position but not AT the desired position. If that happens your crane will get stuck waiting for the position to change while not driving the motor hard enough to change it. That is why people use a PI or PID controller instead of a simple P (Proportional) controller like we are implementing. The I (Integral) term will increase over time as long as the error term is non-zero. Eventually it grows large enough to overcome friction and the motor moves.

I don't expect this will be a problem because the position measurement is likely to fluctuate. At some point it is likely to fluctuate enough to either move the motor or match the current position.

6

Is somthing like this an acceptable pid controller for the motorspeed? the ultrasonic distance detector (HC-SR04) has an error of about 6 mm

#include <Servo.h>

Servo grijperservo;

//hoeken voor open en dicht doen servo
int opene = 110;
int dicht = 70;

int sensorvoor = 12;
int knop = 11;
long x[] = {0, 400, 100, 600, 150, 800}; //mm
long puls, afstand, afstandcal, s;

//h-brug
int enablepin1 = 4;
int motor1links = 3;
int motor1rechts = 2;

//set van waar die moet afremmen
int set = 100;

//error tussen gemeten en bepaalde waarde
long error = 1;

//voltage dat er naar de motor wordt gestuurd en dus de motorsnelheid beinvloed
long motorsnelheid;


void setup() {
  grijperservo.attach(10);

  Serial.begin(9600);
  pinMode(knop, INPUT);
  digitalWrite(knop, LOW);

  //h-brug
  pinMode(enablepin1, OUTPUT);
  pinMode(motor1links, OUTPUT);
  pinMode(motor1rechts, OUTPUT);


}

void loop() {
  Serial.println("baas");
  if (digitalRead(knop) == HIGH) {
    Serial.println("kaas");
    for (int i = 1; i < 6; i++) {
      error = 1;
      while (error != 0) {
        
        tijd = millis()
        
        //motor1 on
        digitalWrite(enablepin1, HIGH);
        afstand = afstandsensor(sensorvoor);
        Serial.println(afstand);

        //calculate PID
        error = x[i] - afstand;
        derror = (error - erroroud)/(tijd-tijdoud);
        ierror = error * (tijd - tijdoud) + ierroroud;

        //calculate output
        output = error + derror + ierror;
        
        //calculate motorspeed
        motorsnelheid = map(constrain(output, -set, set), -set, set, 255, -255);

        Serial.print("  s ");
        Serial.print(set);
        Serial.print("  e ");
        Serial.print(output);
        Serial.print("  v ");
        Serial.print(motorsnelheid);
        Serial.print("  i ");
        Serial.print(i);
        Serial.print("  x ");
        Serial.println(x[i]);


        //set old variables to new
        erroroud = error;
        tijdoud = tijd;
        ierroroud = ierror;
        
        if (motorsnelheid > 0) {
          analogWrite(motor1rechts, 0);
          analogWrite(motor1links, abs(motorsnelheid));
        }
        if (motorsnelheid < 0) {
          analogWrite(motor1rechts, abs(motorsnelheid));
          analogWrite(motor1links, 0);
        }
        //if error is between 1 an - 1 mm the error is acceptable
        if (output < 1 && output > -1) {
          digitalWrite(enablepin1, LOW);
          Serial.print("pickup");
          delay(1000);
        }
      }


    }
  }
}
//calculate distance to refference point
long afstandsensor(int ping) {
  for (int j = 0; j < 10; j++) {
    pinMode(ping, OUTPUT);
    digitalWrite(ping, HIGH);
    delayMicroseconds(5);
    digitalWrite(ping, LOW);
    //meet terugkerend signaal
    pinMode(ping, INPUT);
    puls = pulseIn(ping, HIGH);
    s = puls * (0.343 / 2);
    Serial.print("10 waarden ");
    Serial.println(s);
    afstandcal += s;
  }
  s = afstandcal / 10;
  delay(500);
  Serial.print("  gemiddelde ");
  Serial.println(s);
  afstandcal = 0;
  return s;
}
7

In the actual assignment we have to compensate for the sling motion so that the package does not sling to much when it reaches its final position. When the state space model is made the system can be made stable by moving its poles to a stable position.

      //distance - offset
      afstand = afstand - offset;

      //speed
      xdot = (afstand - afstandoud) / (tijd - tijdoud);

      //angle
      theta = asin((afstandsling -afstand)/touwlengte);

      //anglespeed
      thetadot = (theta - thetaoud)/(tijd - tijdoud);

      e1 = x[i] - afstand;
      e2 = 0 - xdot;
      e3 = 0 - hoek;
      e4 = 0 - thetadot;

      //motorforce
      Fm = k * (-k1 * e1 - k2 * e2 - k3 * e3 - k4 * e4);
      
      //calculate motorspeed from needed force on cart
      motorsnelheid=(Vnom/Ts)*(Fm*r+(Ts*xdot)/(wo*r));

      //als motorsnelheid positief is moet de kar naar voren rijden anders achteruit
      if (motorsnelheid > 0) {
        analogWrite(motor1rechts, 0);
        analogWrite(motor1links, abs(motorsnelheid));
      }
      if (motorsnelheid < 0) {
        analogWrite(motor1rechts, abs(motorsnelheid));
        analogWrite(motor1links, 0);
      }
      else {
      //get package
      }

In this code the variables k1 to k4 are the compensators calculated form moving the poles in matlab the variable k is an compenstor for when the cart does not reach its expected possition.

We cann't use this code for our project because the package does not only swing but slightly rotates and the powercable conecting to the sensors on the gripper mess up the whole system. So we chose to move the cart very slowly to its final position so the swinging of the package is compensated

8 
long x[] = {0, 400, 100, 600, 150, 800}; //mm //distances to stop motor eventually

Do you really need to piss away 2 bytes per element?

9

the data that we read from the sensor is also a long so we need to keep the data type the same

10

15108066:
the data that we read from the sensor is also a long so we need to keep the data type the same

You have a ping sensor that is capable of determining distances in excess of 327 meters?

The duration, in microseconds, that it takes the pulse to return does not correspond to needing a long to store the distance in.