How to drive straight with Robot using Optical Encoder Help.

[brightjoey]

I'm following this guide http://www.societyofrobots.com/sensors_encoder.shtml
 to use an optical encoder to drive straight, I'm using mbed as the microcontroller.
in order to drive straight forward, i have ticksA_new!=ticksA_old to represent when the encoder changes digital value 1 or 0.

Code: [Select]


#include "mbed.h"

Serial pc(USBTX, USBRX);
Serial ax500(p9,p10);
Timeout off;
int a,b,c;


void stop(){
int b=0;

      ax500.printf("!a00\r");
      wait_ms(10);
      ax500.printf("!b00\r");
      wait_ms(1000);
      b=1;
     
}

void forward(unsigned char speed_A,unsigned char speed_B){
char tick_A,tick_B,optical_A,optical_B;

    ax500.printf("!A%02x\r",speed_A);
    wait_ms(10);
    ax500.printf("!B%02x\r",speed_B);
   
if (tick_A!=tickA){
optical_A++;

if(tickB!=tickB){
optical_B++;
}
}
int main() {
int speed_A =60;
int speed B =60;


if speed A // from dec 0 --> 127(00-->7F)

  forward(speed);
  wait(5);
               
}


Do I need to use an interrupt for the optical encoders? Since they have to constantly measure and not be stuck on into a function.

Speed accepts 00 to 127 in dec which later converts to hex. How do I set the min max values for speed so it wont go of range.
I'm thinking of using PID like the link below but without a C program example It's too confusing to me.
http://www.societyofrobots.com/programming_PID.shtml

[mstacho]

Howdy,

First, yes, you need to use an interrupt for encoders, otherwise you'll miss ticks and you'll be screwed :-)

Second: PID code for C

Let's assume I'm reading a sensor in a variable int SENSOR_IN;  And that I want the device to go to location int SENSOR_LOC;

Code: [Select]

#define PGain 10
#define DGain 5
#define IGain 1 //You play with these to make it work

//Define global variables
int error = 0;
int dEr = 0;
int oldEr = 0;
int integrateEr = 0;
int SENSOR_LOC = 5; //5?  Why not?  Whatever you want it to be
int PID_OUT = 0; //This is what you use to compute your control command

main()
{
     ...do stuff, doesn't matter what.
     //READ SENSORS
     SENSOR_IN = readMySensor(); //this is whatever function you use to do it
     //update the errors
     error = SENSOR_LOC = SENSOR_IN;
     dEr = error - oldEr; //technically divide by dt, but this is handled by DGain
     integrateEr += error; //Add the error to the integrator
     oldEr = error; //vitally important, this step
     PID_OUT = PGain*error + DGain*dEr + IGain*integrateEr;

     //Now you set your limits.  Let's say we're going to put PID_OUT directly
    //into the motors as a PWM command, but we need to limit it tbetween      //0 and 127

    if (PID_OUT < 0)
             PID_OUT = 0;
    if(PID_OUT > 127)
             PID_OUT = 127;

   //set your speed using your function
    SET_SPEED(PID_OUT);

}

And that's about it.  Of course, you're goiung to have to modify it so that it works for your particular application and functions, but that's the gist. If it goes all wobbly, increase the DGain and decrease the PGain.  I almost never use an integrator, so start by setting IGain to zero.

**EDIT: Added code blocks

MIKE

[brightjoey]

The main problem is I don't know how to make both motors drive straight.

if I set speed to 60 for both, and If left motor is faster than right motor, then right motor speed++. But then right motor speed would be faster, so left motor have to ++. this continues again and again until both motor will reach the max speed 127. and no longer the originally 60 which is about half of the speed, which is not what i want.

How can I fix it in c program code cause I like an example of how they do this.

[mstacho]

Ultimately, what you want to do is not keep increasing until you get to the max speed, but keep TURNING until you're driving straight, right?

So let's try this (I make no assurances that this works.  In fact, I'm reasonable sure it should, but...)

If you have two encoders, ENCA and ENCB, on motor A and motor B, respectively, then modify the above code like this:

error = ENCA - ENCB;
//do all the PID computation

But here is the tricky part.  We're only going to be controlling speed on ONE of motors: the one attached to ENCB.  Why B?  Well if you look at the code again, it has error = SENSOR_LOC - SENSOR_IN.  In general, for PID, this is:

error = WHERE_I_WANT_TO_BE - WHERE_I_AM;

So we're using motor A as the reference.

NOW, if the PID output is negative, what that really means is that ENCB has gone through MORE ticks than ENCA.  It is moving faster.  So we have to *slow it down*.  If the PID output is positive, the opposite is true, so we have to *speed it up*.  All of this is relative to the speed setting on motor A, which is 60 or whatever.

How much do we slow it down or speed it up?  Well...that's up to you.  It should be proportional to the value in PID_OUT.  Maybe do something like:

SPEED_CHANGE = 0.1*PID_OUT; 

or something.  It REALLY depends on your particular system and the gains you choose, so it's another thing to play with.

What this will do is make sure that motor A and motor B are moving *at the same speed*.  Get this going, then try to make the robot turn.  Given how the above code works, how do you think you'll be able to do that?

MIKE

[jkerns]

The main problem is I don't know how to make both motors drive straight.

if I set speed to 60 for both, and If left motor is faster than right motor, then right motor speed++.

So far, so good.

But then right motor speed would be faster,

Faster than what? Faster than a target speed, or faster than too slow?

so left motor have to ++.

Why would you do that?

Control each motor to get the encoder reported speed to match a target value. Then it will go as straight as it can given differences in wheel diameter, slip, etc. If one motor is too slow, speed it up. If the other motor is too fast, slow it down.

[mstacho]

Yeah, that's a good point, I can't believe I missed that.  Instead of using error = ENCA - ENCB, just do error = SPEEDA - SPEEDB.

(the control theory purists will tell you that this is equivalent if you use a PI controller instead of a PD controller.  I have remarkably strong opinions on why, although that is mathematically true, it is practically irritating  )

MIKE

[brightjoey]

Okay, so what i do is I count the number of ticks for each encoder.

Then I use those values and put

error = ENCA-ENCB

SO lets say encA records 10 and encB records 5. Therefore value would be positive and SpeedB has to increase.
If error > 0
Speed B++

If encA records 6 and encB records 12. Therefore value would be negative and speedB has to decrease.

if error < 0
Speed B--

How do I say that if error is more or less 1 I want speed B to stop increasing.

[brightjoey]

But then right motor speed would be faster,

Faster than what? Faster than a target speed, or faster than too slow?

Control each motor to get the encoder reported speed to match a target value. Then it will go as straight as it can given differences in wheel diameter, slip, etc. If one motor is too slow, speed it up. If the other motor is too fast, slow it down.

Cause what I'm afraid is it will keep adding up until eventually the right motor will be faster than the leftmotor ,and then it will just snowball again and again until it reaches the max .

How do I get it to match a target value when theres no target value?

[mstacho]

You'd just use an IF statement. 

if(SOMETHING_IS_TRUE)
{
     DO_THING_A
}
else if(SOMETHING_ELSE_IS_TRUE)
{
       DO_THING_B
}
else
{
       DO_SOMETHING_DEFAULT
}

In your case, you need to check to see if error is between 0 and some value, and also if it's between the negative of that value and 0 (since you want to be able to stop doing things when error is "small", whether it's positive or negative.  If neither of those things are true (the "else" part), then do your control.

I should point out that just doing Speed-- or Speed++ isn't the BEST of ways to do this.  What you'll probably find is that the robot jerks back and forth a lot.  Let's say now that you have a maximum change in speed that you want the robot to go through.  say...10?  Then do this:

error = whateverItIs;
PID_OUT = All that computation above;


if(PID_OUT > 10)
    PID_OUT = 10;
if(PID_OUT < -10)
   PID_OUT = -10;

SpeedB = SpeedB + PID_OUT;

This takes care of that error less than 1 stuff, and you just tune your gains and you're done!  Notice that I didn't use an "else if" statement there.  I didn't need to (I could have, it wouldn't have made a difference).

Hope that helped!

mIKE

[jkerns]

But then right motor speed would be faster,

Faster than what? Faster than a target speed, or faster than too slow?

Control each motor to get the encoder reported speed to match a target value. Then it will go as straight as it can given differences in wheel diameter, slip, etc. If one motor is too slow, speed it up. If the other motor is too fast, slow it down.

Cause what I'm afraid is it will keep adding up until eventually the right motor will be faster than the leftmotor ,and then it will just snowball again and again until it reaches the max .

How do I get it to match a target value when theres no target value?

Ok, so your only control objective is to get one wheel to turn at the same speed as the other?

Then either:
       Control only one wheel - add and subtract as necessary until it matches the other wheel.
       Or, add to one wheel and subtract from the other wheel at the same time so that the average speed remains the same.

[brightjoey]

Yeap thanks I think I've got it.

Now it's just programming it right. Can I ask if my programming is right? I have a feeling I'm not passing my variables right. Speed_B doesn't seemed to wanna change. After I entered calc() a software interrupt for every second, It's suppose to change speed_B, but I don't see any difference.

Code: [Select]

#include "mbed.h"

Serial pc(USBTX, USBRX);
Serial ax500(p9,p10);
InterruptIn trigger_A(p26);
InterruptIn trigger_B(p25);
Ticker calculate;

//******************************************************************************************************************************************************************88
//Define global variables.
//Initialization.
int enc_A,enc_B=0;
char optical_A = 0;
char optical_B = 0;

int PGain=10;
int DGain=5;
int error = 0;
int dEr = 0;
int oldEr = 0;
int integrateEr = 0;
int SENSOR_LOC = 5; //5?  Why not?  Whatever you want it to be
int PID_OUT = 0;
int speed_B;
unsigned int distance=0;
//******************************************************************************************************************************************************************
void calc(){

error = enc_A - enc_B;
dEr = error - oldEr; //technically divide by dt, but this is handled by DGain
oldEr = error; //Copy error into oldEr
PID_OUT = PGain*error + DGain*dEr ;

if(PID_OUT > 10) // Why 10? Okay I'll try 10
{
PID_OUT = 10;
}
if(PID_OUT < -10)
{
PID_OUT = -10;
}

   speed_B = speed_B + PID_OUT;

}
void stop(){
calculate.detach();
distance=(enc_A+enc_B)/2*20; //Total ticks from both enc/2 multiply with 20mm
ax500.printf("!a00\r");
wait_ms(10);
ax500.printf("!b00\r");
wait_ms(1000);
  enc_A=enc_B=0;
}
void forward(unsigned char speed_A,unsigned char speed_B){

    if (speed_A>127)
{
speed_A=127;
}

if (speed_A<0)
{
speed_A=0;
}

else if (speed_B>127)
{
speed_B=127;
}

else if (speed_B<0)
{
speed_B=0;
}
ax500.printf("!A%02x\r",speed_A);
    wait_ms(10);
    ax500.printf("!B%02x\r",speed_B);
calculate.attach(&calc,1.0); //Software interrupt. Calls calc function every 1 s.

}

void tick_A(){
enc_A++;
}

void tick_B(){
enc_B++;
}
 
int main() { // from dec 0 --> 127(00-->7F)
int speed_A =60;
int speed_B =60;
trigger_A.rise(&tick_A);
trigger_B.rise(&tick_B);
   
   
  forward(speed_A,speed_B);
  wait(4);
  stop();
  wait(2);
  pc.printf("the distance is %u mm\r\n",distance);
             
}







[mstacho]

Howdy,

In your main function you overwrite speed_B.  You've already declared it as a global variable, so when you re-declare it in main, it overwrites and things screw up.  Also, are you SURE you're in calc?  Just checking: sometimes interrupts don't do what you think they do, and you have to make sure they are actually firing before you check your variables. :-)

MIKE