Mount Dispenser.c

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     ElevatorBottomStop,     sensorTouch)
#pragma config(Sensor, S3,     SMUX2,               sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX1,               sensorI2CCustom)
#pragma config(Motor,  motorA,          IndicatorLight, tmotorNormal, openLoop)
#pragma config(Motor,  motorB,          ,              tmotorNormal, PIDControl)
#pragma config(Motor,  motorC,          ,              tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C1_1,     Left,          tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     Right,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     Elevator,      tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C3_1,    Kicker,               tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C3_2,    Gate,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    Slide,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    RightStomper,         tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    LeftStomper,          tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


//  SMUX1
#define Magnet msensor_S4_4
#define Sonar msensor_S4_3
#define Accelerometer msensor_S4_2


//  SMUX2
#define ElevatorTopStop msensor_S3_1
#define IR msensor_S3_2
#define LeftLightSensor msensor_S3_3
#define RightLightSensor msensor_S3_4


#include "shared/include.c"
#include "Autonomous/include.c"


task main()
{
  SensorsInit();
  MechanismInit();

  /*
  while ( true )
  {
  }*/


  MechanismElevatorSetHeight(kElevatorHeightMidDispenser);

  while ( true )
  {
    nxtDisplayCenteredTextLine(0, (string)SonarSensorDistance());
    nxtDisplayCenteredTextLine(1, (string)IRSensorValue());
  }




  RobotMountCenterDispenser();

  PlaySound(soundUpwardTones);
  wait10Msec(100);
}