/*
 * this software interfaces to our RCX compass sensor based on the
 * Dinsmore 1655 analog compass sensor.  It takes two readings from
 * our compass sensor to get a full compass reading.  This is needed
 * because the RCX can only provide enough power to run 1/2 of the
 * Disnore sensor at a time.
 *
 * The software signals the compass sensor to switch from 1/2 of the
 * Dinsmore part to the other by switching from active sensor (light)
 * to passive (touch) and back to active (light).  This toggles a
 * flip flop that controls power to the Dinsmore compass halves.
 */

/* We have to provide certain delays to the hardware to make sure
 * it can do its job properly.
 */

#define ON_OFF_DELAY 11  // passive to active delay
#define STABLE_DELAY  9  // power stablization delay

#define COMPASS_SENSOR SENSOR_1
#define UPPER_CROSS 957  // upper crossing RAW reading
#define LOWER_CROSS 598  // lower crossing RAW reading
#define GAP (UPPER_CROSS - LOWER_CROSS)

int angle; // the direction the compass is pointed.

sub read_compass()
{
  int t;
  int curve1, curve2;

  // The sensor is set to LIGHT (active) mode already
  // and providing readings for curve 1

  SetSensorMode(COMPASS_SENSOR, SENSOR_MODE_RAW);
  Wait(STABLE_DELAY);       // wait for power to stablize
  curve1 = COMPASS_SENSOR;  // get curve 1 reading

  // make the sensor read curve 2 by toggling flip flop

  SetSensorType(COMPASS_SENSOR, SENSOR_TYPE_TOUCH);
  Wait(ON_OFF_DELAY);       // make sure power on/off/on is detected
  SetSensorType(COMPASS_SENSOR, SENSOR_TYPE_LIGHT);

  SetSensorMode(COMPASS_SENSOR, SENSOR_MODE_RAW);
  Wait(STABLE_DELAY);       // wait for power to stablize
  curve2 = COMPASS_SENSOR;  // get curve 2 reading

  // switch back to curve 1 by toggling flip flop

  SetSensorType(COMPASS_SENSOR, SENSOR_TYPE_TOUCH);
  Wait(ON_OFF_DELAY);
  SetSensorType(COMPASS_SENSOR, SENSOR_TYPE_LIGHT);

  // determine which quadrant we're in

  if (curve1 >= UPPER_CROSS) {
    t = UPPER_CROSS - curve2;          // 45 to 135 degrees
  } else if (curve2 <= LOWER_CROSS) {
    t = UPPER_CROSS - curve1 + GAP;    // 135 to 225 degrees
  } else if (curve1 <= LOWER_CROSS) {
    t = curve2 - LOWER_CROSS + 2 * GAP;  // 225 to 315 degrees
  } else {
    t = curve1 - LOWER_CROSS + 3 * GAP;  // 315 to 45 degrees
  }
  angle = t;
}

// simple program to display angle on RCX LCD screen

task main()
{
  SetSensorType(COMPASS_SENSOR, SENSOR_TYPE_LIGHT);
  SetUserDisplay(angle,4);
  while (1) {
    read_compass();
  }
}