#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  HTMotor)
#pragma config(Hubs,  S4, HTMotor,  none,     none,     none)
#pragma config(Sensor, S2,     Force,          sensorAnalog)
#pragma config(Sensor, S3,     HTSMUX,          sensorI2CCustom)
#pragma config(Motor,  motorB,          X_Axis_1,      tmotorNXT, PIDControl, reversed, encoder)
#pragma config(Motor,  motorC,          X_Axis_2,      tmotorNXT, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     Elevator,      tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     LeftDrive,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     RightDrive,    tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     Scissor_Lift,  tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_1,     Blank,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_2,     Helix,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C1_1,     Blue,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C1_2,     Red,           tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    Helix_Rot,            tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    Door,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    Flipper,              tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    Latch,                tServoStandard)
#pragma config(Servo,  srvo_S1_C2_6,    Latch2,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "C:\Program Files (x86)\Robomatter Inc\ROBOTC Development Environment\Sample Programs\NXT\3rd Party Sensor Drivers\drivers\hitechnic-irseeker-v2.h" //Include file for interpreting IR Sensor Data.
#include "C:\Program Files (x86)\Robomatter Inc\ROBOTC Development Environment\Sample Programs\NXT\3rd Party Sensor Drivers\drivers\hitechnic-force.h"
#include "C:\Program Files (x86)\Robomatter Inc\ROBOTC Development Environment\Sample Programs\NXT\3rd Party Sensor Drivers\drivers\hitechnic-sensormux.h"

const tMUXSensor Top_IR = msensor_S3_1;
const tMUXSensor Bottom_IR = msensor_S3_2;
const tMUXSensor US_Left = msensor_S3_3;
const tMUXSensor US_Right = msensor_S3_4;

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
int Tower_VAL = 0;
int Weight = 0;


int GetDirection()
{
      int UpperDir, LowerDir, CalcDir;
      UpperDir = HTIRS2readACDir(Top_IR);  //* UPPER Seeker
      LowerDir = HTIRS2readACDir(Bottom_IR);  //* LOWER Seeker
      switch(UpperDir)
      {
        case(0):
          if (LowerDir == 0) CalcDir =  0;
          if (LowerDir == 1) CalcDir = 95;
          break;
        case(9):
          if (LowerDir == 1) CalcDir = 90;
          if (LowerDir == 2) CalcDir = 85;
          break;
        case(8):
          if (LowerDir == 2) CalcDir = 80;
          if (LowerDir == 3) CalcDir = 75;
          break;
        case(7):
          if (LowerDir == 3) CalcDir = 70;
          if (LowerDir == 4) CalcDir = 65;
          break;
        case(6):
          if (LowerDir == 4) CalcDir = 60;
          if (LowerDir == 5) CalcDir = 55;
          break;
        case(5):
          if (LowerDir == 5) CalcDir = 50;
          if (LowerDir == 6) CalcDir = 45;
          break;
        case(4):
          if (LowerDir == 6) CalcDir = 40;
          if (LowerDir == 7) CalcDir = 35;
          break;
        case(3):
          if (LowerDir == 7) CalcDir = 30;
          if (LowerDir == 8) CalcDir = 25;
          break;
        case(2):
          if (LowerDir == 8) CalcDir = 20;
          if (LowerDir == 9) CalcDir = 15;
          break;
        case(1):
          if (LowerDir == 9) CalcDir = 10;
          if (LowerDir == 0) CalcDir = 05;
          break;
        default:
            CalcDir =0;
      }
      return(CalcDir);
}

**Error**:Expression does not fit parameter. Call to 'HTIRS2readACDir'. Parameter: 'tSensors link' is 'Top_IR' of type 'tMUXSensor'.
**Error**:Expression does not fit parameter. Call to 'HTIRS2readACDir'. Parameter: 'tSensors link' is 'Bottom_IR' of type 'tMUXSensor'.

/*!@addtogroup HiTechnic
 * @{
 * @defgroup htsmux Sensor MUX
 * HiTechnic Sensor MUX Sensor
 * @{
 */

/*
 * $Id: hitechnic-sensormux.h 123 2012-11-02 16:35:15Z xander $
 */

#ifndef __HTSMUX_H__
#define __HTSMUX_H__
/** \file hitechnic-sensormux.h
 * \brief Commonly used functions used by drivers.
 *
 * common.h provides a number of frequently used functions that are useful for writing
 * drivers.
 * License: You may use this code as you wish, provided you give credit where its due.
 *
 * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 3.54 AND HIGHER.
 *
 * Changelog:
 * - 0.1: Initial release, split off from common.h
 *
 * \author Xander Soldaat (xander_at_botbench.com)
 * \date 18 January 2011
 * \version 0.1
 */

/*!@addtogroup HiTechnic
 * @{
 * @defgroup htirs2 IR Seeker V2
 * HiTechnic IR Seeker V2
 * @{
 */

/*
 * $Id: hitechnic-irseeker-v2.h 123 2012-11-02 16:35:15Z xander $
 */

#ifndef __HTIRS2_H__
#define __HTIRS2_H__
/** \file hitechnic-irseeker-v2.h
 * \brief HiTechnic IR Seeker V2 driver
 *
 * hitechnic-irseeker-v2.h provides an API for the HiTechnic IR Seeker V2.
 *
 * Changelog:
 * - 0.1: Initial release
 * - 0.2: Added SMUX functions
 * - 0.3: All functions using tIntArray are now pass by reference.<br>
 *        HTIRS2_SMUXData removed
 * - 0.4: Removed all calls to ubyteToInt()<br>
 *        Replaced all functions that used SPORT/MPORT macros
 * - 0.5: Driver renamed to HTIRS2
 *
 * Credits:
 * - Big thanks to HiTechnic 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 3.54 AND HIGHER.
 * \author Xander Soldaat (xander_at_botbench.com)
 * \date 06 April 2010
 * \version 0.5
 * \example hitechnic-irseeker-v2-test1.c
 * \example hitechnic-irseeker-v2-test2.c
 * \example hitechnic-irseeker-v2-SMUX-test1.c
 */

#pragma config(Sensor, S1,     HTSMUX,              sensorI2CCustom)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*
 * $Id: hitechnic-irseeker-v2-SMUX-test1.c 123 2012-11-02 16:35:15Z xander $
 */

/**
 * hitechnic-irseeker-v2.h provides an API for the HiTechnic IR Seeker V2.  This program
 * demonstrates how to use that API.
 *
 * Changelog:
 * - 0.1: Initial release
 * - 0.2: Driver renamed to HTIRS2
 *
 * Credits:
 * - Big thanks to HiTechnic 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 3.54 AND HIGHER.
 * Xander Soldaat (xander_at_botbench.com)
 * Dick Swan
 * 06 April 2010
 * version 0.2
 */

#include "drivers/hitechnic-sensormux.h"
#include "drivers/hitechnic-irseeker-v2.h"

// The sensor is connected to the first port
// of the SMUX which is connected to the NXT port S1.
// To access that sensor, we must use msensor_S1_1.  If the sensor
// were connected to 3rd port of the SMUX connected to the NXT port S4,
// we would use msensor_S4_3

// Give the sensor a nice easy to use name
const tMUXSensor HTIRS2 = msensor_S1_1;

// global variables
string sTextLines[8];

// function prototypes
void displayText(int nLineNumber, const string cChar, int nValueDC, int nValueAC);
void displayInstructions();

// main task
task main ()
{
  int _dirDC = 0;
  int _dirAC = 0;
   int dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
   int acS1, acS2, acS3, acS4, acS5 = 0;
   string tmpString;

  // show the user what to do
  displayInstructions();

  eraseDisplay();
  for (int i = 0; i < 8; ++i)
    sTextLines[i] = "";

  // display the current DSP mode
  // When connected to a SMUX, the IR Seeker V2 can only be
  // used in 1200Hz mode.
  nxtDisplayTextLine(0, "      DC  1200");

  // The sensor is connected to the first port
  // of the SMUX which is connected to the NXT port S1.
  // To access that sensor, we must use msensor_S1_1.  If the sensor
  // were connected to 3rd port of the SMUX connected to the NXT port S4,
  // we would use msensor_S4_3

  while (true)
  {
    // Read the current non modulated signal direction
    _dirDC = HTIRS2readDCDir(HTIRS2);
    if (_dirDC < 0)
      break; // I2C read error occurred

    // read the current modulated signal direction
    _dirAC = HTIRS2readACDir(HTIRS2);
    if (_dirAC < 0)
      break; // I2C read error occurred

    // Read the individual signal strengths of the internal sensors
    // Do this for both unmodulated (DC) and modulated signals (AC)
    if (!HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5))
      break; // I2C read error occurred
    if (!HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 ))
      break; // I2C read error occurred

      displayText(1, "D", _dirDC, _dirAC);
      displayText(2, "0", dcS1, acS1);
      displayText(3, "1", dcS2, acS2);
      displayText(4, "2", dcS3, acS3);
      displayText(5, "3", dcS4, acS4);
      displayText(6, "4", dcS5, acS5);
    if (HTSMUXreadPowerStatus(HTSMUX))
      nxtDisplayTextLine(7, "Batt: bad");
    else
      nxtDisplayTextLine(7, "Batt: good");
  }
}

// Display the instructions to the user
void displayInstructions()
{
  nxtDisplayCenteredTextLine(0, "HiTechnic");
  nxtDisplayCenteredBigTextLine(1, "IRSeekr2");
  nxtDisplayCenteredTextLine(3, "SMUX Test");
  nxtDisplayCenteredTextLine(5, "Connect SMUX to");
  nxtDisplayCenteredTextLine(6, "S1 and sensor to");
  nxtDisplayCenteredTextLine(7, "SMUX Port 1");
  wait1Msec(2000);
}

// Minimize LCD screen flicker by only updating LCD when data has changed
void displayText(int nLineNumber, const string cChar, int nValueDC, int nValueAC)
{
  string sTemp;

  StringFormat(sTemp, "%4d  %4d", nValueDC, nValueAC);
  // Check if the new line is the same as the previous one
  // Only update screen if it's different.
  if (sTemp != sTextLines[nLineNumber])
  {
    string sTemp2;

    sTextLines[nLineNumber] = sTemp;
    StringFormat(sTemp2, "%s:  %s", cChar, sTemp);
    nxtDisplayTextLine(nLineNumber, sTemp2);
  }
}

/*
 * $Id: hitechnic-irseeker-v2-SMUX-test1.c 123 2012-11-02 16:35:15Z xander $
 */