View unanswered posts | View active topics It is currently Sat Oct 25, 2014 12:49 pm






Reply to topic  [ 17 posts ]  Go to page 1, 2  Next
NXT CNC 
Author Message
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
Post NXT CNC
Hello i have one problem, i hope i fill find here someone who can help me.


I cant find out function which would move my engines with different nmotorencoder value same time. The power of engines has to be different but i cant find any possibility how it would work properly. I just found out this funcion:

if(x!=0||y!=0)
{
if(abs(x)>abs(y))
{
speedx=SPEED;
t2=abs(x)/SPEED;
speedy=round(abs(y)/t2);
}
if(abs(x)<abs(y))
{
speedy=SPEED;
t2=abs(y)/SPEED;
speedx=round(abs(x)/t2);
}
x is nmotorencoder value for engine A
y is nmotorencoder value for engine B
this is calculating my speed of engines ( speedx is engine A speedy is engine B) SPEED is defined speed of first engine.

while(nMotorRunState[motorB] != runStateIdle || nMotorRunState[motorA] != runStateIdle)
{}
motor[motorA] = 0;
motor[motorB] = 0;
}
else
{
if (x!=0 )
{

while( nMotorRunState[motorA] != runStateIdle)
{}
motor[motorA] = 0;
}
if(y!=0 )
{
while( nMotorRunState[motorB] != runStateIdle)
{}
motor[motorB] = 0;
}
}
this function is than turning them on and off
The PROBLEM is that they never stop together everytime one of the motor stops first than the another and this i dont know how to solve.


Would be here somebody who could help me??
Thanks for all kind of help!


Wed Dec 04, 2013 12:24 pm
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post Re: NXT CNC
hi,
in general you will have to use a PID controller for this issue, the regulated variable in this case is the rotation speed which you manipulate by the pwm power.

Now given that
motA has to move 3000 degrees
motB has to move 1000 degrees.

The motor with the larger target has to be run faster, the other one proportionally slower
(the NXT motors usually are running 100 degrees/100ms at 100pwm without load).

motA has to be run e.g. a little less than 100%, e.g. about 75 (+/-) pwm to achieve constant 75 degrees/100ms
motB has to be run by 1/3 of that, i.e. about 25 pwm (+/-) to achieve constant 25 degrees/100ms

At the start a ramp-up to 75 (25) and at the end a ramp-down to 0 will make the movement smooth.

Because of inaccuracies of measured values and manipulated variables the relative ratio has to be re-adjusted from time to time.

I'm curious if RobotC provides such a PID speed controller which is compatible with the already existing setEncoderTarget function, otherwise you'll have also to adjust the target approximation by yourself to avoid jerking at the start and also particularly overshooting or jerking at the end.

ps:
your code will be better readable if you put it into code tags!

_________________
regards,
HaWe aka Ford
#define S sqrt(t+2*i*i)<2
#define F(a,b) for(a=0;a<b;++a)
float x,y,r,i,s,j,t,n;task main(){F(y,64){F(x,99){r=i=t=0;s=x/33-2;j=y/32-1;F(n,50&S){t=r*r-i*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}


Wed Dec 04, 2013 1:33 pm
Profile
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
Post Re: NXT CNC
Thank you really much for your answer!!

Code:
if(x!=0||y!=0)
{
if(abs(x)>abs(y))
{
speedx=SPEED;
t2=abs(x)/SPEED;
speedy=round(abs(y)/t2);
}
if(abs(x)<abs(y))
{
speedy=SPEED;
t2=abs(y)/SPEED;
speedx=round(abs(x)/t2);
}


I think exactly that what you said is this code or? i calculate time for which the motor with speedx reachs the target and than calculate the speedy which i need to reach target y for the same time.
Problem is that the movements aren't smooth.

what is the difference between pwm and normal power (like motor[motorA]=value)

and what should i take as ERROR by PID control. I cant imagine how to write that PID control for this problem. I tried but i am not able. Could you please give me some advice or some idea how to start write that PID code?

Thank you really much


Wed Dec 04, 2013 1:55 pm
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post Re: NXT CNC
pwm is power
and error is the difference between target speed and current speed.
The PID formula is the following (we had this issue just in a different forum)...

first 1 hint: in your PID regulation loop you should check the actual loop time, otherwise the regulation may run out of control.
In my code it's called dtime

dtime = CurrentTick() - clock; // get current (actual) loop time
clock = CurrentTick();

now the explanation for the PID formula :
the motor power (PWMpwr) is calculated by

error = (targetspeedvalue-currenspeedvalue)
errorintegral = (0.75*errorintegral) + error

PWMpwr= (P*error) + (I*errorintegral) + (D*(error-olderror))

the I part has to multiplied by a looptime factor
the D part has to be devided through the looptime factor,

so this is the more exact formula:

PWMpwr= (P *error)
+ (I *errorintegral)*looptime
+ (D *(error-olderror) )/looptime ;

to tune the PID values is not a simple task, it will take most of the development time.
I have written code for a encoder target approximation (not target speed, though!) of single motors but not for 2 synchronized motors yet - and just for NXC, not for RobotC.

_________________
regards,
HaWe aka Ford
#define S sqrt(t+2*i*i)<2
#define F(a,b) for(a=0;a<b;++a)
float x,y,r,i,s,j,t,n;task main(){F(y,64){F(x,99){r=i=t=0;s=x/33-2;j=y/32-1;F(n,50&S){t=r*r-i*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}


Wed Dec 04, 2013 2:05 pm
Profile
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
Post Re: NXT CNC
what is target speed??? is that calculated speed which it should have ?
and how should i get the current speed?
and what is loop time/looptime factor basically?

could you please post that your program? nxc is not problem i had been working in nxc co i know it a little and it could help me with the structure of program?

Thank you for your time!!!


Wed Dec 04, 2013 2:53 pm
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post Re: NXT CNC
target speed: yes, that calculated speed which it should have

you have a loop
you take the loop time since the last round like
Code:
looptime = CurrentTime() - oldtime; // get current (actual) loop time
oldtime= CurrentTime();


now you monitor the encoder values:
Code:
oldEncoder = newEncoder
newEncoder = EncoderValue(motA);


if you have the loop time then you can calculate the speed by
Code:
actualSpeed = (newEncoder - oldEncoder)*100ms/looptime // edited



the looptime factor may be equal to looptime or you can adjust it to a waiting period after you aplied the new calculated pwm:

Code:
PWMpwr= (P *error)+ (I *errorintegral)*looptimefactor + (D *(error-olderror) )/looptimefactor ;
motor[motorA]=PWMpwr;
msleep(20); // msleep: pseudo code for wait_milliseconds


In that case I would use looptimefactor=looptime/20
this only has one reason: to make the P,I,D factors more handsome (like 1, 1.5, 10 instead of 1, 0.075, 200)

a very very simplified program is this (just the PID regulation loop for motor_A).
But notice, that this code is for approximating an encoder target value, not a rotation speed value
Code:
task task_PID_A() {

  float PWMpwr, readold, errorold;
  long clock, dtime; // timer

  PID_A.read = (MotorRotationCount(OUT_A));
  PID_A.err = PID_A.target - PID_A.read; // error to target
  clock = CurrentTick();

  do {

    dtime = CurrentTick() - clock; // get current (actual) loop time
    clock = CurrentTick();

    PID_A.integr = (0.75 * PID_A.integr) + PID_A.err;

    PWMpwr= (P *PID_A.err) + (I *PID_A.integr)*dtime/20.0 + (D *(PID_A.err-errorold))/(dtime/20.0);

    PID_A.outp = round(PWMpwr);

    //**************************************************

    OnFwd(OUT_A, (PID_A.outp)); // action !

    Wait(20); // wait regulation time

    //**************************************************

    readold = PID_A.read; // save old sensor
    errorold = PID_A.err; // save old error

    PID_A.read = (MotorRotationCount(OUT_A)); // get new encoder value
    PID_A.err = PID_A.target-PID_A.read; // new error to target

  } while ((abs(PID_A.err)> 0 ); // target reached ?

  Off(OUT_A); // finished - brake motor

  PID_A.outp=0;

Wait(1);

}


as you see, I'm using a structure for all values stored to this PIDcontroller, each motor has it's own.

Now the whole code, even for fine tuning, for final approach and for continuous follow-up and for and monitoring everything on the nxt screen for visual control you'll find here:

http://sourceforge.net/apps/phpbb/mindboards/viewtopic.php?f=3&t=1850

HTH!

_________________
regards,
HaWe aka Ford
#define S sqrt(t+2*i*i)<2
#define F(a,b) for(a=0;a<b;++a)
float x,y,r,i,s,j,t,n;task main(){F(y,64){F(x,99){r=i=t=0;s=x/33-2;j=y/32-1;F(n,50&S){t=r*r-i*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}


Wed Dec 04, 2013 4:19 pm
Profile
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
Post Re: NXT CNC
Thank you i am getting understand it, i am just still not getting..What is looptime?Or what you mean under word loop?


Wed Dec 04, 2013 5:26 pm
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post Re: NXT CNC
I don't know if it's the correct English term - it's the time to run once through the do-while-loop, sort of laptime, in my code I also use dtime for delta_time

_________________
regards,
HaWe aka Ford
#define S sqrt(t+2*i*i)<2
#define F(a,b) for(a=0;a<b;++a)
float x,y,r,i,s,j,t,n;task main(){F(y,64){F(x,99){r=i=t=0;s=x/33-2;j=y/32-1;F(n,50&S){t=r*r-i*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}


Wed Dec 04, 2013 6:28 pm
Profile
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
Post Re: NXT CNC
I understand thank you but i dont understand what does it influence the looptime.

You mean something like this? Because here error is distance.
Code:
int Ta = 10;    //sampling time
int aTarget = 0;  //current motor Target
bool aWaiting = false; //we arrived at the Target
task maController()
{
  int pos = 0;   //current position
  int e = 0;  //current error
  int ealt = 0;  //old error
  float Kp = 2.2; //proportional gain
  float Kd = 0.5; //differential gain
  float y = 0;  //computed motor power
  int MotorNumber = 0;
  bFloatDuringInactiveMotorPWM = false;   //brake motors
  nSyncedMotors = synchNone;     //disable any sync
  nMotorPIDSpeedCtrl[MotorNumber] = mtrNoReg; //disable PID
  nMotorEncoder[MotorNumber] = 0;       //set encode to 0
  while(true)
  {
        wait1Msec(Ta);    //wait for the sampling time     
        aWaiting = false;        //we are moving
        pos = nMotorEncoder[MotorNumber]; //reading the current position
        e = -aTarget - pos;      //calculating the error to the target
        int d = e-ealt;   //calculate difference between old error
        y = Kp * e + Kd * (d)/Ta;   //calculate the motor power
ealt = e;          //set the current error as old error
if(abs(e) < 2) aWaiting = true;  //if we are less then 2 degree from the target we are finished
        motor[MotorNumber] = y; //set the motor power
  }
}


Thu Dec 05, 2013 7:28 am
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post Re: NXT CNC
the real "actual" time running through the loop is depending from cpu load, so it's not a constant but variable and thus has to be measured in each loop-through anew. After applying the new pwm you should wait a short time (e.g., 20ms) before going on in your loop.
To my experience the calculation of all values in that loop takes another 10-30ms depending on addtionally running tasks, so it's up to 30-50ms overall laptime (looptime).

edit: don't forget - if the RobotC function setEncoderTarget does not support varying rotation speed during the runtime you'll need to write 2 integrated different PID controllers by your own:
1 for the speed plus 1 for the target, and both for 2 motors in the same loop - that's quite challenging and anything but a simple task.

_________________
regards,
HaWe aka Ford
#define S sqrt(t+2*i*i)<2
#define F(a,b) for(a=0;a<b;++a)
float x,y,r,i,s,j,t,n;task main(){F(y,64){F(x,99){r=i=t=0;s=x/33-2;j=y/32-1;F(n,50&S){t=r*r-i*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}


Thu Dec 05, 2013 9:37 am
Profile
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
Post Re: NXT CNC
So basically this code which i posted is correct because is just PD control than i need to write I control for each motor?

but i will count error from position and depend on where i am i will set speed of each motor. My error cant be difference between current speed and target speed or?


Thu Dec 05, 2013 12:34 pm
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post Re: NXT CNC
you'll need to have all over 2 different sets of P,I,D (1 set for the target, 1 set for the speed),
you'll need to have 2 errors (1 for the target, 1 for the speed)
you'll need to have 2 errorIntegrals (1 for the target, 1 for the speed)
you have to combine both PIDs to get just 1 pwm to be aplied to have the correct speed but slow down at the end and avoid overshooting

and you need to have all of this twice, 1 for the faster motor and 1 for the slower motor and to make them run synchronized.
Quite tricky, IMO... but doable :)

_________________
regards,
HaWe aka Ford
#define S sqrt(t+2*i*i)<2
#define F(a,b) for(a=0;a<b;++a)
float x,y,r,i,s,j,t,n;task main(){F(y,64){F(x,99){r=i=t=0;s=x/33-2;j=y/32-1;F(n,50&S){t=r*r-i*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}


Last edited by Ford Prefect on Thu Dec 05, 2013 12:56 pm, edited 1 time in total.



Thu Dec 05, 2013 12:40 pm
Profile
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
Post Re: NXT CNC
yes i would say tricky! I will have to probably find another way how to control my cnc becuase i really dont know how would i programm this:)


Thu Dec 05, 2013 12:55 pm
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post Re: NXT CNC
I don't want to discourage you...:
for the long run, until you reach the vicinity of your target, you'd have to control by the speed-PID
and then from the vicinity of your target (i.e., the last few degrees) you'd run controlled by the target-PID

_________________
regards,
HaWe aka Ford
#define S sqrt(t+2*i*i)<2
#define F(a,b) for(a=0;a<b;++a)
float x,y,r,i,s,j,t,n;task main(){F(y,64){F(x,99){r=i=t=0;s=x/33-2;j=y/32-1;F(n,50&S){t=r*r-i*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}


Thu Dec 05, 2013 12:59 pm
Profile
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
Post Re: NXT CNC
I understand..but how can i get from motor his actual speed? The speed which should be i can calculate from relation which you said before.And the PID control for speed doesn't have already robotc? It is calculating every time power which he has to put in the motor to reach the speed which we want.

I want to say more, that i am beginner in the way of PID control I have never write any PID control, for me this is something completely new so sorry for some stupid questions.


Thu Dec 05, 2013 1:11 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 17 posts ]  Go to page 1, 2  Next

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.