View unanswered posts | View active topics It is currently Sun Apr 20, 2014 7:05 am






Reply to topic  [ 8 posts ] 
Old NXT color sensor sample code? 
Author Message
Rookie

Joined: Wed Jan 25, 2012 5:54 pm
Posts: 6
Location: New Brighton, PA
Post Old NXT color sensor sample code?
Hi, I'm trying to get two Lego color sensors to proportionally follow a line, AND tell me what color it is seeing. I remember in ROBOTC 2.16.1, (the first version supporting the color sensor) there was a sample program that demonstrated this. It showed what color the sensor was seeing , plus it showed the raw peg counts the sensor was reading. Now that I have upgraded to 3.05, the sample program is VERY simple, just showing the color it is seeing.

Can anybody either give me the old sample code, or explain to me how to use the raw peg counts. (I can find no documentation on it) Any help will be greatly appreciated, THANKS!

--mr64bit

EDIT: sorry, my bad. They're not peg counts, it's the AtoD values. I am not sure what that stands for though, but I know it works! :bigthumb:

_________________
--mr64bit


Last edited by mr64bit on Tue Jan 31, 2012 1:15 pm, edited 1 time in total.



Wed Jan 25, 2012 6:26 pm
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3107
Location: Rotterdam, The Netherlands
Post Re: Old NXT color sensor sample code?
Hah, that's actually quite funny. You are the first to ask for a more complex colour sensor sample :)

I quickly installed 2.26 on my VM and plucked the .c file from it.

Enjoy :)

- Xander


Attachments:
ColorSensor - complex.c [11.17 KiB]
Downloaded 573 times

_________________
| 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 Jan 26, 2012 5:15 am
Profile WWW
Rookie

Joined: Wed Jan 25, 2012 5:54 pm
Posts: 6
Location: New Brighton, PA
Post Re: Old NXT color sensor sample code?
Wow, thanks a million! (now, if I can figure out how it works!)
Just a little background, I am doing this for ION's Mini Urban Challenge. http://www.ion.org/outreach/muc/ I am part of a four man team from my local Christian school. I wanted the peg count ability so that I could use a proportional line follower, AND tell which color the line is. That matters, because while most lines are white, there are some yellow lines, (meaning your robot has to go slower) and blue lines. (parking spots) Thanks again for the help, this will be invaluable!

--mr64bit

_________________
--mr64bit


Thu Jan 26, 2012 10:44 am
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3107
Location: Rotterdam, The Netherlands
Post Re: Old NXT color sensor sample code?
That looks like a pretty cool challenge! I wish you and your team all the luck :)

- 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 Jan 26, 2012 10:53 am
Profile WWW
Rookie

Joined: Wed Jan 25, 2012 5:54 pm
Posts: 6
Location: New Brighton, PA
Post Re: Old NXT color sensor sample code?
Well, here is my code so far. I got the light sensors working perfectly now, THANKS! I am a somewhat new programmer, so any tips/advice/ constructive criticism on compacting code, optimizing and combining and the like would be most appreciated.

Code:
#pragma config(Sensor, S1,     colorLeft,      sensorCOLORFULL)
#pragma config(Sensor, S2,     colorRight,     sensorCOLORFULL)
#pragma config(Sensor, S3,     sonarSensor,    sensorSONAR)
#pragma config(Motor,  motorB,          left,          tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  motorC,          right,         tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//global variables

int directions[20]; //just a large enough number, enough for 20 turns. the program will end when it comes to a "0"
//0**=start/end program;
//1**=straight;
//2**=left;
//3**=right;
//4**=left(loop);
//5**=right(loop);
//6**=parkingLeft;
//7**=parkingRight;
//*1*=follow left line;
//*2*=follow right line;
//**1,2,3,4= park in X number of parking places from the end. zero if not parking.
int dirIndex = 0; //which number in the array are we reading?
int whiteSpeed = 75; //in RPM
int yellowSpeed = 50;
int speedOutput = 50;
bool lineFollow = false;
float kp = 0.3;
float ki = 1;
float kd = 1;
int tp = 75;
int mean = 237;
float error = 0;
float turn = 0;
float integral = 0;
int integralReducer = 100;
int derivitiveReducer = 100;
float lastError = 0;
float derivitive = 0;
int powerLeft = 0;
int powerRight = 0;
int lineReading = 0;
int sonar = 0;
int dirTurn = 0;
int dirSide = 0;
int dirPark = 0;
int nAtoDValues[3]={0,0,0};
int red = 0, green = 0, blue = 0;
int stopSensor, lineSensor, lineColor;

void intDirections() //feed the directions into the array
{
  directions[0] = 110;
  directions[1] = 220;
  directions[2] = 420;
  directions[3] = 000;
  //.....
  //0**=start/end program;
  //1**=straight;
  //2**=left;
  //3**=right;
  //4**=left(loop);
  //5**=right(loop);
  //6**=parkingLeft;
  //7**=parkingRight;
  //*1*=follow left line;
  //*2*=follow right line;
  //**1,2,3,4 = park in X number of parking places from the end. zero if not parking.
  wait1Msec(100);
}

void jumpTo();
task line();
task sensors();
task motors();
void dirDecode();
void goStraight();
//void turnLeft();
//void turnRight();
//void turnLeftL();
//void turnRightL();

task main()
{
  intDirections();
  StartTask(sensors);
  StartTask(motors);
  jumpTo();
  StartTask(line);
  dirDecode();
  lineFollow = true;
  wait1Msec(100);
  while(true)
  {
    while(/*stopSensor < 200*/true)
    {
    }
    lineFollow = false;
    motor[left] = 0;
    motor[right] = 0;
    wait1Msec(100);
    motor[left] = 0;
    motor[right] = 0;
    dirIndex ++;
    dirDecode();
    //if(directions[dirIndex] == 000)
    //{
    //  StopAllTasks();
    //}
    //else {}
    //wait1Msec(250);
    switch(dirTurn)
    {
      case 1:  goStraight();  break;
      //case 2:  turnLeft();  break;
      //case 3:  turnRight();  break;
      //case 4:  turnLeftL();  break;
      //case 5:  turnRightL();  break;
    }
  }
}

void jumpTo() //a function to jump to a specific point in our directions
{
  while(nNxtButtonPressed != 3) //while the orange button is NOT pressed...
  {
    bNxtLCDStatusDisplay = true;
    if(dirIndex < 0) //to keep the index variable from going negative (no negative array slots)
    {
      dirIndex = 0;
    }
    if(nNxtButtonPressed == 1) //if the right button is pressed...
    {
      dirIndex ++; //increse the index variable by 1
    }
    if(nNxtButtonPressed == 2) // if the left button is pressed...
    {
      dirIndex --; //decrese the index variable by 1.
    }
    nxtDisplayCenteredTextLine(4, "%d", dirIndex); //display which slot we are at on the NXT's screen
    wait1Msec(250);
  }
  eraseDisplay();
  wait1Msec(500); //wait for half a second, so all the variables can "settle"
}

task line()
{
  while(true)
  {
    while(lineFollow == true)
    {
      error = lineReading - mean;
      integral = (integral + error) / integralReducer;
      derivitive = (error - lastError) / derivitiveReducer;
      turn = (kp * error) + (ki * integral) + (kd * derivitive);
      powerLeft = tp + turn;
      powerRight = tp - turn;
      lastError = error;
    }
    wait1Msec(100);
  }
}

void dirDecode()
{
  dirTurn = directions[dirIndex] / 100;
  dirSide = (directions[dirIndex] / 10) - (dirTurn * 10);
  dirPark = directions[dirIndex] - ((dirTurn * 100) + (dirSide * 10));
}

task sensors()
{
  while(true)
  {
    sonar = SensorValue(sonarSensor);
    switch(dirSide)
    {
      case 1:
      SensorType[colorLeft] = sensorCOLORFULL;
      SensorType[colorRight] = sensorCOLORRED;
      switch (SensorValue[colorLeft])
      {
        case BLACKCOLOR:    lineSensor = 1;     break;
        case BLUECOLOR:     lineSensor = 2;      break;
        case GREENCOLOR:    lineSensor = 3;     break;
        case YELLOWCOLOR:   lineSensor = 4;    break;
        case REDCOLOR:      lineSensor = 5;       break;
        case WHITECOLOR:    lineSensor = 6;     break;
        default:            lineSensor = 0;       break;
      }
      stopSensor = SensorRaw(colorRight);
      getColorSensorData(colorLeft, colorAtoD, nAtoDValues );
      break;

      case 2:
      SensorType[colorRight] = sensorCOLORFULL;
      SensorType[colorLeft] = sensorCOLORRED;
      switch (SensorValue[colorRight])
      {
        case BLACKCOLOR:    lineSensor = 1;     break;
        case BLUECOLOR:     lineSensor = 2;      break;
        case GREENCOLOR:    lineSensor = 3;     break;
        case YELLOWCOLOR:   lineSensor = 4;    break;
        case REDCOLOR:      lineSensor = 5;       break;
        case WHITECOLOR:    lineSensor = 6;     break;
        default:            lineSensor = 0;       break;
      }
      stopSensor = SensorRaw(colorLeft);
      getColorSensorData(colorRight, colorAtoD, nAtoDValues );
      break;
    }
    red = nAtoDValues[0];
    green = nAtoDValues[1];
    blue = nAtoDValues[2];
    if(lineSensor != 1)
    {
      lineColor = lineSensor;
    }
    switch(lineColor)
    {
      case 4:  lineReading = (red + green) / 2;  break;
      case 2: lineReading = blue;  break;
      default: lineReading = (red + green + blue) / 3;  break;
    }
  }
}

task motors()
{
  while(true)
  {
    motor[left] = powerLeft;
    motor[right] = powerRight;
  }
}

void goStraight()
{
  PlaySound(soundBeepBeep);
  wait1Msec(1000);
  motor[left] = 100;
  wait1Msec(3000);
  //nSyncedMotors = synchBC;
  //nSyncedTurnRatio = 100;
  //nMotorEncoder[left] = 0;
  //nMotorEncoderTarget[left] = 3600;
  //motor[left] =  speedOutput;
}


I am banging my head against something right now, :breakcomputer: can't get the program to get to "void goStraight". Ideas?

_________________
--mr64bit


Tue Jan 31, 2012 1:13 pm
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3107
Location: Rotterdam, The Netherlands
Post Re: Old NXT color sensor sample code?
Here are some tips:
Use a lot more comments than you do now. Don't comment the obvious like "button 2 pressed", rather explain what the function is doing. If a particularly tricky piece of code requires a little explanation, you can add some text there, too.
Below the pragmas, add a bit of text to tell the reader what your program is doing, I usually put something like this in my test programs:
Quote:
#pragma config(Sensor, S1, MSTMUX, sensorLightInactive)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

/*
* $Id: MSTMUX-test1.c 25 2010-05-11 13:18:02Z xander $
*/

/**
* MSTMUX-driver.h provides an API for the Mindsensors Touch Sensor MUX. This program
* demonstrates how to use that API.
*
* Changelog:
* - 0.1: Initial release
*
* 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 it's due.
*
* THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 2.00 AND HIGHER.
* Xander Soldaat (mightor_at_gmail.com)
* 11-May-2010
* version 0.1
*/

You may not have credits or a license, but you get the idea.
Don't use tasks unless you really, really need them. Your task sensors and motors are not necessary. A single task should be more than enough for your line follower:
Code:
while(true)
{
  read_sensors();
  check_where_line_is();
  calculate_motor_speeds();
  change_motor_speeds();
}

You have a while() at line 94 that stops your code from reaching anything below it.

So I would advise you go over your code again, make it all into a single task and simplify it greatly. KISS is your friend! (Keep It Simple & Stupid).

- 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]


Tue Jan 31, 2012 2:40 pm
Profile WWW
Rookie

Joined: Wed Jan 25, 2012 5:54 pm
Posts: 6
Location: New Brighton, PA
Post Re: Old NXT color sensor sample code?
Yeah, the while(true) was for debugging, as was several other lines that were commented out. Got those out now. (variable mix up) Yeah, I know I should be commenting more, but I find it easier to do after I finish at least a piece of code. I am doing the line by line commenting some places, because I am also teaching three even newer programmers. The motor task will have much more in it, for controlling the motor speeds using inputs from the speed limit and US sensor. How is the sensor task not necessary? The task Main is not repeating all of itself continually, so I either have it as a task, of put it in more than once. Or is there some other way?

_________________
--mr64bit


Tue Jan 31, 2012 10:09 pm
Profile WWW
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3107
Location: Rotterdam, The Netherlands
Post Re: Old NXT color sensor sample code?
The sensor task is not necessary because you can just read the sensors at the top of your loop and use those values. It takes less than a few ms to go through your entire loop, so you're not going to miss valuable information unless you're going at 1m/s.

You need to rework your program. You need a single while loop that does the things I put in my previous post. Trying to teach tasks to new programmers is not recommended as it's a very advanced topic and takes quite a bit of work to do correctly. So for now, keep your program simple and use a single task to do everything.

- 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]


Wed Feb 01, 2012 3:00 am
Profile WWW
Display posts from previous:  Sort by  
Reply to topic   [ 8 posts ] 

Who is online

Users browsing this forum: No registered users 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

Search for:
Jump to:  



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