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
}
}
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() {
short raw = 0;
short nrm = 0;
bool active = true;
// Turn the light on
LSsetActive(LEGOLS);
displayCenteredTextLine(0, "Lego");
displayCenteredBigTextLine(1, "LIGHT");
displayCenteredTextLine(3, "SMUX Test");
displayCenteredTextLine(5, "Connect SMUX to");
displayCenteredTextLine(6, "S1 and sensor to");
displayCenteredTextLine(7, "SMUX Port 1");
sleep(2000);
displayClearTextLine(7);
displayTextLine(5, "Press [enter]");
displayTextLine(6, "to toggle light");
sleep(2000);
while (true) {
// The enter button has been pressed, switch
// to the other mode
if (getXbuttonValue(xButtonEnter))
{
active = !active;
if (!active)
LSsetInactive(LEGOLS);
else
LSsetActive(LEGOLS);
// wait 500ms to debounce the switch
sleep(500);
}
displayClearTextLine(5);
displayClearTextLine(6);
raw = LSvalRaw(LEGOLS);
nrm = LSvalNorm(LEGOLS);
displayTextLine(5, "Raw: %4d", raw);
displayTextLine(6, "Norm: %4d", nrm);
sleep(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()
{
init();
waitForStart(); //Waits for FTC match to officialy start
//Main Loop
while(true){
getJoystickSettings(joystick);
armEncoder=nMotorEncoder[armR];
lsVal = LSvalRaw(LEGOLS);
if(ServoValue[servo1]==100&&up&&ServoValue[servo2]==215-ser){
ser=0;
up=false;
}
if(ServoValue[servo1]==0&&ServoValue[servo2]==215-ser&&!up){
ser=100;
up=true;
}
servo[servo1]=ser;
servo[servo2]=215-ser;
//Update Motors////////////
///////////////////////////
//Update Servos////////////
if(rampLock){
servo[servo3]=35;
}
else{
servo[servo3]=100;
}
///////////////////////////
//Hi Alejandro :D
//Update Sensors///////////
//Check for weighted Ring///
fs1 = HTFreadSensor(HTFS1);
fs2 = HTFreadSensor(HTFS2);
if(fs1<1024)
fs1+=1000;
if(fs2<1024)
fs2+=1000;
if(fs1>1410||fs2>1375){//||(fs2>1325&&fs1<1160&&fs1>1150)){
LSsetActive(light);
//motor[light]=100;
//PlaySound(soundBlip);
}
else{
LSsetInactive(light);
//motor[light]=0;
}
nxtDisplayCenteredTextLine(1,"Servo:%i %i",servo[servo1],ser);
nxtDisplayCenteredTextLine(2,"Force1: %i",fs1);
nxtDisplayCenteredTextLine(3,"Force2: %i",fs2);
nxtDisplayCenteredTextLine(4,"RawColor: %i",lsVal);
nxtDisplayCenteredTextLine(5,"Encoder: %i",armEncoder);
////////////////////////////
//Gamepad 2(Arm and claw Control)////////////////
getJoystickSettings(joystick);
if(joy2Btn(1)){ //Speed Toggle
while(joy2Btn(1)){}
toggle2 = !toggle2;
}
if(toggle2)
mult2 = 1;
if(!toggle2) //USE AN ELSE STATMENT!!!
mult2 = .5;
getJoystickSettings(joystick);
if(joy2Btn(5)){ //Arm Control
motor[armR] = 100*.5*mult2;//mult2;
//motor[armL] = 100*mult2;
}
else if(joy2Btn(6)){
motor[armR] = -100*mult2;
//motor[armL] = -100*mult2;
}
else{
motor[armR] = 0;
//motor[armL] = 0;
}
//////////////////////////////
//Gamepad 1(Drive Train Control)///////////////////
getJoystickSettings(joystick);
if(joy1Btn(1)){ //Speed Toggledjjj
while(joy1Btn(1)){}
toggle = !toggle;
}
if(joy1Btn(6)){
motor[ramp]=-50;
}
else if(joy1Btn(5)){
motor[ramp]=100;
}
else{
motor[ramp]=0;//UNESSECARRY BRACKETS
}
if(joy1Btn(4)){
while(joy1Btn(4)){}
rampLock=!rampLock;
}
if(toggle)
mult=.8;
if(!toggle||abs(armEncoder)>150)
mult=.5;
getJoystickSettings(joystick);
if(abs(joystick.joy1_y1)>threshold){ //Diagonal Forward Right & Backwards Left
motor[motorFL] = joystick.joy1_y1*mult;
motor[motorBL] = joystick.joy1_y1*mult;
}
else{
motor[motorBL] = 0;
motor[motorFL] = 0;
}
if(abs(joystick.joy1_y2)>threshold){
motor[motorFR] = joystick.joy1_y2*mult;
motor[motorBR] = joystick.joy1_y2*mult;
}
else{ //Stop Motors
motor[motorFR] = 0;
motor[motorBR] = 0;
}
////////////////////////////////////////////
time+=10;
wait1Msec(10);
}
}
//=============================================================================================================================================
//---------------------------------------------------BEGIN INITIALIZATION CODE-----------------------------------------------------------------
task main() {
//Initialize the display with the program choices
chooseProgram();
switch (PROGID) {
case 1:
FORWARD_SCORE_FORWARD_LINE_1 = true;
linesToFind = 1;
break;
case 2:
FORWARD_SCORE_FORWARD_LINE_2 = true;
linesToFind = 2;
break;
case 3:
FORWARD_SCORE_BACKWARD_LINE_1 = true;
linesToFind = 1;
break;
case 4:
FORWARD_SCORE_BACKWARD_LINE_2 = true;
linesToFind = 2;
break;
case 5:
useDummyAutonomous();
break;
case 6:
//useOriginalAutonomous();
PlaySoundFile("Woops.rso");
break;
}
//---------------------------------------------------------END INITIALIZATION CODE-------------------------------------------------------------
//=============================================================================================================================================
//if (PROGID == 1 || PROGID == 2 || PROGID == 3 || PROGID == 4) {
TFileHandle irFileHandle;
TFileIOResult IOResult;
HTGYROstartCal(HTGYRO);
//PlaySound(soundBlip);
//wait1Msec((2 * PI) * 1000); //TAUUUU
//wait1Msec(10000);//wait 10 seconds for other teams who **might** have better autonomous code
PlaySound(soundFastUpwardTones);
//_________________________________BLOCK TO GET SENSORVALUES FROM IRSEEKER_________________________
//=================================================================================================
int _dirDCL = 0;
int _dirACL = 0;
int dcS1L, dcS2L, dcS3L, dcS4L, dcS5L = 0;
int acS1L, acS2L, acS3L, acS4L, acS5L = 0;
int _dirEnhL, _strEnhL;
// the default DSP mode is 1200 Hz.
initializeRobot();
servo[servoLift] = 123;
servo[servoSweep] = 199;
waitForStart();
ClearTimer(T1);
OpenWrite(irFileHandle, IOResult, fileName, sizeOfFile);
// FULLY DYNAMIC CODE W/ SCORING OF CUBE
while(searching)
{
//float irval = acS3L;
//StringFormat(irvalres, "%3.0f", irval);
//WriteText(irFileHandle, IOResult, "Test");
//WriteString(irFileHandle, IOResult, irvalres);
//WriteByte(irFileHandle, IOResult, 13);
//WriteByte(irFileHandle, IOResult, 10);
_dirDCL = HTIRS2readDCDir(HTIRS2L);
if (_dirDCL < 0)
break; // I2C read error occurred
_dirACL = HTIRS2readACDir(HTIRS2L);
if (_dirACL < 0)
break; // I2C read error occurred
//===========LEFT SIDE
// Read the individual signal strengths of the internal sensors
// Do this for both unmodulated (DC) and modulated signals (AC)
if (!HTIRS2readAllDCStrength(HTIRS2L, dcS1L, dcS2L, dcS3L, dcS4L, dcS5L))
break; // I2C read error occurred
if (!HTIRS2readAllACStrength(HTIRS2L, acS1L, acS2L, acS3L, acS4L, acS5L ))
break; // I2C read error occurred
//=================Enhanced IR Values (Left and Right)===========
// Read the Enhanced direction and strength
if (!HTIRS2readEnhanced(HTIRS2L, _dirEnhL, _strEnhL))
break; // I2C read error occurred
//______________END SENSORVAL BLOCK________________________________________________________________
//=================================================================================================
if (acS3L < irFindVal) { //While sensor is heading towards beacon: acs3 = side
motor[motorLeft] = -80;
motor[motorRight] = -80;
if (time1[T1] > timeToEnd) {
searching = false;
koth = true;
goToEnd = false;
//if it doesnt find the beacon, dont bother returning to start if it has been set to
}
}
//===================================BLOCK FOR IR DETECTION=====================
if (acS3L > irFindVal) { //if sensor is directly in front of beacon
if (time1[T1] < 2000) {
wait1Msec(600);
}
motor[motorLeft] = 0;
motor[motorRight] = 0;
//irOnLeft = true;
searching = false;
koth = true;
goToEnd = true;
}
//==================END IR DETECTION BLOCK========================
wait1Msec(30);
}//while searching
//Close(irFileHandle, IOResult);
roundTime = time1[T1]; //probably unnecessary, is to cut out the time from the cube scorer
scoreCube();
if (goToEnd) {
if (FORWARD_SCORE_FORWARD_LINE_1 || FORWARD_SCORE_FORWARD_LINE_2) {
driveToEnd(-80, timeToEnd - roundTime);//drive to end of ramp
}
if (FORWARD_SCORE_BACKWARD_LINE_1 || FORWARD_SCORE_BACKWARD_LINE_2) {
driveToEnd(80, roundTime);
}
}
wait1Msec(300);
//=======================================================================================================================================
//------------------------BEGIN 90 DEGREE TURNS------------------------------------------------------------------------------------------
//HTGYROstartCal(HTGYRO);
ClearTimer(T3);
while (true) {
wait1Msec(20);
//if (abs(rotSpeed) > 3) {
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02);//find gyro heading in degrees??
motor[motorLeft] = 60;
motor[motorRight] = -60;
if (heading <= degFirstTurn) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
//---------------LINE DETECTOR--------------------------
LSsetActive(LEGOLS);
while (linesFound < linesToFind) {
motor[motorLeft] = -50;
motor[motorRight] = -50;
wait1Msec(10);
if (LSvalNorm(LEGOLS) >= WHITE_LINE_VALUE) {
linesFound++;
}
if (linesFound >= linesToFind) { //ever-present failsafe
break;
LSsetInactive(LEGOLS);
}
}
if (FORWARD_SCORE_FORWARD_LINE_1 || FORWARD_SCORE_FORWARD_LINE_2) {
while (true) {
wait1Msec(20);
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02);//find gyro heading in degrees??
motor[motorLeft] = 60;
motor[motorRight] = -60;
if (heading <= degSecondTurn) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
moveForward(3.3, 100);
break;
}
}
} else {
while (true) {
wait1Msec(20);
rotSpeed = HTGYROreadRot(HTGYRO);//find gyro rotation speed
heading += (rotSpeed * 0.02);//find gyro heading in degrees??
motor[motorLeft] = -60;
motor[motorRight] = 60;
if (heading <= 92) {
motor[motorLeft] = 0;
motor[motorRight] = 0;
moveForward(3.3, 100);
break;
}
}
}
break;
}
}
//==================================================================================
//Begin KotH routine
servo[servoUSSeeker] = 92;
USScanVal = 92;
float heading = 92;
HTGYROstartCal(HTGYRO);
while (koth) {
//wait1Msec(1000);
//SCAN LEFT==========================
while(true) {
servo[servoUSSeeker] = ServoValue[servoUSSeeker] + 5;
USScanVal += 5;
wait1Msec(100);
if (SensorValue[US1] < kothAttackDistance && nPgmTime < 27000) { //if something is in range AND program time is less than 27 seconds
PlaySound(soundFastUpwardTones);
searchingForBot = true;
turnedLeft = true;
turnedRight = false;
turnTowardsRobot();
pushOffRamp();
turnTowardsCenter();
}
if (USScanVal > 135) {
break;
}
}
//SCAN RIGHT=========================
while(true) {
servo[servoUSSeeker] = ServoValue[servoUSSeeker] - 5;
USScanVal -= 5;
wait1Msec(100);
if (USScanVal < 40) {
break;
}
if (SensorValue[US1] < kothAttackDistance && nPgmTime < 27000) { //if something is in range AND program time is less than 27 seconds
PlaySound(soundFastUpwardTones);
searchingForBot = true;
turnedLeft = false;
turnedRight = true;
turnTowardsRobot();
pushOffRamp();
turnTowardsCenter();
}
}
if (nPgmTime > 29000) {
koth = false;
}
}//while koth
MissionImpossible();
/*
}//END MAIN IF PROGIDS THING
else if (PROGID == 5) {
useDummyAutonomous();
}
*/
}//task main
void sensorsLightLightOff() {
LSsetInactive(lightMUX);
}
