Author 
Message 
jaruj9
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!=0y!=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 


Ford Prefect
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 rampup to 75 (25) and at the end a rampdown to 0 will make the movement smooth.
Because of inaccuracies of measured values and manipulated variables the relative ratio has to be readjusted 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/332;j=y/321;F(n,50&S){t=r*ri*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}

Wed Dec 04, 2013 1:33 pm 


jaruj9
Rookie
Joined: Tue Oct 01, 2013 10:23 am Posts: 13

Re: NXT CNC
Thank you really much for your answer!! 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 


Ford Prefect
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 = (targetspeedvaluecurrenspeedvalue) errorintegral = (0.75*errorintegral) + error
PWMpwr= (P*error) + (I*errorintegral) + (D*(errorolderror))
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 *(errorolderror) )/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/332;j=y/321;F(n,50&S){t=r*ri*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}

Wed Dec 04, 2013 2:05 pm 


jaruj9
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 


Ford Prefect
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 now you monitor the encoder values: if you have the loop time then you can calculate the speed by the looptime factor may be equal to looptime or you can adjust it to a waiting period after you aplied the new calculated pwm: 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 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 followup 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=1850HTH!
_________________ 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/332;j=y/321;F(n,50&S){t=r*ri*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}

Wed Dec 04, 2013 4:19 pm 


jaruj9
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 


Ford Prefect
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 dowhileloop, 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/332;j=y/321;F(n,50&S){t=r*ri*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}

Wed Dec 04, 2013 6:28 pm 


jaruj9
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.

Thu Dec 05, 2013 7:28 am 


Ford Prefect
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 loopthrough 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 1030ms depending on addtionally running tasks, so it's up to 3050ms 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/332;j=y/321;F(n,50&S){t=r*ri*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}

Thu Dec 05, 2013 9:37 am 


jaruj9
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 


Ford Prefect
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/332;j=y/321;F(n,50&S){t=r*ri*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 


jaruj9
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 


Ford Prefect
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 speedPID and then from the vicinity of your target (i.e., the last few degrees) you'd run controlled by the targetPID
_________________ 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/332;j=y/321;F(n,50&S){t=r*ri*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}

Thu Dec 05, 2013 12:59 pm 


jaruj9
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 


Who is online 
Users browsing this forum: jasontalk 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

