View unanswered posts | View active topics It is currently Thu Oct 23, 2014 5:16 pm






Reply to topic  [ 10 posts ] 
LineLeader PID HELP!!! 
Author Message
Senior Roboticist
User avatar

Joined: Wed Sep 28, 2011 10:13 pm
Posts: 630
Location: If I told you, I'd have to kill you.
Post 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 549 times

_________________
A.K.A. inxt-generation
Self-proclaimed genius, and future world dominator.
My Brickshelf Folder
"Don't they teach recreational mathematics anymore?" - The Tenth Doctor
Bow down to Nikola Tesla, King of the Geek Gods.
Wed Nov 16, 2011 10:41 pm
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3227
Location: Rotterdam, The Netherlands
Post 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

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Thu Nov 17, 2011 2:18 am
Profile WWW
Moderator
Moderator
User avatar

Joined: Tue Sep 14, 2010 9:19 pm
Posts: 496
Post 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

_________________
sudo rm -rf /


Thu Nov 17, 2011 3:29 pm
Profile
Senior Roboticist
User avatar

Joined: Wed Sep 28, 2011 10:13 pm
Posts: 630
Location: If I told you, I'd have to kill you.
Post 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?

_________________
A.K.A. inxt-generation
Self-proclaimed genius, and future world dominator.
My Brickshelf Folder
"Don't they teach recreational mathematics anymore?" - The Tenth Doctor
Bow down to Nikola Tesla, King of the Geek Gods.


Sun Nov 20, 2011 10:38 am
Profile WWW
Senior Roboticist
User avatar

Joined: Wed Sep 28, 2011 10:13 pm
Posts: 630
Location: If I told you, I'd have to kill you.
Post Re: LineLeader PID HELP!!!
Never mind. I figured it out. 100% SPEED :!: :!: :!: :!: :!: :!: :!: :poke: :bigthumb: :eek: :D :) :shock: :lol: :P :D :mrgreen:

_________________
A.K.A. inxt-generation
Self-proclaimed genius, and future world dominator.
My Brickshelf Folder
"Don't they teach recreational mathematics anymore?" - The Tenth Doctor
Bow down to Nikola Tesla, King of the Geek Gods.


Sun Nov 20, 2011 12:15 pm
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3227
Location: Rotterdam, The Netherlands
Post Re: LineLeader PID HELP!!!
So what parameters did you end up needing to use for the 100% speed?

- Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Sun Nov 20, 2011 3:54 pm
Profile WWW
Senior Roboticist
User avatar

Joined: Wed Sep 28, 2011 10:13 pm
Posts: 630
Location: If I told you, I'd have to kill you.
Post 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.

_________________
A.K.A. inxt-generation
Self-proclaimed genius, and future world dominator.
My Brickshelf Folder
"Don't they teach recreational mathematics anymore?" - The Tenth Doctor
Bow down to Nikola Tesla, King of the Geek Gods.


Sun Nov 20, 2011 9:43 pm
Profile WWW
Senior Roboticist
User avatar

Joined: Wed Sep 28, 2011 10:13 pm
Posts: 630
Location: If I told you, I'd have to kill you.
Post Re: LineLeader PID HELP!!!
Oops, forgot to mention that I lengthened the sensor mounting too.

_________________
A.K.A. inxt-generation
Self-proclaimed genius, and future world dominator.
My Brickshelf Folder
"Don't they teach recreational mathematics anymore?" - The Tenth Doctor
Bow down to Nikola Tesla, King of the Geek Gods.


Sun Nov 20, 2011 9:47 pm
Profile WWW
Senior Roboticist
User avatar

Joined: Wed Sep 28, 2011 10:13 pm
Posts: 630
Location: If I told you, I'd have to kill you.
Post 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.

_________________
A.K.A. inxt-generation
Self-proclaimed genius, and future world dominator.
My Brickshelf Folder
"Don't they teach recreational mathematics anymore?" - The Tenth Doctor
Bow down to Nikola Tesla, King of the Geek Gods.


Mon Nov 21, 2011 1:27 pm
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3227
Location: Rotterdam, The Netherlands
Post Re: LineLeader PID HELP!!!
It's this part:
Code:
// Keep the robot on the line!
task followTheYellowBrickRoad () {

Around line 155 or so.

- Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Mon Nov 21, 2011 2:09 pm
Profile WWW
Display posts from previous:  Sort by  
Reply to topic   [ 10 posts ] 

Who is online

Users browsing this forum: No registered users and 1 guest


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

Search for:
Jump to:  



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