Developing NXT Balancing Robot with Gyro

[egutake]

Hi all,

I'm trying to develop NXT Balancing Robot with gyro sensor in NXC language.
gyro sensor : http://www.hitechnic.com/cgi-bin/commer ... ey=NGY1044
the sensor's output is round/sec.

I want to use the robot to educate undergraduate students.
In the NXC code, I want to use PID control to teach students how the controller works.

Dose anyone have example?
The example should be as simple as possible.
Though I have one example, it is too difficult for students.
example : http://www.hitechnic.com/blog/gyro-sensor/htway/

Thanks!

[HaWe]

hey there!

do you think you can teach your students coding a PID controller roughly like this (simplyfied) one...?
(it's just sort of framework which has to be adjusted)

Code: Select all

struct PIDstruct {
                 // custom target values
  long  target;                  // set target
  int   tarpwm;                  // motor target speed
                 // custom regulation parameters
  float P;                       // P: basic propotional to error
  float I;                       // I: integral: avoid perish
  float D;                       // D: derivative: avoid oscillating
  float precis;                  // error precision to target
  int   regtime;                 // PID loop time
  char  cont;                 // target: continue or hit once
                 // internal control variables
  char  runstate;                // monitors runstate
  int   outp;                    // PID control output value
  int   maxout;                  // max output (max motor pwr)
  long  read;                    // current sensor reading
  float err;                     // current error
  float integr;                  // integral of errors
  float cspeed;                  // current speed


} ;


PIDstruct PID_A, PID_B, PID_C;


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);

}

[gloomyandy]

Hi,
Just a few random thoughts and comments...

One of the things that makes a balancing robot like this complex (at least with a cheap gyro) is that you will have to deal with gyro offset and drift. The hitechnics code has stuff in it to deal with this as well as code to allow you to steer the robot etc. It also deals with the real world issue of trying to calculate a speed value when using small sample times and encoders that return relatively crude steps (which can result in big swings in the calculated velocity). Perhaps the way to tackle this is to start with a basic PID control loop and then allow students to discover the issues of offset and drift and to augment the code with solutions for these issues, then move on to allowing the addition of control of the the robot.

Another issue is that this HiTechnics control algorithm has more than one control point and error term (in this case it has the angle of the robot for balance and the position of the wheels to try and keep it in place) and when you add in the capability to control the robot you get even more. In effect there are two PID loops here. Add in that the classic derivative term is actually the value read directly from the sensor (angular velocity) and that the main process variable has to be obtained from this via integration, and that there is no obvious integral term and it starts to get a little difficult to actually spot the PID control code!

The problem is that to make the robot work well you will need a lot of the above (in one form or another), balancing robots are fun, but they do get complex (did I mention the issue with the backlash in the Lego motors...).

Andy

[nikzagvit]

I can advise this resource http://robotsquare.com/2012/02/13/tutor ... th-robotc/, project just uses the PID, but it is necessary to adapt the code for NXC.

[egutake]

Hi all,
Thank you for your response.

>doc-helmut
I will try to create as simple code as yours.
Thank you.

>gloomyandy
I'm sure that your advice is correct. Though whole code will be complex, I will try.
I'm planing that students will create part of whole code.

>nikzagvit
I will try to translate ROBOTC to NXC!

Thanks

[HaWe]

hey,
notice that in the first line you will need to change
PID_A.read = (MotorRotationCount(OUT_A)); // get new encoder value
by sth like
PID_A.read = Gyro_Angle; // get new gyro angle, calculated by rotation speed

also the while loop will have to run forever.
Don't hesitate to ask!

HTH,
share and enjoy!

[egutake]

Does anyone has idea about updating gyro drift easily during the robot moving?