ROBOTC.net forums
http://www.robotc.net/forums/

LineLeader PID HELP!!!
http://www.robotc.net/forums/viewtopic.php?f=41&t=3740
Page 1 of 1

Author:  NeXT-Generation [ Wed Nov 16, 2011 10:41 pm ]
Post subject:  LineLeader PID HELP!!!

I am using the line following program supplied with the drivers, but it just isn't following the line!! I've tried tweaking everything!! The only modification I've made is motor and sensor configuration. Here is the code:
Code:
#pragma config(Sensor, S4,     LLEADER,             sensorI2CCustom9V)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*
 * $Id: MSLL-test3.c 65 2011-09-06 13:52:43Z xander $
 */

/** \file MSLL-test3.c
 * \brief Mindsensors LineLeader Sensor demo program
 *
 * MSLL-test3.c is a demo program for the Mindsensors LineLeader Sensor.
 *
 * Changelog:
 * - 0.1: Initial release
 * - 0.2: Reworked to use new driver API
 * - 0.3: More comments<br>
 *        Use clip() instead of manual clipping
 * - 0.4: Updated to use new array types that don't use structs\n
 *        Removed common.h from includes
 *
 * Credits:
 * - Big thanks to Mindsensors for providing me with the hardware necessary to write and test this.
 *
 * License: You may use this code as you wish, provided you give credit where its due.
 *
 * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 2.00 AND HIGHER.
 * Xander Soldaat (mightor_at_gmail.com)
 * 14 February 2011
 * version 0.4
 */

#define MSLL_I2C_ADDR 0x02

#include "drivers/MSLL-driver.h"

#define printXY nxtDisplayStringAt
#define println nxtDisplayTextLine
#define clearln nxtDisplayClearTextLine

// The PID constants and base speed
#define DEFAULT_KP    5
#define DEFAULT_KI    0
#define DEFAULT_KD   30
#define DEFAULT_SP   50

// The file for the log data
#define LOGFILE   "linelead.dat"

#define MENUITEMS 5

// function prototypes
void doMainMenu();
void doMenuItem(int activeOption);
bool checkTimer(TTimers timer);
void doLineLead();
void writeParams();

string menuHeader;
string menuFooter;

const TTimers rightButtonTimer = T1;
const TTimers leftButtonTimer  = T2;
const TTimers enterButtonTimer = T3;

int activeOption = 0;

int keep_running = 0;

byte sensor = 0;

// array holding default values for PID constants and base speed
const int default_params[4] = {
                              DEFAULT_KP,
                              DEFAULT_KI,
                              DEFAULT_KD,
                              DEFAULT_SP };

// array to hold user entered values
int params[4];

const string optionMainMenu[5] = {
                              "Kp",
                              "Ki",
                              "Kd",
                              "Sp",
                              "Run" };

const string optionMainMenuFooter[5] = {
                              "Ent=Edit",
                              "Ent=Edit",
                              "Ent=Edit",
                              "Ent=Edit",
                              "Ent=Run" };

tByteArray signalstr;


// This function draws the current values of the sensor and other data on the screen
// in a visually pleasing way
task drawSensors() {
  while (keep_running == 1) {
    // This clears the entire area occupied by the small rectangles
    nxtEraseRect(6,62, 91, 43);
    for (int i = 0; i < 8; i++) {
      // Draw the rectangles for the signal strength first
      nxtDrawRect(6+(i*11),62, 14+(i*11), 50);
      nxtFillRect(6+(i*11),51+signalstr[i]/10, 14+(i*11), 50);
      // Draw a black rectangle if the sensor has detected the line,
      // a hollow one when nothing has been detected.
      if ((sensor >> i) & 1) {
        nxtFillRect(6+(i*11),48, 14+(i*11), 43);
      } else {
        nxtDrawRect(6+(i*11),48, 14+(i*11), 43);
      }
    }
    wait1Msec(100);
  }
}


// Keep the robot on the line!
task followTheYellowBrickRoad () {
  int powerA = 0;
  int powerC = 0;
  byte steering = 0;

  eraseDisplay();
  nxtDisplayCenteredTextLine(3, "Running...");
  nxtDisplayCenteredTextLine(5, "Press exit");
  nxtDisplayCenteredTextLine(6, "to stop");

  time1[T4] = 0;
  while (keep_running == 1) {
    steering = LLreadSteering(LLEADER);
    sensor = LLreadResult(LLEADER);
    LLreadSensorRaw(LLEADER, signalstr);

    powerA = (params[3] + steering);
    powerC = (params[3] - steering);

    // If your robot is going in the wrong direction, comment out the
    // lines above and uncomment the lines below.
    //powerA = (params[3] - steering);
    //powerC = (params[3] + steering);

    // this clips the values
    powerA = clip(powerA, -100, 100);
    powerC = clip(powerC, -100, 100);

    //if(powerA>100)  powerA=100;
    //if(powerA<-100) powerA=-100;
    //if(powerC<-100) powerC=-100;
    //if(powerC>100)  powerC=100;

    motor[motorA] = (byte)powerA;
    motor[motorC] = (byte)powerC;
      wait1Msec(1);
      if (sensor != 0xFF) {
        time1[T4] = 0;
     } else if (time1[T4] > 500) {
       keep_running = 0;
     }
  }

  motor[motorA] = 0;
  motor[motorC] = 0;
}

task redrawMenu() {
  while(true) {
    eraseDisplay();
    println(0, menuHeader);
    for (int i = 0; i < MENUITEMS; i++) {
      clearln(i + 1);
      if (i == activeOption) {
        if (i < 4)
          println(i + 1, "> %s [%3d] <", optionMainMenu[i], params[i]);
        else
          println(i + 1, "> %s      <", optionMainMenu[i]);
        println(7, menuFooter);
      } else {
        if (i < 4)
          println(i + 1, "  %s [%3d]", optionMainMenu[i], params[i]);
        else
          println(i + 1, "  %s", optionMainMenu[i]);
      }
    }
    wait1Msec(100);
  }
}

// Main task
task main () {

  // Set the default parameters and write them to the sensor
  memcpy(params, default_params, sizeof(default_params));
  writeParams();

  nNxtButtonTask  = -2;
  nNxtExitClicks = 3;

  StartTask(redrawMenu);
  doMainMenu();
  while(true)
    wait1Msec(100);
}

// Draw the main menu
void doMainMenu () {
  while (true) {
    menuHeader = "L/R to select";
    menuFooter = optionMainMenuFooter[activeOption];

    switch(nNxtButtonPressed) {
      case kRightButton:
            if (!checkTimer(rightButtonTimer)) {
              break;
            }
            if (activeOption == (MENUITEMS - 1))
              activeOption = 0;
            else
              activeOption++;
            menuFooter = optionMainMenuFooter[activeOption];
            wait1Msec(300);
            break;
      case kLeftButton:
            if (!checkTimer(leftButtonTimer)) {
              break;
            }
            if (activeOption == 0)
              activeOption = (MENUITEMS - 1);
            else
              activeOption--;
            menuFooter = optionMainMenuFooter[activeOption];
            wait1Msec(300);
            break;
      case kEnterButton:
            if (!checkTimer(rightButtonTimer)) {
              break;
            }
            wait1Msec(600);
            doMenuItem(activeOption);
            break;
      case kExitButton:
            wait1Msec(500);
            StopAllTasks();

    }
  }
}

// When enter is pressed in the doMainMenu() we come here.
// Here we handle the actual actions for each menu item.
void doMenuItem(int activeOption) {
  PlaySound(soundBlip);
  while(bSoundActive) EndTimeSlice();

  if (activeOption == 4) {
    doLineLead();
    return;
  }

  while (true) {
    menuHeader = "L/R to edit val";
    menuFooter = "Ent=Save/Exit=Def";
    switch(nNxtButtonPressed) {
      case kRightButton:
            if (!checkTimer(rightButtonTimer)) {
              break;
            }
            if (params[activeOption] < 128)
              params[activeOption]++;
            break;
      case kLeftButton:
            if (!checkTimer(leftButtonTimer)) {
              break;
            }
            if (params[activeOption] > 0)
              params[activeOption]--;
            break;
      case kEnterButton:
            if (!checkTimer(enterButtonTimer)) {
              break;
            }
            writeParams();
            wait1Msec(600);
            return;
            break;
      case kExitButton:
            params[activeOption] = default_params[activeOption];
            writeParams();
            wait1Msec(600);
            break;
    }
  }
}

// See if more than 300ms has elapsed
bool checkTimer(TTimers timer) {
  if (time1[timer] < 300) {
    return false;
  } else {
    time1[timer] = 0;
    return true;
  }
}

// Start and stop the line following task (followTheYellowBrickRoad)
void doLineLead() {
  sensor = 0;
  StopTask(redrawMenu);
  keep_running = 1;
  for (int i = 0; i < 5; i++) {
    PlaySound(soundBlip);
    wait1Msec(600);
  }
  PlaySound(soundFastUpwardTones);
  while(bSoundActive) EndTimeSlice();
  StartTask(drawSensors);
  StartTask(followTheYellowBrickRoad);
  while(nNxtButtonPressed != kExitButton && keep_running != 0) {
    wait1Msec(10);
  }
  // this will kill off the followTheYellowBrickRoad task
  keep_running = 0;
  wait1Msec(1000);
  StartTask(redrawMenu);
}

// Write the PID values to the LineLeader sensor
void writeParams() {
  LLsetKp(LLEADER, params[0], 32);
  LLsetKi(LLEADER, params[1], 32);
  LLsetKd(LLEADER, params[2], 32);
}

/*
 * $Id: MSLL-test3.c 65 2011-09-06 13:52:43Z xander $
 */


I would like to get it going at 100% speed.

I'm including a LDD file of the robot.

Attachments:
File comment: The LineLeader should be oriented with the plug facing the robot. The wheels should be the ones from the 8051 Motorcycle. The other sensors will be implemented later.
Maze Solver.zip [22.74 KiB]
Downloaded 1224 times

Author:  mightor [ Thu Nov 17, 2011 2:18 am ]
Post subject:  Re: LineLeader PID HELP!!!

You did calibrate both black and white, right? You can do that with one of the other MSLL-test programs. I've never managed to make my line follower move any faster than about 60% or so but maybe I didn't try hard enough. There's nothing inherently wrong with the program as such, it really is a very simple program. To get those incredible speeds you're after, you're going to have to spend even more time getting the parameters just right. It's a very delicate balance between:
  • speed of the robot
  • tightness of the corners
  • wheel size
  • surface slipperiness
  • dust/dirt on your wheels
  • battery level
If any of these change even slightly when you're pushing the envelope, you'll find your robot flying out of the corner. If you've ever watched those micro-mouse videos of those tiny little robots flying through a maze, you'll see them furiously cleaning the wheels between runs and cleaning the track meticulously. This is not by accident.
Perhaps there is something you can do to modify the algorithm a little.

- Xander

Author:  magicode [ Thu Nov 17, 2011 3:29 pm ]
Post subject:  Re: LineLeader PID HELP!!!

There's no doubt that with the right algorithms, line following can be very fast: http://www.youtube.com/watch?v=AriuYTqxAMg

Author:  NeXT-Generation [ Sun Nov 20, 2011 10:38 am ]
Post subject:  Re: LineLeader PID HELP!!!

I just want it to follow a straight line. Can anyone give me a idea of what values to start with?

Author:  NeXT-Generation [ Sun Nov 20, 2011 12:15 pm ]
Post subject:  Re: LineLeader PID HELP!!!

Never mind. I figured it out. 100% SPEED :!: :!: :!: :!: :!: :!: :!: :poke: :bigthumb: :eek: :D :) :shock: :lol: :P :D :mrgreen:

Author:  mightor [ Sun Nov 20, 2011 3:54 pm ]
Post subject:  Re: LineLeader PID HELP!!!

So what parameters did you end up needing to use for the 100% speed?

- Xander

Author:  NeXT-Generation [ Sun Nov 20, 2011 9:43 pm ]
Post subject:  Re: LineLeader PID HELP!!!

It works decently with: P=16 I=0 and D=2. I also added something to the program. When it assigns the motor power, it multiplies the steering value by two, so that the effect is essentially the same as with 50% speed.

Author:  NeXT-Generation [ Sun Nov 20, 2011 9:47 pm ]
Post subject:  Re: LineLeader PID HELP!!!

Oops, forgot to mention that I lengthened the sensor mounting too.

Author:  NeXT-Generation [ Mon Nov 21, 2011 1:27 pm ]
Post subject:  Re: LineLeader PID HELP!!!

Could anyone tell me which part of the code is used to follow the line? I'm not really that experienced with RobotC yet, and can't really tell what I actually need to save.

Author:  mightor [ Mon Nov 21, 2011 2:09 pm ]
Post subject:  Re: LineLeader PID HELP!!!

It's this part:
Code:
// Keep the robot on the line!
task followTheYellowBrickRoad () {

Around line 155 or so.

- Xander

Page 1 of 1 All times are UTC - 5 hours [ DST ]
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
http://www.phpbb.com/