View unanswered posts | View active topics It is currently Mon Sep 24, 2018 4:04 am

 Page 1 of 2 [ 17 posts ] Go to page 1, 2  Next
 Print view Previous topic | Next topic
NXT CNC
Author Message
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
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
Guru

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
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
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
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)

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
Guru

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
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
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
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
Guru

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
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 timeoldtime= CurrentTime();

now you monitor the encoder values:
 Code:oldEncoder = newEncodernewEncoder = 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
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
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
Guru

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
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
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
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 timeint aTarget = 0;  //current motor Targetbool aWaiting = false; //we arrived at the Targettask 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 powerealt = e;          //set the current error as old errorif(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
Guru

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
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
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
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
Guru

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
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
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
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
Guru

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
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
Rookie

Joined: Tue Oct 01, 2013 10:23 am
Posts: 13
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
Display posts from previous:  Sort by
 Page 1 of 2 [ 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 forumYou cannot reply to topics in this forumYou cannot edit your posts in this forumYou cannot delete your posts in this forumYou cannot post attachments in this forum

Search for:
 Jump to:  Select a forum ------------------ ROBOTC Applications    ROBOTC for LEGO MINDSTORMS       Third-party sensors    ROBOTC for CORTEX & PIC    ROBOTC for VEX IQ    ROBOTC for Arduino    Robot Virtual Worlds    Multi-Robot Communications    Issues and Bugs Competitions & Partners    Mini Urban Challenge    CS2N Robot Virtual Worlds Competitions       VEX Skyrise Competition 2014-2015       VEX Toss Up 2013-2014       FTC Block Party! 2013-2014    Competitions using VEX - BEST, TSA, VEX, and RoboFest!    FTC Programming    RoboCup Junior and Other ROBOT Competitions Virtual Brick Robotics Discussions    General Discussions    Project Discussions Off-Topic ROBOTC Forum & ROBOTC.net Suggestions/Feedback    ROBOTC Forums Suggestions/Comments    ROBOTC.net Suggestions/Comments       NXT Programming: Tips for Beginning with ROBOTC       VEX Programming: Tips for Beginning with ROBOTC    2013 Robotics Summer Of Learning       VEX Toss Up Programming Challenge       FTC Ring It Up! Programming Challenge    International Forums       Spanish Forums          ROBOTC for MINDSTORMS          ROBOTC for VEX       French Forums          ROBOTC pour Mindstorms          ROBOTC pour IFI VEX       Japanese Forums （日本語のフォーラム）       German Forums    2015 Spring Carnival Event    PLTW (Project Lead The Way)    Robotics Merit Badge    2014 Robotics Academy Summer of Learning

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