How I reduce vibration when tracking

bernard · 4 Juni 2017

Hi.

In fact in your 2 video the pid work each time for at least 2500 or 6000ms
I think the problem is in the result of the PID
The only way to be sure is to put the motorSpeedMaxPwm=160 the PerimeterTransitionTimeOut=5000 and add this console line after

setMotorPWM( max(-motorSpeedMaxPwm, min(motorSpeedMaxPwm, motorSpeedMaxPwm/2 - perimeterPID.y)),
max(-motorSpeedMaxPwm, min(motorSpeedMaxPwm, motorSpeedMaxPwm/2 + perimeterPID.y)), false);

Code:

Console.print(" MAG: ");
      Console.print (perimeterMag);
      Console.print("  ");
      Console.print(perimeterInside);
      Console.print(" PerimeterPID.y: ");
      Console.println(perimeterPID.y);

and verify in the console

If the result of perimeterPID.y change very faster or is superior at motorSpeedMaxPwm/2 then it's normal that one of the 2 wheels stop then so need to change the PID value in perimeter setting(not in motor setting)..

Hope you can find the good value.

alda · 4 Juni 2017

Hi,
I will test it.
But I'm sure, that must be possible calculate new PID values.

If we are able measure actual perimeter values in time and we know PID values, then must be possible recalculate it :

For all PID calculations must be a maxPWM still the same, then we can take a motor power as constant.
It's theory.

Alex

edit :
or :
Trial and Error Method

This is the simple method of tuning a PID controller. Once we get the clear understanding of PID parameters, the trial and error method become relatively easy.

Set integral and derivative terms to zero first and then increase the proportional gain until the output of the control loop oscillates at a constant rate. This increase of proportional gain should be in such that response the system becomes faster provided it should not make system unstable.

Once the P-response is fast enough, set the integral term, so that the oscillations will be gradually reduced. Change this I-value until the steady state error is reduced, but it may increase overshoot.

Once P and I parameters have been set to a desired values with minimal steady state error, increase the derivative gain until the system reacts quickly to its set point. Increasing derivative term decreases the overshoot of the controller response.

other methods :

so what I need now is values in time
Attachment: https://forum.ardumower.de/data/media/kunena/attachments/1183/PID3.jpg/

holoratte · 4 Juni 2017

then we should control tracking by perimeterMag not by perimeterInside:

Code:

//if (perimeterInside)
      //perimeterPID.x = -1;
    //else
      //perimeterPID.x = 1;
  perimeterPID.x = perimeterMag;

but the mag value will jump from + to - in no time when crossing the cable. Not sure that PID controll will like it... probabely same as perimeterInside or even worse. The controller will direct it away from the perimeter. The values getting lower...

maybe this could do it

Code:

if (perimeterInside)
      perimeterPID.x = -perimeterMag/permiterMagMax;
    else
      perimeterPID.x = perimeterMag/permiterMagMax;

  perimeterPID.w = 1;

one should know the permiterMagMax, which we don't...but can learn while driving.

ok that is almost same as the code from Bernard...

alda · 4 Juni 2017

one more question, can somebody tell me what is distance between coil and axis of tyres on the original mower for which is tracking SW writen ?

Attachment: https://forum.ardumower.de/data/media/kunena/attachments/1183/Tire_coil_distance.jpg/

roland · 4 Juni 2017

This was my pid linefollwoer code. But i don't use it anymore. I use also -1 and 1. In my robot negative values means outside.

Code:

class TFLfollowLine: public Node
{
private:

    double Setpoint, Amplitude, Output; // Pid setpoint, input und oputput
    double last_error, integral;
    int counter;

public:
    //PIDPOS myPID;
    double Kp, Ki, Kd;
    unsigned long nextTimeMotorPerimeterControl;

    TFLfollowLine() {
        Kp = 0.0021f; //0.003f 21
        Ki = 0.00013f; //0.00004f;0.000168f000247
        Kd = 0.00446f; //0.003f0.0065625f
        Setpoint = 0.0f;
        Amplitude = 0.0f;
        Output = 0.0f; // Pid setpoint, input und oputput

        nextTimeMotorPerimeterControl = 0;
        last_error=0;
        integral = 0;
    }

    virtual void onInitialize(Blackboard& bb) {
        Output = 0;
        Setpoint = bb.perimeterSensoren.magLineFollow;
        //myPID.Initialize();
        nextTimeMotorPerimeterControl = 0;
        last_error=0;
        integral = 0;
    }

    virtual NodeStatus onUpdate(Blackboard& bb) {
        if (millis() < nextTimeMotorPerimeterControl) return BH_RUNNING;
        nextTimeMotorPerimeterControl = millis() + 100;

        Amplitude =  bb.perimeterSensoren.magnetudeB;
        if(Amplitude < 0)
            Amplitude *= 1.34f; // 1.34f;1,5

        double error = Amplitude;
        double derivate = error-last_error;
        integral = integral + (Ki*error);

        //debug->printf("error: %frn",error);


        //Set integral to 0 if crossing the line
        if (sign0minus(error) != sign0minus(integral)) {
            integral = 0;  //sign0minus => 0 belongs to minus
        }

        //     debug->printf("trans: %lurn",millis()-bb.perimeterSensoren.perimeterLastTransitionTimeB);

        //      if(integral > bb.LINEFOLLOW_SPEED_LOW) integral = bb.LINEFOLLOW_SPEED_LOW;
        //      if(integral < -1*bb.LINEFOLLOW_SPEED_LOW) integral = -1*bb.LINEFOLLOW_SPEED_LOW;

        Output = Kp  * error + integral + Kd * derivate ;


        //if(millis()-bb.perimeterSensoren.perimeterLastTransitionTimeB > 900){
        //    Output *=1.3;
        //}


        //debug->printf("p:%f i%f d:%f ",Kp * error , integral, Kd * derivate);
        //debug->printf("output: %frn",Output);

        if(Output > bb.LINEFOLLOW_SPEED_LOW) Output = bb.LINEFOLLOW_SPEED_LOW;
        if(Output < -1*bb.LINEFOLLOW_SPEED_LOW) Output = -1*bb.LINEFOLLOW_SPEED_LOW;

        last_error = error;

        bb.cruiseSpeed = bb.LINEFOLLOW_SPEED_LOW;
        bb.driveDirection = DD_REVERSE_LINE_FOLLOW;

        if(error>0.0f) {
            bb.motor.L->setSpeed(-(bb.cruiseSpeed - Output));
            bb.motor.R->setSpeed(-(bb.cruiseSpeed));
        } else {
            bb.motor.L->setSpeed(-(bb.cruiseSpeed+5));
            bb.motor.R->setSpeed(-(bb.cruiseSpeed + Output));
        }


        return BH_RUNNING;
    }
};

This is my current code:

Code:

class TGotoAreaX: public Node
{
private:
    double integral;
    unsigned long lastRun;
    unsigned long lastTransitionTime;

public:
    double Ki;

    TGotoAreaX() {
        Ki = 0.7;
        lastRun = 0;
        integral = 0;
    }

    virtual void onInitialize(Blackboard& bb) {
        lastRun = 0;
        integral = 0;
        lastTransitionTime = millis();
        bb.motor.enablePerTrackRamping();
    }

    virtual NodeStatus onUpdate(Blackboard& bb) {


        if ( millis() - lastRun  < 100) return BH_RUNNING;
        lastRun = millis();


        // Calculate Speed
        //============================================
        double error =  bb.perimeterSensoren.magnetudeB;
        if(error <= 0) {
            error =-1;
        } else {
            error = 1;
        }

        integral = integral + (Ki*error);

        //debug->printf("error: %frn",error);

        //Set integral to 0 if crossing the line
        if (sign0minus(error) != sign0minus(integral)) { //sign0minus => 0 belongs to minus
            integral = 0;
            lastTransitionTime = millis();
        }

        //debug->printf("trans: %lurn",millis()-bb.perimeterSensoren.perimeterLastTransitionTimeB);

        double Output = integral ;

        //debug->printf("p:%f i%f d:%f o:%frn",Kp * error , integral, Kd * derivate, Output);

        if(Output > bb.LINEFOLLOW_SPEED_HIGH) Output = bb.LINEFOLLOW_SPEED_HIGH;
        if(Output < -1*bb.LINEFOLLOW_SPEED_HIGH) Output = -1*bb.LINEFOLLOW_SPEED_HIGH;


        bb.cruiseSpeed = bb.LINEFOLLOW_SPEED_HIGH;
        bb.driveDirection = DD_REVERSE_LINE_FOLLOW;

        if(error>0.0f) { //Set Speed Inside Perimeter
            if ( (millis()-lastTransitionTime) > 3300 ) { // If more than 3.5sec inside rotate full
                bb.cruiseSpeed = 15;
                bb.driveDirection = DD_ROTATECW;
                bb.motor.L->setSpeed(15);
                bb.motor.R->setSpeed(-15);
            } else if ( (millis()-lastTransitionTime) > 2800 ) { // If more than 2.8sec inside rotate more aggressive
                bb.motor.L->setSpeed(-(bb.cruiseSpeed - Output));
                bb.motor.R->setSpeed(-(bb.cruiseSpeed+10));
            } else if ( (millis()-lastTransitionTime) > 2000 ) { // If more than 2sec inside rotate aggressive
                bb.motor.L->setSpeed(-(bb.cruiseSpeed - Output));
                bb.motor.R->setSpeed(-(bb.cruiseSpeed+5));
            } else {
                bb.motor.L->setSpeed(-(bb.cruiseSpeed - Output));
                bb.motor.R->setSpeed(-(bb.cruiseSpeed));
            }

        } else { //Set Speed Outside Perimeter

            if ( (millis()-lastTransitionTime) > 1800 ) { // // If more than 2sec outside rotate full
                bb.cruiseSpeed = 15;
                bb.driveDirection = DD_ROTATECC;
                bb.motor.L->setSpeed(-15);
                bb.motor.R->setSpeed(15);
            } else if ( (millis()-lastTransitionTime) > 1500 ) { // If more than 1.5sec outside rotate more aggressive
                bb.motor.L->setSpeed(-(bb.cruiseSpeed+10));
                bb.motor.R->setSpeed(-(-15));
            } else if ( (millis()-lastTransitionTime) > 1000 ) { // If more than 1sec outside rotate aggressive
                bb.motor.L->setSpeed(-(bb.cruiseSpeed+10)); //50+10=60
                bb.motor.R->setSpeed(-(-8));  // -8
            } else {
                bb.motor.L->setSpeed(-(bb.cruiseSpeed+15)); //50+10=60
                bb.motor.R->setSpeed(-(bb.cruiseSpeed -25)); //50-25=25

            }
        }


        if ( (millis()-lastTransitionTime) > 12000 ){
            errorHandler.setError("!03,TGotoAreaX line crossing > 12000rn");
        }     
        return BH_RUNNING;
    }

    virtual void onTerminate(NodeStatus status, Blackboard& bb) {
        //bb.motor.enableDefaultRamping();
    }
};

holoratte · 4 Juni 2017

that would be approx. 16-17cm from wheel axis to the coil if the coil is most front. There is no fixed position of the coil. My setup is shown here:
http://www.ardumower.de/index.php/de/forum/schleifensensoren-generator/1025-probleme-mit-stoerungen-an-der-empfangsspule#10009

alda · 4 Juni 2017

Only for me as not so much skilled arduino user :

robot.cpp loop calls each cycle : else if (stateCurr == STATE_PERI_TRACK) motorControlPerimeter();

in motor.h is function motorControlPerimeter which will activate each 100mS.
if function motorControlPerimeter is activated and
if ((millis() > stateStartTime + 5000) && (millis() > perimeterLastTransitionTime + trackingPerimeterTransitionTimeOut))
then one motor is running and second is blocked or running counter clockwise (depends on setup).

if the condition is not met, then motor is controled via PID code which starts: if (perimeterInside)........

OK.
but for first condition :
if ((millis() > stateStartTime + 5000) && (millis() > perimeterLastTransitionTime + trackingPerimeterTransitionTimeOut))
millis is actual time in mS
trackingPerimeterTransitionTimeOut is defined as constant (default 2000 mS)
perimeterLastTransitionTime is defined in robot.cpp :
if ((perimeterUse) && (millis() >= nextTimePerimeter)){
nextTimePerimeter = millis() + 50; // 50
perimeterMag = readSensor(SEN_PERIM_LEFT);
if ((perimeter.isInside(0) != perimeterInside)){
perimeterCounter++;
perimeterLastTransitionTime = millis();
perimeterInside = perimeter.isInside(0);
}
So it's time when mower position changed from inside to outside or vice versa.

and stateStartTime is also defined in robot.cpp :
if (!buttonUse){
// robot has no ON/OFF button => start immediately
setNextState(STATE_FORWARD,0);
}
stateStartTime = millis();

And now my question, how is possible that this condition is fulfilled ?? :
if ((millis() > stateStartTime + 5000) && (millis() > perimeterLastTransitionTime + trackingPerimeterTransitionTimeOut))

Thanks for explanation

Alex

holoratte · 4 Juni 2017

stateStartTime is the time it startet tracking so earliest after 5s after finding perimeter... or should't that be stateTime?

alda · 4 Juni 2017

Right, I found, that new time is readed into stateStartTime always when state is changed, so
if ((millis() > stateStartTime + 5000) - valid after 5s when switched to tracking perimeter
and :
&& (millis() > perimeterLastTransitionTime + trackingPerimeterTransitionTimeOut))

perimeterLastTransitionTime - is time when mower position changed from inside to outside or vice versa.

Now it's clear for me, but I must make in house "test track" to see influence of each parameter change on the mower tracking.

Alex

edit:
sending again videos, old links doesn't work, but I'm not able edit my old posts :

Tracking with default settings:
https://youtu.be/Z7IJtUEeLNo
and with maxPWM=100:
https://youtu.be/vWwSEm9jLY4
But for further analysis is better first video. Here I can see on the start first problem, that after perimeter find mower turn too much and angle between mower and perimeter is too big (first two seconds) and in perimetr track PID isn't able go over perimeter in time and wheel is blocked.

Small skica :

and wire position:

Attachment: https://forum.ardumower.de/data/media/kunena/attachments/1183/peri.jpg/

bernard · 5 Juni 2017

Hi.
For me but need to be confirm ????
In the first video the 2 wheels rotate at different speed so the PID is called but the PID value are not perfect .
In the second video the PID is also called but you set maxpwm to 100 so maxpwm/2 egal 50 - perimeterPid.y the result is for example 25 and i think that with 25 the Wheel don't rotate.
In fact i never see a video of one ardumower and azurit 1.06 make a smooth tracking.If a member have one, i think he can give you the correct PID value and maybe a video .
The Roland mower tracking is perfect but if i understand correctly all his post he did not use arduino and Azurit and use 2 coil so it's different

What is the speed of your mower with pwm=255 , because never forget that if you want your mower moved on the wire at for example 0.4m/s it's 2cm each 50ms on one Wheel so the angle on your post 12436 change too faster for the pid to compute
With the mega the perimeter need 25 or 30 ms to be refresh so not enought to read IN and OUT transition at this speed.

Try this line

Code:

Console.print("Tracking ; ");
      Console.print(millis());
      Console.print("; MAG: ;");
      Console.print (perimeterMag);
      Console.print("; ;");
      Console.print(perimeterInside);
      Console.print("; PerimeterPID.x: ;");
      Console.print (perimeterPID.x);
      Console.print("; PerimeterPID.y: ;");
      Console.println(perimeterPID.y);

And normaly you are going to constate that the PerimeterPID.y is the main problem.
Good luck.

holoratte · 5 Juni 2017

Success! https://www.youtube.com/watch?v=4APKvuYYwbg&feature=youtu.be https://youtu.be/yBU4W0PLeLs

bernard · 5 Juni 2017

Hi.
PERFECT.
It's the first time i view a smooth perimetertracking.
What's your azurit version and the perimeterPid value ?
Very good.
by

holoratte · 5 Juni 2017

I have a fork on Github.
I will soon upload the changes to the code and will tell you more how its done.
Azurit1.07a-dev

alda · 5 Juni 2017

Very nice. Little bit slow but perfectly smooth. Is your solution only in speed adjustment in the code which you posted? PID setup is default?

holoratte · 5 Juni 2017

This is how it's done: i uploaded the relevant changes here included in my master branch of the fork. Ready for a pull request on request. I am running a slightly different customized version from SheepSheep ( also on Github ). Processor is Arduino Mega.
Tracking PID:
P = 4.4
I = 8.8
D=0
Transition timeout = 10000 -> no more needed..
Track error timeout = 10000
the control loop runs every 30ms as suggested by Bernard.
the PID.x value I get from perimeterMag divided by the maximum perimeterMag value of the last ~10s
PID.w is -1 when the Mower is inside and +1 when the mower is outside. So outside I am controlling for the positive maximum perimeterMag and inside for the negative maximum.
PID is reset when a transition occurs
pwmMax is lowerd for tracking only
finally, since perimeterMag is going to be close to 0 when the PID controller is doing a good Job, I had to switch off the timedoutBelowSMag for tracking.
here is the relevant part of the story in motor.h:

Code:

perimeterPID.x = 5*((double(perimeterMag)/double(perimeterMagMedian.getHighest())));
  if (perimeterInside){
      perimeterPID.w = -1;
      if (!lastPerimeterTrackInside) perimeterPID.reset();
      lastPerimeterTrackInside = 1;
  }

    else{
      perimeterPID.w = 1;
      if (lastPerimeterTrackInside) perimeterPID.reset();
      lastPerimeterTrackInside = 0;
    }

holoratte · 5 Juni 2017

I had no chance testing at higher speeds, maybe tomorrow
I have to say: I had no clue what I was doing until it worked and now I still don't fully understand why.:woohoo:

Best Chris

bernard · 5 Juni 2017

Hi.
For me it's normal that the result is more smooth with PerimeterMag as cible instead of 1 and -1 because it's the normal use of a PID control Pid.x is the realtime value .
What is Strange for me :
Why and How did you find the coeff 5* in

Code:

perimeterPID.x = 5*((double(perimeterMag)/double(perimeterMagMedian.getHighest())))

PID.w is -1 when the Mower is inside and +1 when the mower is outside

Also the Pid.w is the cible of the Pid control so why 1 and -1 and not 0 , for me 0 is when the mower is directly upper the wire.

Last year i make a video of tracking with PerimeterMag as cible and Arduino MEGA actualy i use a DUE and the result is the same but a little faster and it was for me the only way to have a smooth tracking with the Denna Platform (Distance between coil and motor is 450mm).
https://www.youtube.com/watch?v=GMfAO6OJyUI&feature=youtu.be
I think i'am going to try your code to view if it's better to use 1 and -1 instead of 0 and tell you the result.

Zum Vergrößern anklicken....

holoratte · 5 Juni 2017

Good morning Bernard,

I totally agree: PID's usually don't like fast changes (+1-1) that is why I reset them. I think this is an important point.
the coeff 5 is the result of a wild guess and some trial and error...actually now i think 4 would be worth a try for faster movement.
I am curious to see if that code works for others as well.
Actually I was expecting still more oscilation. I guess the large I-value is smoothing good....
I also tried PID.w = 0 procedure but I could not get it as stable.
Before starting to tune PID with the new code I already realized that is somthing very stable. It kind of locks in and wouldn' let go.
Then with tuning it's just smooth.
As said, I not yet fully understand how this system works, but it works....

Chris

holoratte · 5 Juni 2017

Bernard,

PID.w = 0 has 3 possible stable positions: 1 on top of the cable, which i thought is very difficult to hold and need to be very fast computed, and two far away from perimeter. so once you get over the maxima it will drift away.

PID.w = +-1 has two stable position (one for each condition) and thus is a stable system it will not drift away once out of tune.

Chris

alda · 5 Juni 2017

Nice, but first of all I must understand how complete PID regulation is working.
What isn't clear for me, why requested value is perimeterPID.w = -1; or perimeterPID.w = 1; why not perimeterPID.w = 0; ?

How I reduce vibration when tracking

Well-known member

Member

Member

Member

Active member

Member

Member

Member

Member

Well-known member

Member

Well-known member

Member

Member

Member

Member

Well-known member

Member

Member

Member