int findLine()//Sweeps the servo-mounted light sensor to find a value that is 40 points from the absolute value of the initial reading.
{
servo[servo2] = 0;
int angle = 0;
int initValue = 0;
int raw = 0;
bool isFinished = false;
int i = 127;
LSsetActive(LightSensor);
wait10Msec(100);
initValue = LSvalRaw(LightSensor);
nxtDisplayBigTextLine(2, "%d", initValue);
raw = initValue;
for(i = 0; abs(raw-initValue)<=40 && i < 255; i++)
{
raw = LSvalRaw(LightSensor);
nxtDisplayBigTextLine(4, "%d", raw);
if(abs(raw - initValue)> 4)
{
isFinished = true;
}
wait10Msec(1);
servo[servo2] = i;
}
angle = i * 180/255;
int distance = 40*cosDegrees(angle);
return distance;//The distance tells us how far we need to move the V-Bucket(Trademark Pending) in order to score autonomously.
};
// Follow a line, after we've been aligned to it, stopping based on color, distance, or time
void _followLine(FloorColor color, int speed, FloorColor stopColor, int stopDistance, int stopTime) {
// Reset the drive encoder if we're going to use it
if (stopDistance != 0) {
resetDriveEncoder();
}
// Loop until we hit a stop condition
while (true) {
if (stopColor != UNKNOWN && (onColor(stopColor, LSvalRaw(lineRight)))) {
break;
} else if (stopDistance > 0 && readDriveEncoder() > stopDistance) {
break;
} else if (stopDistance < 0 && readDriveEncoder() < stopDistance) {
break;
} else if (stopTime > 0 && true) {
// Can we check elapsed time?
break;
}
if (onColor(color, LSvalRaw(lineLeft))) {
runDriveMotors(0, speed);
} else if (onColor(color, LSvalRaw(lineRight))) {
runDriveMotors(speed, 0);
} else {
runDriveMotors(speed, speed);
}
}
// Always stop when we're done
stopDriveMotors();
}
void goStraightUntilIRNothing(int speed)//This function allows locomotion over a predefined period (per the parameters) and stop. We create a timer in order to measure the time elapsed between the beginning and end of execution of this function.
{
servo[servo2]=127;
int initValue = 0;
int raw = 0;
initValue = LSvalRaw(LightSensor);
raw = initValue;
while(readIR() > 0)//While loop saying that while the value of the light sensor is less than the value of the parametrically set stop value.
{
raw = LSvalRaw(LightSensor);
nxtDisplayCenteredBigTextLine(1, "%d", raw);
driveStraight (speed, 0);//Drivers forward with no light sensor value change (the null value is set in the parameters)
}
}
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);
}
}
// Align to a line, assuming we start at or just beyond it, but stop if we hit the STOP color with either sensor
void alignLine(FloorColor color, int speed, FloorColor stop = UNKNOWN, bool reverse = false) {
// Spin ~180 if we've been asked to align in reverse
if (reverse) {
// Nudge forward
runDriveMotors(100, 100);
wait1Msec(150);
// Spin in place
runDriveMotors(100, -100);
wait1Msec(1500);
stopDriveMotors();
}
// Start our turn to get the front sensor on the line
while(!onColor(color, LSvalRaw(lineLeft))) {
// Stop alignment efforts if we hit the specified stop floor color
if (stop != UNKNOWN && (onColor(stop, LSvalRaw(lineRight)) || onColor(stop, LSvalRaw(lineLeft)))) {
break;
}
// Turn in place
runDriveMotors(speed, -1 * speed);
}
stopDriveMotors();
// When the back sensor is on the line, we're aligned
while(!onColor(color, LSvalRaw(lineRight))) {
// Stop alignment efforts if we hit the specified stop floor color
if (stop != UNKNOWN && (onColor(stop, LSvalRaw(lineRight)) || onColor(stop, LSvalRaw(lineLeft)))) {
break;
}
// Drive forward until the front sensor is clear of the line
// TODO: Adjust onColor to provide more useful results, so we can invert this test
if (!onColor(GREY, LSvalRaw(lineLeft))) {
runDriveMotors(speed, speed);
// Turn while neither sensor is on the line
} else if (!onColor(color, LSvalRaw(lineRight)) && !onColor(color, LSvalRaw(lineLeft))) {
runDriveMotors(speed, 0);
}
}
// Always stop when we're done
stopDriveMotors();
}
void init(){
fs1 = fs2 = dist = 0; //Force Sensor Values
mult = mult2 = 1; //Speed Control
rampLock=toggle = toggle2 = true; //Speed Toggles
nMotorEncoder[ramp] = 0; //Initiate Encoder Pos
nMotorEncoder[armR] = 0; //Initiate Encoder Pos
bFloatDuringInactiveMotorPWM = false;
servoChangeRate[servo1] = 10;
servoChangeRate[servo2] = 10;
LSsetActive(LEGOLS);
lsVal = LSvalRaw(LEGOLS);
ser=0;
}
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);
}
}
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);
}
}
task main() {
waitForStart();
/*int raw = 0;
int nrm = 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, "S1 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
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);
}
nxtDisplayClearTextLine(5);
nxtDisplayClearTextLine(6);
raw = LSvalRaw(LEGOLS);
nrm = LSvalNorm(LEGOLS);
nxtDisplayTextLine(5, "Raw: %4d", raw);
nxtDisplayTextLine(6, "Norm: %4d", nrm);
wait1Msec(50);
}*/
int ser=0,fs1=0;
servo[servo1]=ser;
servo[servo2]=215-ser;
bool rightOfLine = true,touch=false;
LSsetActive(LEGOLS);
touch = TSreadState(LEGOTS);
lsVal = LSvalRaw(LEGOLS);
nMotorEncoder[arms] = 0; //Initiate Encoder Pos
wait1Msec(2500);
while(lsVal<360){
lsVal = LSvalRaw(LEGOLS);
forward(15);
}
stop();
//reverse(50);
wait1Msec(500);
stop();
wait1Msec(1000);
while(nMotorEncoder[arms]<30){
raiseArm(50);
nxtDisplayCenteredTextLine(1,"Encoder:%i",nMotorEncoder[arms]);
}stop();
wait1Msec(1000);
while(lsVal<210){
nxtDisplayCenteredTextLine(3,"Time:%i",time1[T1]);
lsVal = LSvalRaw(LEGOLS);
left(50);
}
stop();
wait1Msec(1000);
fs1 = HTFreadSensor(HTFS1);
nxtDisplayCenteredTextLine(3,"Time:%i",time1[T1]);
ClearTimer(T1);
while(time1[T1]<2500){
nxtDisplayCenteredTextLine(3,"Time:%i",time1[T1]);
fs1 = HTFreadSensor(HTFS1);
lsVal = LSvalRaw(LEGOLS);
touch = TSreadState(LEGOTS);
nxtDisplayTextLine(5, "Raw: %4d", lsVal);
if(lsVal>210){
while(time1[T1]<2500){
forward(20);
lsVal = LSvalRaw(LEGOLS);
nxtDisplayTextLine(5, "Raw: %4d", lsVal);
}
rightOfLine=!rightOfLine;
}
else if(lsVal<210){
if(rightOfLine){
rotateLeft(50);
}else{
rotateRight(50);
}
}
}
stop();
ser=100;
servo[servo1]=ser;
servo[servo2]=215-ser;
while(nMotorEncoder[arms]>5){
lowerArm(50);
nxtDisplayCenteredTextLine(1,"Encoder:%i",nMotorEncoder[arms]);
}stop();
reverse(50);
wait1Msec(2500);
}
int sensorsLightGetReflected() {
return LSvalRaw(lightMUX);
}
