task main() {
int raw = 0;
int nrm = 0;
nNxtButtonTask = -2;
eraseDisplay();
nxtDisplayTextLine(0, "Dexter Industries");
nxtDisplayCenteredBigTextLine(1, "dFlex");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
eraseDisplay();
nxtDisplayTextLine(0, "dFlex Sensor");
while (true) {
nxtDisplayClearTextLine(5);
nxtDisplayClearTextLine(6);
// Get the raw value from the sensor
raw = DFLEXvalRaw(LEGOLS);
// Get the normalised value from the sensor
nrm = DFLEXvalNorm(LEGOLS);
nxtDisplayTextLine(2, "Raw: %4d", raw);
nxtDisplayTextLine(4, "Norm: %4d", nrm);
wait1Msec(50);
}
}
task main()
{
servo[topServo] = 235;
initializeRobot();
int _raw = 0;
int _processed = 0;
// Set the sensor to short range
HTEOPDsetShortRange(HTEOPD);
while(true){
// Read the raw sensor value
_raw = HTEOPDreadRaw(HTEOPD);
// read the processed value which is linear with
// the distance detected. Use the processed value
// when you want to determine distance to an object
_processed = HTEOPDreadProcessed(HTEOPD);
nxtDisplayClearTextLine(3);
nxtDisplayClearTextLine(4);
nxtDisplayTextLine(4, "Proc: %4d", _processed);
nxtDisplayTextLine(3, "Raw : %4d", _raw);
wait1Msec(50);
if (_processed > 41) {
LSsetActive(LEGOLS); // turn light on
PlaySound(soundBeepBeep);
while(bSoundActive){};
LSsetInactive(LEGOLS); // turn light off
} // if ball close
}
}
//
// DECIDE
//
task_r decide() {
nxtDisplayClearTextLine(1);
nxtDisplayClearTextLine(2);
nxtDisplayClearTextLine(3);
task_r cmd = FORAGE;
nxtDisplayCenteredBigTextLine(1,"FORAGE");
if(should_run_line_follow()){
cmd = LINE_FOLLOW;
nxtDisplayCenteredBigTextLine(1,"LN FOLLOW");
}
if(should_run_avoid()){
//Checks Sonar Sensor
cmd = AVOID;
nxtDisplayCenteredBigTextLine(1,"AVOID");
}
if(false){
//Checks if we need to observe
cmd = OBSERVE;
nxtDisplayCenteredBigTextLine(1,"OBSERVE");
}
return cmd;
}
/*
=============================================================================
main task with some testing code
*/
task main() {
int _raw = 0;
int _processed = 0;
// Standard range is set to short range
bool shortrange = true;
nNxtButtonTask = -2;
eraseDisplay();
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "EOPD");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Press enter to");
nxtDisplayCenteredTextLine(6, "switch between");
nxtDisplayCenteredTextLine(7, "ranges");
wait1Msec(2000);
eraseDisplay();
// Set the sensor to short range
HTEOPDsetShortRange(HTEOPD);
while(true) {
if (time1[T1] > 1000) {
if (shortrange == false) {
// set the sensor to short range and display this
HTEOPDsetShortRange(HTEOPD);
nxtDisplayClearTextLine(1);
nxtDisplayTextLine(1, "Short range");
shortrange = true;
} else {
// set the sensor to long range and display this
HTEOPDsetLongRange(HTEOPD);
nxtDisplayClearTextLine(1);
nxtDisplayTextLine(1, "Long range");
shortrange = false;
}
PlaySound(soundBeepBeep);
while(bSoundActive)
time1[T1] = 0;
}
while(nNxtButtonPressed != kEnterButton) {
// Read the raw sensor value
_raw = HTEOPDreadRaw(HTEOPD);
// read the processed value which is linear with
// the distance detected. Use the processed value
// when you want to determine distance to an object
_processed = HTEOPDreadProcessed(HTEOPD);
nxtDisplayClearTextLine(3);
nxtDisplayClearTextLine(4);
nxtDisplayTextLine(4, "Proc: %4d", _processed);
nxtDisplayTextLine(3, "Raw : %4d", _raw);
wait1Msec(50);
}
}
}
task main() {
int raw = 0;
int nrm = 0;
int tempRaw = 0;
int tempNrm = 0;
bool active = true;
// Turn the light on
LSsetActive(LEGOLS);
nNxtButtonTask = -2;
nxtDisplayCenteredTextLine(0, "Lego");
nxtDisplayCenteredBigTextLine(1, "LIGHT");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S2 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 2");
wait1Msec(2000);
nxtDisplayClearTextLine(7);
nxtDisplayTextLine(5, "Press [enter]");
nxtDisplayTextLine(6, "to toggle light");
wait1Msec(2000);
while (true) {
// The enter button has been pressed, switch
// to the other mode
if (nNxtButtonPressed == kEnterButton) {
active = !active;
if (!active)
LSsetInactive(LEGOLS);
else
LSsetActive(LEGOLS);
// wait 500ms to debounce the switch
wait1Msec(500);
}
raw = LSvalRaw(LEGOLS);
nrm = LSvalNorm(LEGOLS);
if (raw != tempRaw){
nxtDisplayClearTextLine(5);
nxtDisplayTextLine(5, "Raw: %4d", raw);
tempRaw = raw;
}
if (nrm != tempNrm){
nxtDisplayClearTextLine(6);
nxtDisplayTextLine(6, "Norm: %4d", nrm);
tempNrm = nrm;
}
wait1Msec(50);
}
}
task main () {
int raw = 0;
int nrm = 0;
// Get control over the buttons
nNxtButtonTask = -2;
eraseDisplay();
nxtDisplayTextLine(0, "Dexter Industries");
nxtDisplayCenteredBigTextLine(1, "dFlex");
nxtDisplayCenteredTextLine(3, "Test 2");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
eraseDisplay();
nxtDisplayTextLine(0, "dFlex Calibration");
nxtDisplayTextLine(2, "Left: set min");
nxtDisplayTextLine(3, "Right: set max");
nxtDisplayTextLine(7, "Grey: exit");
while (true) {
switch(nNxtButtonPressed) {
// if the left button is pressed calibrate the black value for the sensor
case kLeftButton:
DFLEXcalLow(DFLEX);
PlaySound(soundBeepBeep);
while(bSoundActive);
break;
// if the left button is pressed calibrate the white value for the sensor
case kRightButton:
DFLEXcalHigh(DFLEX);
PlaySound(soundBeepBeep);
while(bSoundActive);
break;
}
nxtDisplayClearTextLine(5);
nxtDisplayClearTextLine(6);
// Read the raw value of the sensor
raw = DFLEXvalRaw(DFLEX);
// Read the normalised value of the sensor
nrm = DFLEXvalNorm(DFLEX);
// Display the raw and normalised values
nxtDisplayTextLine(5, "R: %4d N: %4d", raw, nrm);
// Display the values for black and white
nxtDisplayTextLine(6, "B: %4d W: %4d", dflexlow, dflexhigh);
wait1Msec(50);
}
}
/*
=============================================================================
main task with some testing code
*/
task main() {
// Standard range is set to short range
bool shortrange = true;
tObstacleZone zone = 0;
nNxtButtonTask = -2;
eraseDisplay();
nxtDisplayCenteredTextLine(0, "Mindsensors");
nxtDisplayCenteredBigTextLine(1, "SUMO Eyes");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Press enter to");
nxtDisplayCenteredTextLine(6, "switch between");
nxtDisplayCenteredTextLine(7, "ranges");
wait1Msec(2000);
eraseDisplay();
// Set the sensor to short range
MSSUMOsetShortRange(HTMSSUMO);
while(true) {
if (time1[T1] > 1000) {
if (shortrange == false) {
// set the sensor to short range and display this
MSSUMOsetShortRange(HTMSSUMO);
nxtDisplayClearTextLine(1);
nxtDisplayTextLine(1, "Short range");
shortrange = true;
} else {
// set the sensor to long range and display this
MSSUMOsetLongRange(HTMSSUMO);
nxtDisplayClearTextLine(1);
nxtDisplayTextLine(1, "Long range");
shortrange = false;
}
PlaySound(soundBeepBeep);
while(bSoundActive)
time1[T1] = 0;
}
while(nNxtButtonPressed != kEnterButton) {
// Read the zone data
zone = MSSUMOreadZone(HTMSSUMO);
switch (zone) {
case MSSUMO_FRONT: nxtDisplayCenteredBigTextLine(4, "FRONT"); break;
case MSSUMO_LEFT: nxtDisplayCenteredBigTextLine(4, "LEFT"); break;
case MSSUMO_RIGHT: nxtDisplayCenteredBigTextLine(4, "RIGHT"); break;
case MSSUMO_NONE: nxtDisplayCenteredBigTextLine(4, "NONE"); break;
}
wait1Msec(50);
}
}
}
task main () {
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "GYRO");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
nxtDisplayCenteredTextLine(5, "Press enter");
nxtDisplayCenteredTextLine(6, "to set relative");
nxtDisplayCenteredTextLine(7, "heading");
wait1Msec(2000);
// Before using the SMUX, you need to initialise the driver
HTSMUXinit();
// Tell the SMUX to scan its ports for connected sensors
HTSMUXscanPorts(HTSMUX);
// 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
wait1Msec(2000);
eraseDisplay();
time1[T1] = 0;
while(true) {
if (time1[T1] > 1000) {
eraseDisplay();
nxtDisplayTextLine(1, "Resetting");
nxtDisplayTextLine(1, "heading");
wait1Msec(500);
// Start the calibration and display the offset
nxtDisplayTextLine(2, "Offset: %4d", HTGYROstartCal(msensor_S1_1));
PlaySound(soundBlip);
while(bSoundActive);
time1[T1] = 0;
}
while(nNxtButtonPressed != kEnterButton) {
eraseDisplay();
nxtDisplayTextLine(1, "Reading");
// Read the current calibration offset and display it
nxtDisplayTextLine(2, "Offset: %4d", HTGYROreadCal(msensor_S1_1));
nxtDisplayClearTextLine(4);
// Read the current rotational speed and display it
nxtDisplayTextLine(4, "Gyro: %4d", HTGYROreadRot(msensor_S1_1));
nxtDisplayTextLine(6, "Press enter");
nxtDisplayTextLine(7, "to recalibrate");
wait1Msec(100);
}
}
}
task main () {
int raw = 0;
int nrm = 0;
// Get control over the buttons
nNxtButtonTask = -2;
LSsetActive(LEGOLS);
eraseDisplay();
nxtDisplayTextLine(0, "Light Sensor Cal.");
nxtDisplayTextLine(2, "Left: set black");
nxtDisplayTextLine(3, "Right: set white");
nxtDisplayTextLine(7, "Grey: exit");
while (true) {
switch(nNxtButtonPressed) {
// if the left button is pressed calibrate the black value for the sensor
case kLeftButton:
LScalLow(LEGOLS);
PlaySound(soundBeepBeep);
while(bSoundActive);
break;
// if the left button is pressed calibrate the white value for the sensor
case kRightButton:
LScalHigh(LEGOLS);
PlaySound(soundBeepBeep);
while(bSoundActive);
break;
}
nxtDisplayClearTextLine(5);
nxtDisplayClearTextLine(6);
// Read the raw value of the sensor
raw = LSvalRaw(LEGOLS);
// Read the normalised value of the sensor
nrm = LSvalNorm(LEGOLS);
// Display the raw and normalised values
nxtDisplayTextLine(5, "R: %4d N: %4d", raw, nrm);
// Display the values for black and white
nxtDisplayTextLine(6, "B: %4d W: %4d", lslow, lshigh);
wait1Msec(50);
}
}
//#include "FTC_Comp_Include.c"
task main()
{
while(true) //continously loop to check for "mode" switching
{
getJoystickSettings(joystick); //update joystick values from FMS
//if (joystick.UserMode == false) //While in "autonomous" mode (UserMode = false)
if (0 == joystick.UserMode) //While in "autonomous" mode (UserMode = false)
//if (false == (short) joystick.UserMode) //While in "autonomous" mode (UserMode = false)
//if (!joystick.UserMode) //While in "autonomous" mode (UserMode = false)
{
//do autonomous
if( joystick.StopPgm == true )
{
// we are disabled
nxtDisplayClearTextLine( 4 );
nxtDisplayTextLine( 4, "Disabled Autonomous" );
}
else
{
// we are enabled
nxtDisplayClearTextLine( 4 );
nxtDisplayTextLine( 4, "Running Autonomous" );
}
}
else //While in "User Control" mode (UserMode = false)
{
//do usercontrol
if( joystick.StopPgm == true )
{
// we are disabled
nxtDisplayClearTextLine( 4 );
nxtDisplayTextLine( 4, "Disabled Teleop" );
}
else
{
// we are enabled
nxtDisplayClearTextLine( 4 );
nxtDisplayTextLine( 4, "Running Teleop" );
}
}
// wait for next packet
wait1Msec( 50 );
}
}
task main() {
int raw = 0;
int nrm = 0;
bool active = true;
LSsetActive(LEGOLS);
nNxtButtonTask = -2;
eraseDisplay();
nxtDisplayTextLine(0, "Light Sensor");
nxtDisplayTextLine(2, "Press orange");
nxtDisplayTextLine(3, "button to switch");
while (true) {
// The enter button has been pressed, switch
// to the other mode
if (nNxtButtonPressed == kEnterButton) {
active = !active;
if (!active)
// Turn the light off
LSsetInactive(LEGOLS);
else
// Turn the light on
LSsetActive(LEGOLS);
// wait 500ms to debounce the switch
wait1Msec(500);
}
nxtDisplayClearTextLine(5);
nxtDisplayClearTextLine(6);
// Get the raw value from the sensor
raw = LSvalRaw(LEGOLS);
// Get the normalised value from the sensor
nrm = LSvalNorm(LEGOLS);
nxtDisplayTextLine(5, "Raw: %4d", raw);
nxtDisplayTextLine(6, "Norm: %4d", nrm);
wait1Msec(50);
}
}
task main () {
int magFieldValue = 0;
int calibrationValue = 0;
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "MAGNETIC");
nxtDisplayCenteredTextLine(3, "Field Sensor");
nxtDisplayCenteredTextLine(4, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect Sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
nxtDisplayCenteredTextLine(5, "Press enter");
nxtDisplayCenteredTextLine(6, "to set bias");
wait1Msec(2000);
eraseDisplay();
while(true) {
eraseDisplay();
nxtDisplayTextLine(1, "Resetting");
nxtDisplayTextLine(2, "bias");
wait1Msec(500);
// Start the calibration and display the offset
calibrationValue = HTMAGstartCal(HTMAG);
nxtDisplayTextLine(2, "Bias: %4d", calibrationValue);
PlaySound(soundBlip);
while(bSoundActive) EndTimeSlice();
while(nNxtButtonPressed != kNoButton) EndTimeSlice();
while(nNxtButtonPressed != kEnterButton) {
eraseDisplay();
// Read the current calibration offset
calibrationValue = HTMAGreadCal(HTMAG);
// Read the current magnetic field strength
magFieldValue = HTMAGreadVal(HTMAG);
nxtDisplayTextLine(1, "Reading");
// Display the current calibration value
nxtDisplayTextLine(2, "Bias: %4d", calibrationValue);
nxtDisplayClearTextLine(4);
// Display the current magnetic field strength
nxtDisplayTextLine(4, "Mag: %4d", magFieldValue);
nxtDisplayTextLine(6, "Press enter");
nxtDisplayTextLine(7, "to recalibrate");
wait1Msec(100);
}
}
}
task main () {
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "GYRO");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
eraseDisplay();
time1[T1] = 0;
while(true) {
if (time1[T1] > 1000) {
eraseDisplay();
nxtDisplayTextLine(1, "Resetting");
nxtDisplayTextLine(1, "heading");
wait1Msec(500);
// Start the calibration and display the offset
nxtDisplayTextLine(2, "Offset: %4d", HTGYROstartCal(HTGYRO));
PlaySound(soundBlip);
while(bSoundActive) EndTimeSlice();
time1[T1] = 0;
}
while(nNxtButtonPressed != kEnterButton) {
eraseDisplay();
nxtDisplayTextLine(1, "Reading");
// Read the current calibration offset and display it
nxtDisplayTextLine(2, "Offset: %4d", HTGYROreadCal(HTGYRO));
nxtDisplayClearTextLine(4);
// Read the current rotational speed and display it
nxtDisplayTextLine(4, "Gyro: %4d", HTGYROreadRot(HTGYRO));
nxtDisplayTextLine(6, "Press enter");
nxtDisplayTextLine(7, "to recalibrate");
wait1Msec(100);
}
}
}
task main () {
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "MAGNETIC");
nxtDisplayCenteredTextLine(3, "Field Sensor");
nxtDisplayCenteredTextLine(4, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
eraseDisplay();
time1[T1] = 0;
while(true) {
eraseDisplay();
nxtDisplayTextLine(1, "Resetting");
nxtDisplayTextLine(2, "bias");
wait1Msec(500);
// Start the calibration and display the offset
nxtDisplayTextLine(2, "Bias: %4d", HTMAGstartCal(HTMAG));
PlaySound(soundBlip);
while(bSoundActive) EndTimeSlice();
while(nNxtButtonPressed != kNoButton) EndTimeSlice();
while(nNxtButtonPressed != kEnterButton) {
eraseDisplay();
nxtDisplayTextLine(1, "Reading");
// Read the current calibration offset and display it
nxtDisplayTextLine(2, "Bias: %4d", HTMAGreadCal(HTMAG));
nxtDisplayClearTextLine(4);
// Read the current rotational speed and display it
nxtDisplayTextLine(4, "Mag: %4d", HTMAGreadVal(HTMAG));
nxtDisplayTextLine(6, "Press enter");
nxtDisplayTextLine(7, "to recalibrate");
wait1Msec(100);
}
}
}
//
// FUNCTIONS
//
void calibrate(){
nxtDisplayCenteredBigTextLine(2,"WHITE");
do{
white = SensorValue(lightSensor);
}while(nNxtButtonPressed != 3);
Sleep(1000);
nxtDisplayCenteredBigTextLine(2,"BLACK");
do{
black = SensorValue(lightSensor);
}while(nNxtButtonPressed != 3);
Sleep(1000);
nxtDisplayCenteredBigTextLine(2,"START!");
do{
Sleep(100);
}while(nNxtButtonPressed != 3);
nxtDisplayClearTextLine(2);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// ButtonTask
//
// This task monitors the use of the NXT buttons when selecting the autonomous routine and setting up
// any global variables
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
task ButtonTask()
{
nSchedulePriority = kHighPriority;
switch(nNxtButtonPressed)
{
case kLeftButton:
if (gReady == 0)
{
gRunIdx--;
if (gRunIdx < 0)
{
gRunIdx = 3;
}
nxtDisplayTextLine(2, gRun[gRunIdx]);
}
else if (gReady == 1)
{
gSideIdx--;
if (gSideIdx < 0)
{
gSideIdx = 1;
}
nxtDisplayTextLine(3, "Side: %s", gSide[gSideIdx]);
}
else if (gReady == 2)
{
gPos--;
if (gPos<1)
{
gPos=3;
}
nxtDisplayTextLine(4, "Position: %i", gPos);
}
else if (gReady == 3)
{
gPath--;
if (gPath<1)
{
gPath=3;
}
nxtDisplayTextLine(5, "Column: %i", gPath);
}
else if (gReady == 4)
{
gHeight--;
if (gHeight<1)
{
gHeight=3;
}
nxtDisplayTextLine(6, "Row: %i", gHeight);
}
break;
case kRightButton:
if (gReady == 0)
{
gRunIdx++;
if (gRunIdx > 3)
{
gRunIdx = 0;
}
nxtDisplayTextLine(2, gRun[gRunIdx]);
}
else if (gReady == 1)
{
gSideIdx++;
if (gSideIdx > 1)
{
gSideIdx = 0;
}
nxtDisplayTextLine(3, "Side: %s", gSide[gSideIdx]);
}
else if (gReady == 2)
{
gPos++;
if (gPos>3)
{
gPos=1;
}
nxtDisplayTextLine(4, "Position: %i", gPos);
}
else if (gReady == 3)
{
gPath++;
if (gPath>3)
{
gPath=1;
}
nxtDisplayTextLine(5, "Column: %i", gPath);
}
else if (gReady == 4)
{
gHeight++;
if (gHeight>3)
{
gHeight=1;
}
nxtDisplayTextLine(6, "Row: %i", gHeight);
}
break;
case kEnterButton:
gReady++;
if (gReady == 1)
{
nxtDisplayTextLine(3, "Side: %s", gSide[gSideIdx]);
}
else if (gReady == 2)
{
nxtDisplayTextLine(4, "Position: %i", gPos);
}
else if (gReady == 3)
{
nxtDisplayTextLine(5, "Column: %i", gPath);
}
else if (gReady == 4)
{
nxtDisplayTextLine(6, "Row: %i", gHeight);
}
break;
case kExitButton:
gReady--;
if (gReady < 0)
{
gReady = 0;
}
if (gReady == 0)
{
nxtDisplayClearTextLine(3);
}
else if (gReady == 1)
{
nxtDisplayClearTextLine(4);
}
else if (gReady == 2)
{
nxtDisplayClearTextLine(5);
}
else if (gReady == 3)
{
nxtDisplayClearTextLine(6);
}
break;
}
return;
}
task main()
{
initializeRobot();
waitForStart();
int curGyro;
int oldGyro;
oldGyro = getGyroData(S2);
movData data;
//int offset = getOffset();
bool fieldoriented = true;
//bNxtLCDStatusDisplay = false;
while( true )
{
getJoystickSettings(joystick);
if (joystick. joy2_TopHat == 0 || joystick. joy2_TopHat == 1 || joystick. joy2_TopHat == 7) { //lift up
Motors_SetSpeed(S1, 1, 1, -100);
Motors_SetSpeed(S1, 1, 2, 100);
} else if (joystick. joy2_TopHat == 3 || joystick. joy2_TopHat == 4 || joystick. joy2_TopHat == 5){ //lift down
Motors_SetSpeed(S1, 1, 1, 60);
Motors_SetSpeed(S1, 1, 2, -60);
}else {
Motors_SetSpeed(S1, 1, 1, 0);
Motors_SetSpeed(S1, 1, 2, 0);
}
if (joy2Btn(4) == 1) //ball intake
Motors_SetSpeed(S1, 4, 1, 60);
else if (joy2Btn(2) == 1)
Motors_SetSpeed(S1, 4, 1, -60);
else
Motors_SetSpeed(S1, 4, 1, 0);
if (joy2Btn(3) == 1) //kickstand knocker-downer
servo[stand] = 240;
if (joy2Btn(1) == 1)
servo[stand] = 150;
if (joy2Btn(5) == 1) //dropper
servo[ball] = 18;
if (joy2Btn(6) == 1)
servo[ball] = 160;
//if (joy2Btn(8) == 1)
// fieldoriented = false;
// Drive Base
data.xComp = joystick.joy1_x1;
data.yComp = joystick.joy1_y1-1;
data.rot = joystick.joy1_x2;
//data.xComp = 127;
//data.yComp = 0;
//data.rot = 38;
data.xComp = (data.xComp != -128 ? data.xComp : data.xComp+1);
data.yComp = (data.yComp != -128 ? data.yComp : data.yComp+1);
data.rot = (data.rot != -128 ? data.rot : data.rot+1);
//curGyro = getGyroData(S2);
//this block of if statements is the controller dead-zone
if (data.rot < 10 && data.rot > -10)
data.rot = 0;
if (data.xComp < 10 && data.xComp > -10)
data.xComp = 0;
if (data.yComp < 10 && data.yComp > -10)
data.yComp = 0;
nxtDisplayClearTextLine(2);
nxtDisplayClearTextLine(4);
nxtDisplayBigTextLine(2, "%d", curGyro);
wait1Msec(1);
nxtDisplayBigTextLine(4, "%d", oldGyro);
//if (fieldoriented)
// oldGyro = useGyro(data, oldGyro, curGyro, offset);
if (data.rot < 2 && data.rot > -2)
data.rot = 0;
if (data.xComp < 2 && data.xComp > -2)
data.xComp = 0;
if (data.yComp < 2 && data.yComp > -2)
data.yComp = 0;
//byte speed = getSqrt(data.xComp, data.yComp) + abs(data.rot);
int speed = (int)(sqrt(data.xComp*data.xComp + data.yComp*data.yComp) + abs(data.rot)); //finds speed (dist formula)
if (speed > 127) speed = 127; //Regulates speed
drive(data, (byte)speed);
}
drive(data, 0);
}
