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);
}
}
void initializeRobot()
{
// Set light sensors to active
LSsetActive(left_light);
LSsetActive(middle_light);
LSsetActive(right_light);
// Initialize the encoder
nMotorEncoder[shoulderJoint] = 0;
//nMotorEncoder[spear] = 0;
}
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
}
}
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.
};
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);
}
}
void init(){
fs1 = fs2 = 0; //Force Sensor Values
servoPos = 0; //Initiate Claw Servo Pos
servo[servo1] = servoPos; //Set servo to servoPos
nMotorEncoder[armR] = 0; //Initiate Encoder Pos
servo[servo3]=35; //Initiate Servvo Ramp Lock
power=50; //Motor Power Level
LSsetActive(LEGOLS); //Turn on color sensor
lsVal = LSvalNorm(LEGOLS);
////Column positions left to right////
col1=col2=col3=0;
}
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;
}
void initializeRobot()
{
HTGYROstartCal(HTGYRO);
LSsetActive(lineFollower);
setWrist (.95);
while (SensorValue [liftDownSensor] == 0) motor [linearSlides]=-100;
motor [linearSlides]=0;
wait1Msec (500);
// Place code here to sinitialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
return;
}
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);
}
}
void initializeRobot()
{
LSsetActive(lineFollower);
setWrist (.95);
while (!TSreadState (liftDownSensor))
motor [linearSlides]=-100;
motor [linearSlides]=0;
servo [rampLatch] = rampLatchClosed;
servo [seekerPivot]= seekerUp;
HTIRS2setDSPMode(irSeeker, DSP_1200);
wait1Msec (500);
// Place code here to sinitialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
return;
}
//===================================================
// task to read in all sensors to workspace variables
//===================================================
task sensors()
{
float currDir = 0.0; //prevDir = 0.0,
long currtime,prevtime;
LSsetActive(LEGOLS); // set the LEGO light sensor to active mode
//-------------------------
// gyro
//-------------------------
ir_mux_status=HTSMUXreadPowerStatus(IR_MUX); // read the sensor multiplexor status
gyro_mux_status=HTSMUXreadPowerStatus(GYRO_MUX); // read the sensor multiplexor status
while (ir_mux_status || gyro_mux_status) // check good battery power on both muxes
{
PlayTone(750,25); // if not we beep indefinitely
wait1Msec(500);
}
//SMUX_good = true;
while(calibrate != 1){}; // wait for a request to start calibrating the gyro
wait1Msec(300); // short delay to ensure that user has released the button
HTGYROstartCal(HTGYRO); // initiate the GYRO calibration
drift = MyHTCal(gyroCalTime*1000);
Driver_Cal = HTGYROreadCal(HTGYRO); // read the calculated calibration value for saving to file
//---------------------------------------
// write the GYRO calibration data to file for Tele-Op
//---------------------------------------
Delete(sFileName, nIoResult); // delete any pre-existing file
nFileSize = 100; // new file size will be 100 bytes
OpenWrite( hFileHandle, nIoResult, sFileName, nFileSize); // create and open the new file
WriteFloat( hFileHandle, nIoResult, drift); // write the current drift value to the file
WriteFloat( hFileHandle, nIoResult, Driver_Cal); // write the driver calibration to the file
Close(hFileHandle, nIoResult); // close the file
//---------------------------------------
for (int i=0;i<5;i++) // check if there is too much spread in the data
{
if (gyro_noise>10) // if there is too much spread we beep 5 times to alert the drive team
{
gyroTrue = true;
PlayTone (250,25);
wait1Msec(500);
}
}
calibrate = 2; // this signifies to the main program that calibration has been completed
prevtime = nPgmTime;
while(true)
{
currtime=nPgmTime;
rawgyro = HTGYROreadRot(HTGYRO);
constHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
relHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
prevtime = currtime;
//wait1Msec(1);
//---------------------------------------------------------------------
// Read both sonar sensors and filter out non-valid echo readings (255)
// If there is no echo the filter just retains the last good reading
//---------------------------------------------------------------------
sonarRaw = USreadDist(LEGOUS); // read the rear mounted sensor
if (sonarRaw!=255) sonarLive = sonarRaw; // and copy valid results to workspace
sonarRaw2 = USreadDist(LEGOUS2); // read the side mounted sensor
if (sonarRaw2!=255) sonarLive2 = sonarRaw2; // and copy valid results to workspace
//-------------------------
// LEGO light sensor
//-------------------------
light_normalised = LSvalNorm(LEGOLS); // read the LEGO light sensor
//-------------------------
// HiTechnic IR Sensor
//-------------------------
bearingAC = HTIRS2readACDir(HTIRS2); // Read the IR bearing from the sensor
bearingAC2 = HTIRS2readACDir(HTIRS2_2);//here 12334
currDir = (float) bearingAC; // copy into workspace -
/*if (bearingAC == 0) // No IR signal is being detected
{
currDir = prevDir; // so retain the previous reading
}
else // otherwise read all the IR segments
{
{
bearingAC = (bearingAC - 1)/2;
if ((bearingAC < 4) && (acS[bearingAC] != 0) && (acS[bearingAC + 1] != 0))
{
currDir += (float)(acS[bearingAC + 1] - acS[bearingAC])/
max(acS[bearingAC], acS[bearingAC + 1]);
}
}
}
prevDir = currDir;
IR_Bearing=currDir-5; // and setup the main variable for others to use
*/
HTIRS2readAllACStrength(HTIRS2, acS[0], acS[1], acS[2], acS[3], acS[4]);
HTIRS2readAllACStrength(HTIRS2_2, acS2[0], acS2[1], acS2[2], acS2[3], acS2[4]);
//-----------------------------------
// code for the peaks of IR sensor 1
//-----------------------------------
if (bearingAC!=0) // we have a valid IR signal
{
int maximum = -1;
int peak = 0, offset=0;
for (int i=0;i<5;i++) // scan array to find the peak entry
{ if (acS[i]>maximum)
{peak = i;
maximum = acS[i];
}
}
offset=0;
if ((peak < 4) && (peak>0) && (acS[peak] != 0)) // we are not working with extreme value
{
if (acS[peak-1]!=acS[peak+1]) // if the values either side of the peak are identical then peak is peak
{
if (acS[peak-1]>acS[peak+1]) // otherwise decide which side has higher signal
{
offset = -25*(1-(float)(acS[peak]-acS[peak-1])/ // calculate the bias away from the peak
max(acS[peak], acS[peak-1]));
}
else
{
offset = 25*(1-(float)(acS[peak]-acS[peak+1])/
max(acS[peak], acS[peak+1]));
}
}
}
IR_Bearing = (float)((peak-2)*50) + offset; // direction is the total of the peak bias plus the adjacent bias
// range is -100 to +100, zero is straight ahead
}
//-----------------------------------
// code for the peaks of IR sensor 2
//-----------------------------------
if (bearingAC2!=0) // we have a valid IR signal
{
int maximum = -1;
int peak = 0, offset=0;
for (int i=0;i<5;i++) // scan array to find the peak entry
{ if (acS2[i]>maximum)
{peak = i;
maximum = acS2[i];
}
}
offset=0;
if ((peak < 4) && (peak>0) && (acS2[peak] != 0)) // we are not working with extreme value
{
if (acS2[peak-1]!=acS2[peak+1]) // if the values either side of the peak are identical then peak is peak
{
if (acS2[peak-1]>acS2[peak+1]) // otherwise decide which side has higher signal
{
offset = -25*(1-(float)(acS2[peak]-acS2[peak-1])/ // calculate the bias away from the peak
max(acS2[peak], acS2[peak-1]));
}
else
{
offset = 25*(1-(float)(acS2[peak]-acS2[peak+1])/
max(acS2[peak], acS2[peak+1]));
}
}
}
IR_Bearing2 = (float)((peak-2)*50) + offset; // direction is the total of the peak bias plus the adjacent bias
// range is -100 to +100, zero is straight ahead
}
}
}
//=============================================================================================================================================
//---------------------------------------------------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
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);
}
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() {
// Turn the light on
LSsetActive(LEGOLS);
}
void sensorsLightLightOn(){
LSsetActive(lightMUX);
}
task sensors()
{
//-------------------------
// gyro
//-------------------------
long currtime,prevtime;
int acS[5];
while (HTSMUXreadPowerStatus(S3)) // check battery power is on
{
PlayTone(750,25);
wait1Msec(500);
}
SMUX_good = true;
while(calibrate != 1){};
wait1Msec(300);
HTGYROstartCal(HTGYRO);
float drift = MyHTCal(gyroCalTime*1000);
for (int i=0;i<5;i++) // check if there is too much spread in the data
{
if (abs(highest-lowest)>10)
{
PlayTone (250,25);
wait1Msec(500);
}
}
calibrate = 2;
prevtime = nPgmTime;
while(true)
{
currtime=nPgmTime;
newgyro = (float)HTGYROreadRot(HTGYRO);
constHeading += (newgyro - drift) * (float)(currtime-prevtime)/1000;
relHeading += (newgyro - drift) * (float)(currtime-prevtime)/1000;
prevtime = currtime;
wait1Msec(1);
//-------------------------
// IR
//-------------------------
bearingAC = HTIRS2readACDir(HTIRS2);
#define max(a, b) (((a) > (b))? (a): (b))
#define min(a, b) (((a) < (b))? (a): (b))
currDir = (float) bearingAC;
if (bearingAC == 0)
{
currDir = prevDir;
}
else
{
if (HTIRS2readAllACStrength(HTIRS2, acS[0], acS[1], acS[2], acS[3], acS[4]))
{
bearingAC = (bearingAC - 1)/2;
if ((bearingAC < 4) && (acS[bearingAC] != 0) && (acS[bearingAC + 1] != 0))
{
currDir += (float)(acS[bearingAC + 1] - acS[bearingAC])/
max(acS[bearingAC], acS[bearingAC + 1]);
}
}
}
prevDir = currDir;
////-------------------------
//// Sonar
////-------------------------
//num = USreadDist(LEGOUS);
//num2 = USreadDist(LEGOUS2);
//if(num != 255) sonarLive = num;
//if(num2 != 255) sonarLive2 = num2;
//-------------------------
// light
//-------------------------
LSsetActive(LEGOLS);
nrm = LSvalNorm(LEGOLS);
}
}
