[Team 2844]

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTServo)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Sensor, S2,     HTMAG,               sensorHiTechnicMagnetic)
#pragma config(Sensor, S3,     ,                    sensorLightActive)
#pragma config(Motor,  motorA,          motorA,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorB,          motorB,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          motorC,        tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     motorRight,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motorLeft,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorL,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorF,     tmotorNormal, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C3_1,    Mag1,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    lift2,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    leftflap3,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    rightflap4,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    MagR5,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    MagG,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C4_1,    fL7,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    fR8,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

bool btn2Pressed = false;
bool MagnetReleaseIsUp = false;

void initializeRobot()
{
  servoTarget[Mag1] = 127;
  servoTarget[lift2] = 0;
  servoTarget[leftflap3] = 23;
  servoTarget[rightflap4] = 112;
  servoTarget[fL7] = 0;
  servoTarget[fR8] = 0;
  servoTarget[MagR5] = 0;

  return;
}

void processMagnet()
{
    if(SensorValue[HTMAG] > 670) // Magnetic Ball
    {
      servoTarget[Mag1] = 230;
      wait1Msec(1000);
    }
    else if(SensorValue[HTMAG] < 660)// Magnetic Ball
    {
      servoTarget[Mag1] = 230;
      wait1Msec(1000);
    }
    else
    {
      servoTarget[Mag1] = 127;
    }
}

task main()
{
  initializeRobot();

  waitForStart();   // wait for start of tele-op phase

  while (true)
  {

    getJoystickSettings(joystick);
    processMagnet();
    motor[motorLeft] = joystick.joy1_y1;
    motor[motorRight] = joystick.joy1_y2;

In our case, we sort the magnetic ball by a servo. What we did was tap into the servo wires, negative and positive, and attached a LED to that. On our old robot we only determined the magnetic ball by a LED that was hooked into the motor port, really just acting like a motor turning off and on.

Up above you can see the beginning of our teleopcode, you need to have the void magnet before the while true like we have above. put the processMagnet in the while true like we have above too.

I know how you feel, it took me and another team forever last year to find out this.

Hope this helps.