Robogals.c

// Handy functions for Mini Sumo, written in NBC
// See the end of this file for a sample program that uses these functions.
// Author: James McGill

// Functions for reading the touch sensors.
// Make sure you call InitTouchSensors once at the start of your program.

bool touch_one;
bool touch_two;

void InitTouchSensors() {
  SetSensor(IN_3, SENSOR_TOUCH);
  SetSensor(IN_4, SENSOR_TOUCH);
}

void ReadTouchSensors() {
  Acquire(touch_mutex);
  touch_one = SensorBoolean(IN_3);
  touch_two = SensorBoolean(IN_4);
  Release(touch_mutex);
}

void PrintTouchSensors() {
  Acquire(touch_mutex);
  bool one = touch_one;
  bool two = touch_two;
  Release(touch_mutex);

  TextOut(0, LCD_LINE6, "one     = " );
  NumOut(80, LCD_LINE6, one ? 1 : 0);
  TextOut(0, LCD_LINE7, "two    = " );
  NumOut(80, LCD_LINE7, two ? 1 : 0);
}

// Functions for reading the ultrasonic (range finding) sensor.
// Make sure you call InitUltrasonicSensor once at the start of your program.

mutex ultrasonic_mutex;
unsigned int distance;

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

void ReadUltrasonicSensor() {
  Acquire(ultrasonic_mutex);
  distance = SensorUS(IN_2);
  Release(ultrasonic_mutex); 
}

void PrintUltrasonicSesnor() {
  unsigned int dist;
  Acquire(ultrasonic_mutex);
  dist = distance;
  Release(ultrasonic_mutex); 
    
  TextOut(0, LCD_LINE5, "dist(cm) = " );
  NumOut(80, LCD_LINE5, dist);
}

// Functions for reading the color sensor. Make sure you call InitColorSensor
// once at the start of your program.
// This code assumes your color sensor is connected to IN_1. Change the code
// in InitColorSensor to change this.

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

// Remember to copy all of these.
string debug_std_colors[] = {"Black", "Blue", "Green", "Yellow", "Red", "White"};
string debug_color;
ColorState color_state;
mutex color_mutex;

void InitColorSensor() {
  SetSensorColorFull(IN_1);
}

void ReadColorSensor() {
  Acquire(color_mutex);
  ReadSensorColorEx(COLOR_PORT, 
                    color_state.colorval, 
                    color_state.raw, 
                    color_state.norm, 
                    color_state.scaled);
  Release(color_mutex);
}

void PrintColorSensor() {
    Acquire(color_mutex);
    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];
    Release(color_mutex);
  
    TextOut(0, LCD_LINE1, debug_color, true); // true here will clear 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 InitSensorState() {
  InitColorSensor();
  InitTouchSensors();
  InitUltrasonicSensor();
}

void ReadSensors() {
  ReadColorSensor();
  ReadTouchSensors();
  ReadUltrasonicSensor();
}

task main() {
  InitSensorState();
  
  while (true)
  {
    ReadSensors();

    // Work out what to do next.
    Wait(50);
  }
}