segway

[superethan5]

i am building a segway robot using a tilt sensor and a gyro sensor. i attempted to use a PID control system for controlling the balancing part of the program but that did not work. if anyone has done this before and has some tips for me that would be much appreciated

[gloomyandy]

Hi,
I've done this using a gyro and a (home made) accelerometer. The main control algorithm runs using PID. Mine works pretty well and will happily balance until the batteries run out. It also is pretty robust against pushing it by hand. How are you combining the two sensor outputs? I made use of a Kalman filter to fuse the two together which seems to work really well. The other thing I found that helps was to use the large Lego wheels (from the old Mindstorms/RCX kit), as this gives faster response. Oh and you also need to think about the sampling interval that you use. It needs to be long enough so that you can actually measure some rate of change, but fast enough to give you control... It is an interesting problem to solve, I spent a few days on it trying various combinations of control loop etc....

Have fun...

Andy

[superethan5]

i will try larger wheels. see if that helps. with my old program i was using just the accelerometer (HiTechnic one) the control loop. the biggest problem i had was that i could not tune the variables well enough for it to balance. it always either corrected to much and would throw it's self off balance or would never correct enough. also for making the loop time longer you sleep the system and if so how long.

[monkey525467]

I have never worked with a PID before, but I already know that I'm doing something wrong. Also, I haven't worked with the tilt sensor in java before, but none the less, I am trying to modify the original Sejway code. Here is what I have so far:

import lejos.nxt.*;

public class Sejway2 {

// PID constants
final int KP = 28;
final int KI = 4;
final int KD = 33;
final int SCALE = 18;

// Global vars:
int offset;
int prev_error;
float int_error;

TiltSensor ts;

public Sejway2() {
ts = new TiltSensor(SensorPort.S4);
Motor.B.regulateSpeed(false);
Motor.C.regulateSpeed(false);
}

public void getBalancePos() {
// Wait for user to balance and press orange button
while (!Button.ENTER.isPressed()) {
// NXT must be balanced.
offset = ts.getZTilt();
LCD.clear();
LCD.drawInt(offset, 2, 4);
LCD.refresh();
}
}

public void pidControl() {
while (!Button.ESCAPE.isPressed()) {
int normVal = ts.getZTilt();

// Proportional Error:
int error = normVal - offset;
// Adjust Y readings:
if (error < 0) error = (int)(error * 1.8F);

// Integral Error:
int_error = ((int_error + error) * 2)/3;

// Derivative Error:
int deriv_error = error - prev_error;
prev_error = error;

int pid_val = (int)(KP * error + KI * int_error + KD * deriv_error) / SCALE;

if (pid_val > 100)
pid_val = 100;
if (pid_val < -100)
pid_val = -100;

// Power derived from PID value:
int power = Math.abs(pid_val);
power = 55 + (power * 45) / 100; // NORMALIZE POWER
Motor.B.setPower(power);
Motor.C.setPower(power);

if (pid_val > 0) {
Motor.B.forward();
Motor.C.forward();
} else {
Motor.B.backward();
Motor.C.backward();
}
}
}

public void shutDown() {
// Shut down motors
Motor.B.flt();
Motor.C.flt();
}

public static void main(String[] args) {
Sejway2 sej = new Sejway2();
sej.getBalancePos();
sej.pidControl();
sej.shutDown();
}
}

If anyone knows what I'm doing wrong please reply,
(P. S., It's the HiTechnic tilt sensor)

[gloomyandy]

Hi,
Did you see....
http://lejos.sourceforge.net/forum/viewtopic.php?t=866

Andy

[monkey525467]

thanks for the the help

[esmetaman]

Another day I saw this PID Class
https://sonarmotion.dev.java.net/source ... a?rev=1.21

Code: Select all

package com.systronix.jcx;

import com.systronix.util.*;

import java.io.IOException;
/*  Graphical simuation code
import javax.swing.*;
import java.awt.*;
*/


/**
 * PID code developed for driving motors and following an object.
 *
 * @author Brandon Mansfield
 * @version 0.4
 * @see PID#VERSION
 * <UL>
 * <LI> 0.4 2006 april 18 Welding Torches - Added public attribute atCruiseSpeed. This value will keep track
 *       if PID loop is running at the default output.  This fixed wrong behavior where the robot would move
 *       away from objects.
 * <LI> 0.3 2006 april 08 Dustin Kopta, Chris Hansen - moved motor control to main method, main is only used
 *      for testing, look at main for an example of how to use this class
 * <LI> 0.2 revision by Chris Hansen - fixed issue where goal was reached, but motors were still being driven
 *  which resulted in oscillation within a certain distance of goal. Also, created MIN_OUTPUT (which may need
 *   to be tuned) so that we only drive the motors with enough power necessary to move, and cleaned up code.
 * <LI> 0.1 Code created to handle motor control with sonar data.  Tuning still required and may
 * be required for every bot as they often react differently.
 * </UL>
 * <P>PID loops are described <A HREF="http://en.wikipedia.org/wiki/PID">here</A> for further reference.</P>
 * <P>This code is designed to be mostly self contained, assign a goal distance and give it the appropriate
 * left and right ring buffers and it will initialize the motors, run in it's own thread (once started) and
 * try and seek and find objects.</P>
 *
 *   <P>Public methods allow the retrival of current motor values, getLeftOutput() and getRightOutput()</P>
 *
 * </UL>
 */

public class PID extends Thread
{
   /**
    * These constants will most likely need to be tuned on a per bot basis.
    */
   private static final float fractP = 0.5f;
   private static final float fractI = 0.1f;

   // Brandon's values
   //private static final float fractP = 0.4f;
   //private static final float fractI = 0.08f;
   
   
   /**
    * Period (in milliseconds) which to sleep in PID.run()
    */
   public static final int PID_PERIOD = 66;
   
   /**
    * Default "cruising" output level on a scale of 0 - 100.
    * This is output when no object is detected.
    */
   public static final int DEFAULT_OUTPUT = 50;
   
   /**
    * Any sonar reading (raw value) beyond this one will be ignored.
    * This was determined via testing.
    */
   private static final int maxRawValue = 500;

   /**
    * True iff thread is quitting. Changed with an external call to this.quit()
    */
   private boolean quitting;

   /**
    * Basically the "return value" of the PID loop.
    */
   private int output;

   /**
    *  Goal distance, default is 250mm. Stored internally as raw units.
    *  Only set with setGoal() so that conversion is done.
    */
   private int goal;
   
   private int data;

   private RingBuffer rbuffer = null;

   public PID(RingBuffer rbuf)
   {
      init();
      rbuffer = rbuf;
   }

   public void init()
   {
      output = 0;
      setGoal(250);
      quitting = false;      
   }

   /**
    *  Sets the goal for PID loops in mm.
    * 
    *  @param newGoal   new goal distance in mm.
    */
   public void setGoal(int newGoalMm)
   {
      goal = (newGoalMm * 200) / 318;
   }
   
   public void quit() {
      quitting = true;
   }
   
//   private static int convertToMm(int sonarVal)
//   {
//      return (sonarVal * 159) / 100;
//   }

   // get latest sample from the left RingBuffer
   //static int leftError()
   //{
      //return 0;
   //}

   // get the 4th sample from the right RingBuffer
//   private int lastError()
//   {
//      data = rbuffer.get(4);
//      if (data > 0)
//         return data-goal;
//      else
//         return 0;
//   }

   private int calcP(int error)
   {
      if (error == 0)
         return 0;
      else
         return (int)(error*fractP);
   }
   
   private int calcI(int error)
   {
      if (error == 0)
         return 0;
      else
         return (int)(error*fractI);
   }
   
   private void runPID()
   {
      int currentSample = rbuffer.get(0);
      
      // ignore values greater than maxRawValue
      if (currentSample >= maxRawValue) {
         output = DEFAULT_OUTPUT;
         return;
      }
      else // currentSample < maxSonarValue, do not ignore
         currentSample -= goal;
      
      //System.out.println("Current Error: " + currentSample);
      int sum = currentSample+rbuffer.get(1)-goal+rbuffer.get(2)-goal+rbuffer.get(3)-goal+rbuffer.get(4)-goal;

      output = calcP(currentSample) + calcI(sum);
      if (output > 100)
         output = 100;
      else if (output < -100)
         output = -100;
      
      //System.out.println("output: " + output + " , P = " + calcP(currentSample) + ", I = " + calcI(sum));
   }

   public void run()
   {
      if (rbuffer == null)
         return;
      
      while (!quitting) // loop until quit() is called
      {
         try
         {
            runPID();
            Util.waitWhile(PID_PERIOD);
         }
         catch (Exception e)
         {
            System.out.println("Exception in PID.run(): \n" + e.getMessage());
            e.printStackTrace();
         }
      }
   }

   /**
    * Gets the output which is updated periodically by PID.run()
    * This output can be used as power levels to drive a motor (negative values indicate reverse).
    * 
    * @return   int from -100 to 100 (inclusive) based on error
    */
   public int getOutput()
   {
      return output;
   }
   
   public void setBuffer(RingBuffer newBuffer)
   {
      rbuffer = newBuffer;
   }
   
   
   /**
    * Used for testing only
    * @param argv
    */
   public static void main(String argv[])
   {
      try {
      System.out.println("PID Main");   
         
      boolean ok = true;
      Sonar sonarModule0 = Sonar.getSonar0();
      Sonar sonarModule1 = Sonar.getSonar1();   
      
      RingBuffer right = Sonar.getRightBuffer();
      RingBuffer left = Sonar.getLeftBuffer();
      if (null==sonarModule0 || left==null) {
         System.out.println("Sonar0 setup ERROR!");
         ok = false;
         }
      if (null==sonarModule1 || right==null) {
         System.out.println("Sonar1 setup ERROR!");
         ok = false;
         }
      
      // if couldn't set up, can't use them, so bail out   
      if (!ok) return;

      System.out.println("Starting Sonar thread");      
   
      Sonar.startSonar();
      
      System.out.println("Initializing motors.");
      
      Motor leftMotor = null;
      Motor rightMotor = null;

      try {
         leftMotor  = new Motor(Motor.MOTOR_PORT_1,0,Motor.SPI_SLAVE_SELECT_0);
         rightMotor = new Motor(Motor.MOTOR_PORT_0,0,Motor.SPI_SLAVE_SELECT_0);
         leftMotor.setState(Motor.MOTOR_BRAKE);
         leftMotor.setState(Motor.MOTOR_BRAKE);
         leftMotor.setMaximumPowerLevel(100);
         rightMotor.setMaximumPowerLevel(100);
      }
      
      catch(IOException e)
      {
         System.out.println("**Unable to create motors****************************");
         System.out.println(e);
         System.out.println("*****************************************************");
         
         return; // cannot continue         
      }
      
      System.out.println("Creating PID threads.");
      PID leftPid = new PID(left);
      PID rightPid = new PID(right);
      System.out.println("pid threads created");
      
      System.out.println("calling pid.run()");
      leftPid.start();
      rightPid.start();
      System.out.println("done calling run");
      
      int leftOutput;
      int rightOutput;

      // print out distance and speed every second
      for (;;) {
         
         // Do not print these out more often than every 500ms
         System.out.println("LeftPID: " + leftPid.getOutput());
         System.out.println("RightPID: " + rightPid.getOutput());
         System.out.println("");
         
         // This is a Thread.sleep util from the SystronixUtils library
         //Util.waitWhile(500);
         
         
         leftOutput = leftPid.getOutput();
         rightOutput = rightPid.getOutput();
         
         if (leftOutput < 0) {
            leftMotor.setState(Motor.MOTOR_REVERSE);
            leftMotor.setPowerLevel(-1 * leftOutput);
         }
         else {
            leftMotor.setState(Motor.MOTOR_FORWARD);
            leftMotor.setPowerLevel(leftOutput);
         }
         
         if (rightOutput < 0) {
            rightMotor.setState(Motor.MOTOR_REVERSE);
            rightMotor.setPowerLevel(-1 * rightOutput);
         }
         else {
            rightMotor.setState(Motor.MOTOR_FORWARD);
            rightMotor.setPowerLevel(rightOutput);
         }
         
         Util.waitWhile(500);
         
      }

      /* Simulation code
      JFrame sonarControlGui = new JFrame();
      JPanel box = new JPanel();
      box.setLayout(new BoxLayout(box, BoxLayout.X_AXIS));

      JSlider leftJS = new JSlider(JSlider.VERTICAL, 20, 30, 25);
      leftJS.setPreferredSize(new Dimension(50, 200));
      box.add(leftJS);

      JSlider rightJS = new JSlider(JSlider.VERTICAL, 20, 30, 25);
      rightJS.setPreferredSize(new Dimension(50, 200));
      box.add(rightJS);

      sonarControlGui.add(box);
      sonarControlGui.setSize(new Dimension(30, 200));
      
      sonarControlGui.setVisible(true);
      while (true)
      {
         // Graphical simuation code
         //int rightSample = rightJS.getValue();
         //int leftSample = leftJS.getValue();
         

         System.out.println("output: " + pid.getOutput());
         try { Util.waitWhile(500); } catch (Exception e) {}
      }
      */
      }
      catch (Exception e) {
         e.printStackTrace();
      }
   }
}



[Julitza]

Hi! I'm tryiing to program a PID speed controller on my NXT and I'venever worked with this kind of controller. I'm measuring distance and depending on it my PID have conrol the speed, but I got confused because I don't know how to control the speed, I don't know if I have to control the power or the speed, I'm really confused, can someone help me?

I really appreciate your help.

Thanks

Julitza