Lego NXC Utilities

utils.c

// Handy functions for Lego Mindstorms, written in NXC.
// See the end of this file for a sample program that uses these functions.
// Author: James McGill, 2011
//
// To compile:
//    /nbc test.nxc -O=test.rxe -v=127
//
// To download to the Lego brick:
//    /nxtcom -U test.rxe

//////////////////////////////////////////////////////////////////////////////
// TOUCH SENSOR - CONNECT TO IN_3
//////////////////////////////////////////////////////////////////////////////

bool touch_sensor_value;

void InitTouchSensor() {
  SetSensor(IN_3, SENSOR_TOUCH);
}

void ReadTouchSensor() {
  touch_sensor_value = SensorBoolean(IN_3);
}

void PrintTouchSensor() {
  TextOut(0, LCD_LINE6, "Touch     = " );
  NumOut(80, LCD_LINE6, touch_sensor_value ? 1 : 0);
}

//////////////////////////////////////////////////////////////////////////////
// ULTRASONIC SENSOR - CONNECT TO IN_2
//////////////////////////////////////////////////////////////////////////////

unsigned int distance;

void InitUltrasonicSensor() {
  // NOTE(jmcgill): Sensor only works on IN1 or IN2
  SetSensorType(IN_2, SENSOR_TYPE_LOWSPEED_9V);
}

void ReadUltrasonicSensor() {
  distance = SensorUS(IN_2);
}

void PrintUltrasonicSensor() {
  TextOut(0, LCD_LINE7, "dist(cm) = " );
  NumOut(80, LCD_LINE7, distance);
}

//////////////////////////////////////////////////////////////////////////////
// COLOR SENSOR - CONNECT TO IN_1
//////////////////////////////////////////////////////////////////////////////

struct ColorState {
  int colorval;
  unsigned int raw[];
  unsigned int norm[];
  int scaled[];
};

// Names for common colors.
string debug_std_colors[] = {
    "Black", "Blue", "Green", "Yellow", "Red", "White"
};

string debug_color;
ColorState color_state;

void InitColorSensor() {
  SetSensorColorFull(IN_1);
}

void ReadColorSensor() {
  ReadSensorColorEx(
      IN_1, 
      color_state.colorval, 
      color_state.raw, 
      color_state.norm, 
      color_state.scaled);
}

void PrintColorSensor() {
  // Turn a number between 1 and 6 into a color name.
  debug_color = debug_std_colors[color_state.colorval - 1];

  unsigned int red = color_state.norm[0];
  unsigned int green = color_state.norm[1];
  unsigned int blue = color_state.norm[2];
  
  TextOut(0, LCD_LINE1, debug_color, true); // True clears the screen.
  TextOut(0, LCD_LINE2, "red      = " );
  NumOut (80, LCD_LINE2, red);
  TextOut(0, LCD_LINE3, "green    = " );
  NumOut (80, LCD_LINE3, green);
  TextOut(0, LCD_LINE4, "blue     = " );
  NumOut (80, LCD_LINE4, blue);
}

//////////////////////////////////////////////////////////////////////////////
// SAMPLE PROGRAM
//////////////////////////////////////////////////////////////////////////////

void InitAllSensors() {
  InitColorSensor();
  InitTouchSensor();
  InitUltrasonicSensor();
}

task main() {
  InitAllSensors();

  // Turn the motors on.
  OnFwd(OUT_A, 50);
  OnRev(OUT_B, 50);

  // Run forever - without this the program would finish too soon.
  // The code inside this function is run over and over.
  while (1) {
    ReadTouchSensor();
    ReadUltrasonicSensor();
    ReadColorSensor();

    // Remember to clear the screen to avoid garbage.
    ClearScreen();

    PrintColorSensor();
    PrintTouchSensor();
    PrintUltrasonicSensor();

    // Delay for a bit, to avoid the screen flickering.
    Wait(50);
  }
}