Here's our program as of yet:
#pragma config(Hubs, S1, HTMotor, HTMotor, HTServo, none)
#pragma config(Sensor, S2, magnetSensor, sensorHiTechnicMagnetic)
#pragma config(Sensor, S3, touchSensor, sensorTouch)
#pragma config(Sensor, S4, IRSensor, sensorHiTechnicIRSeeker1200)
#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorNormal, openLoop, reversed)
#pragma config(Motor, mtr_S1_C2_1, motorF, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C2_2, motorG, tmotorNormal, openLoop)
#pragma config(Servo, srvo_S1_C3_1, servo1, tServoStandard)
#pragma config(Servo, srvo_S1_C3_2, servo2, tServoNone)
#pragma config(Servo, srvo_S1_C3_3, servo3, tServoNone)
#pragma config(Servo, srvo_S1_C3_4, servo4, tServoNone)
#pragma config(Servo, srvo_S1_C3_5, servo5, tServoNone)
#pragma config(Servo, srvo_S1_C3_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "JoystickDriver.c"
//Deviation/Proximity of the Magnet Determines this value
#define MAGNET_THRESHOLD 5
//Value of the sensor when nothing is detected
#define NO_MAGNET_VALUE 654
//Value of the sensor when somthing is detected
#define MAGNET_VALUE 660
task main()
{
const int threshold = 20; /* Int 'threshold' will allow us to ignore low */
/* readings that keep our robot in perpetual motion. */
while(true)
{
getJoystickSettings(joystick);
// -- Control Motor D
if(abs(joystick.joy1_y1) > threshold) // If the left analog stick's Y-axis readings are either above or below the threshold:
{
motor[motorD] = joystick.joy1_y1; // Motor D is assigned a power level equal to the left analog stick's Y-axis reading.
}
else // Else if the readings are within the threshold:
{
motor[motorD] = 0; // Motor D is stopped with a power level of 0.
}
// -- Control Motor E
if(abs(joystick.joy1_y2) > threshold) // If the right analog stick's Y-axis readings are either above or below the threshold:
{
motor[motorE] = joystick.joy1_y2; // Motor E is assigned a power level equal to the right analog stick's Y-axis reading.
}
else // Else if the readings are within the threshold:
{
motor[motorE] = 0; // Motor E is stopped with a power level of 0.
}
// -- Control Motor G
if(abs(joystick.joy2_y1) > threshold) // If the right analog stick's Y-axis readings are either above or below the threshold:
{
motor[motorG] = joystick.joy2_y2; // Motor G is assigned a power level equal to the right analog stick's Y-axis reading.
}
else // Else if the readings are within the threshold:
{
motor[motorG] = 0; // Motor G is stopped with a power level of 0.
}
// -- Control Motor F
if(abs(joystick.joy2_y1) > threshold) // If the left analog stick's Y-axis readings are either above or below the threshold:
{
motor[motorF] = joystick.joy2_y1; // Motor F is assigned a power level equal to the left analog stick's Y-axis reading.
}
else // Else if the readings are within the threshold:
{
motor[motorF] = 0; // Motor F is stopped with a power level of 0.
}
/////////////////////////////////////////////////////////////////
// -- Control Magnet Sensor
/////////////////////////////////////////////////////////////////
nxtDisplayTextLine(1, "Magnet: %d", SensorValue[magnetSensor]); // display magnet sensor value
if(SensorValue[magnetSensor] > (NO_MAGNET_VALUE + MAGNET_THRESHOLD))
{
nxtDisplayTextLine(3, "Magnet(NorthTop)");
}
else if(SensorValue[magnetSensor] < (NO_MAGNET_VALUE - MAGNET_THRESHOLD))
{
nxtDisplayTextLine(3, "Magnet(SouthTop)");
}
else
{
nxtDisplayTextLine(3, "Nothing Detected");
}
//wait1Msec(25);
}
if(SensorValue[magnetSensor] > (NO_MAGNET_VALUE + MAGNET_THRESHOLD) || SensorValue[magnetSensor] < (NO_MAGNET_VALUE - MAGNET_THRESHOLD))
{
servo[servo1] = 0;
}
if(SensorValue[touchSensor] == 1)
{
servo[servo1] = 90;
}
return;}
We still need to program the IR beacon into the Autonomous, and we'll need help there, but here's our TeleOp. How is it?
Maximilian
Me and max are still trying to figure out why it would see the wall in autonomus. If you have any idea what is going on tell me thanks. 
