// Set the sensor to the selected frequency
void setHz(tSensors link, byte hertz) {
byte msg[4] = { 3, 0x02, 0x41, 0x00 };
if (hertz == 50) {
msg[3] = 0x35;
} else if (hertz == 60) {
msg[3] = 0x36;
} else {
playSound(soundException);
eraseDisplay();
displayCenteredTextLine(3, "Wrong freq.");
displayCenteredTextLine(3, "specified");
sleep(5000);
stopAllTasks();
}
while (nI2CStatus[link] == STAT_COMM_PENDING){
sleep(5);
}
sendI2CMsg(link, &msg[0], 0);
if (nI2CStatus[link] == ERR_COMM_BUS_ERR) {
playSound(soundException);
eraseDisplay();
displayCenteredTextLine(3, "Error setting");
displayCenteredTextLine(3, "sensor to 50Hz");
sleep(5000);
stopAllTasks();
}
}
//IR SENSOR TASK
task IR_Sense(){
ubyte request[3]; // I2C request message
ubyte reply[6]; // accelerometer returns 7 values
int replyLen; // length of reply array.
tSensors port; // port the sensor is attached to.
// which port is the accelerator hooked up to?
port = S4;
// Ensure the sensor is configured correctly
SensorType[port] = sensorI2CCustom;
eraseDisplay();
while (true) {
// Read the data from the sensor
// setup the request message....
request[0] = 2; // Message size is 2 bytes..
request[1] = I2C_ADDR; // I2C Address of sensor(0x02)
request[2] = REG_OFFSET; // location of registers to read 0x42
replyLen = 6; // how many registers to read
// send request message to sensor.
sendI2CMsg(port, &request[0], replyLen);
// wait for a reply on the sensor port.
// also check for sensor error...
if (!waitForI2C(port)) {
displayBigTextLine(4, "Error 1.");
wait1Msec(2000);
stopAllTasks();
}
// read the reply from the sensor.
readI2CReply(port, &reply[0], replyLen);
// make sure the bus is clear...
// check for error.
if (!waitForI2C(port)){
displayBigTextLine(4, "Error 2.");
wait1Msec(2000);
stopAllTasks();
}
//Waits, then updates BrightLight value
wait1Msec(5);
BrightLight = reply[0];
}
}
task e_stop()
{
while(true)
{
if(SensorValue(rightT) == 1)
{
stopAllTasks(); // this ends the program
}
else if (SensorValue(leftT) == 1)
{
stopAllTasks();
}
wait1Msec(10); // this prevents the current task from hogging the CPU
}
}
//function to get passenger's current y location from numeric keypad
int get_passenger_location_y(){
int keys = 0;
unsigned byte key[] = {0};
int number = 0;
string output;
eraseDisplay();
displayCenteredTextLine(0, "Enter y-location");
displayTextLine(1, "-------------------");
displayTextLine(5, "-------------------");
while (true){
if (!MSNPscanKeys(MSNP, keys, key[0], number))
stopAllTasks();
if ((number>=0)&&(number<20)){
//if (number == 8){number = 0;}
displayTextLine(3, "Numpad Key: %d", number);
wait1Msec(1000);
sleep(100);
return number;
break;
}
}
}
task main () {
// Get control over the buttons
nNxtButtonTask = -2;
eraseDisplay();
displayTextLine(0, "HTCS Test 2");
displayTextLine(2, "Press orange");
displayTextLine(3, "button to start");
displayTextLine(4, "calibration.");
displayTextLine(5, "Press grey");
displayTextLine(6, "button to exit.");
while(nNxtButtonPressed != kEnterButton) EndTimeSlice();
eraseDisplay();
displayTextLine(3, "Starting");
displayTextLine(4, "calibration.");
// This call calibrates the white value.
if (!HTCScalWhite(HTCS)) {
eraseDisplay();
playSound(soundException);
displayTextLine(3, "ERROR!!");
displayTextLine(5, "Calibration");
displayTextLine(6, "failed!!");
sleep(2000);
stopAllTasks();
}
sleep(1000);
}
task main () {
short keys = 0;
unsigned byte key[] = {0};
short number = 0;
string output;
displayCenteredTextLine(0, "Mindsensors");
displayCenteredBigTextLine(1, "NumPad");
displayCenteredTextLine(3, "Test 1");
displayCenteredTextLine(5, "Connect sensor");
displayCenteredTextLine(6, "to S1");
sleep(2000);
eraseDisplay();
displayCenteredTextLine(0, "Mindsensors NP");
displayTextLine(1, "-------------------");
displayTextLine(5, "-------------------");
displayTextLine(7, "X: no key");
while (true) {
// Which key is being pressed now?
if (!MSNPscanKeys(MSNP, keys, key[0], number))
stopAllTasks();
// "convert" to string so we can print it
output = key;
displayTextLine(3, "Numpad Key: %s", output);
displayTextLine(4, "Numpad Num: %d", number);
sleep(100);
}
}
task main () {
short red = 0;
short green = 0;
short blue = 0;
short _color = 0;
string _tmp;
displayCenteredTextLine(0, "HiTechnic");
displayCenteredBigTextLine(1, "COLOUR");
displayCenteredTextLine(3, "Test 1");
displayCenteredTextLine(5, "Connect sensor");
displayCenteredTextLine(6, "to S1");
sleep(2000);
eraseDisplay();
while (true) {
// Read the currently detected colour from the sensor
_color = HTCSreadColor(HTCS);
// If colour == -1, it implies an error has occurred
if (_color < 0) {
displayTextLine(4, "ERROR!!");
sleep(2000);
stopAllTasks();
}
// Read the RGB values of the currently colour from the sensor
// A return value of false implies an error has occurred
if (!HTCSreadRGB(HTCS, red, green, blue)) {
displayTextLine(4, "ERROR!!");
sleep(2000);
stopAllTasks();
}
displayCenteredTextLine(0, "Color: %d", _color);
displayCenteredBigTextLine(1, "R G B");
eraseRect(0,10, 99, 41);
fillRect( 0, 10, 30, 10 + (red+1)/8);
fillRect(35, 10, 65, 10 + (green+1)/8);
fillRect(70, 10, 99, 10 + (blue+1)/8);
StringFormat(_tmp, " %3d %3d", red, green);
displayTextLine(7, "%s %3d", _tmp, blue);
sleep(100);
}
}
task main () {
short _dir = 0;
short dcS1, dcS2, dcS3, dcS4, dcS5 = 0;
displayCenteredTextLine(0, "HiTechnic");
displayCenteredBigTextLine(1, "IR Seekr");
displayCenteredTextLine(3, "Test 1");
displayCenteredTextLine(5, "This is for the");
displayCenteredTextLine(6, "v1 seeker");
sleep(2000);
while(true) {
eraseDisplay();
// read all of the sensors' values at once,
// exit the app if an error occurs
if (! HTIRSreadAllStrength(HTIRS, dcS1, dcS2, dcS3, dcS4, dcS5)) {
displayTextLine(4, "ERROR!!");
sleep(2000);
stopAllTasks();
}
// read the direction from which the signal is coming,
// exit the app if an error occurs
_dir = HTIRSreadDir(HTIRS);
if (_dir < 0) {
displayTextLine(4, "ERROR!!");
sleep(2000);
stopAllTasks();
}
// display the info from the sensor
displayTextLine(0,"HT IR Seeker");
displayTextLine(2, "dir: %2d", _dir);
displayTextLine(3, "S1: %3d", dcS1);
displayTextLine(4, "S2: %3d", dcS2);
displayTextLine(5, "S3: %3d", dcS3);
displayTextLine(6, "S4: %3d", dcS4);
displayTextLine(7, "S5: %3d", dcS5);
sleep(100);
}
}
task main () {
short distance = 0;
short voltage = 0;
short mindist = 0;
short maxdist = 0;
string type;
displayCenteredTextLine(0, "Mindsensors");
displayCenteredBigTextLine(1, "DIST-nx");
displayCenteredTextLine(3, "Test 1");
sleep(2000);
playSound(soundBeepBeep);
while(bSoundActive) EndTimeSlice();
eraseDisplay();
// Read the minimum distance the sensor can "see"
mindist = MSDISTreadMinDist(MSDIST);
// Read the maximum distance the sensor can "see"
maxdist = MSDISTreadMaxDist(MSDIST);
// Get the type of Sharp IR module connected to the sensor
switch(MSDISTreadModuleType(MSDIST)) {
case MSDIST_GP2D12: type = " GP2D12"; break;
case MSDIST_GP2D120: type = "GP2D120"; break;
case MSDIST_GP2YA02: type = "GP2YA02"; break;
case MSDIST_GP2YA21: type = "GP2YA21"; break;
case MSDIST_CUSTOM: type = " CUSTOM"; break;
}
displayTextLine(5, "Type: %s", type);
displayTextLine(6, "Min: %4dmm", mindist);
displayTextLine(7, "Max: %4dmm", maxdist);
while (true) {
// Get the distance calculated based on the data from the IR Sharp module
distance = MSDISTreadDist(MSDIST);
// Get the raw voltage data from the Sharp IR module
voltage = MSDISTreadVoltage(MSDIST);
if (distance < 0) {
displayTextLine(4, "ERROR!!");
sleep(2000);
stopAllTasks();
}
displayCenteredBigTextLine(0, "%4dmm", distance);
displayCenteredBigTextLine(3, "%4dmV", voltage);
sleep(50);
}
}
/**
* The main task
*/
task main(){
displayCenteredTextLine(0, "Dexter Ind.");
displayCenteredBigTextLine(1, "IMU");
displayCenteredTextLine(3, "Test 3");
displayCenteredTextLine(5, "Connect sensor");
displayCenteredTextLine(6, "to S1");
sleep(2000);
eraseDisplay();
// If configuration fails, the program ends.
if (!DIMUconfigAccel(DIMU, DIMU_ACC_RANGE_2G))
{
playSound(soundException);
while(bSoundActive){}
stopAllTasks();
}
while(nNxtButtonPressed == kNoButton)
{
z_val = DIMUreadAccelZAxis10Bit(DIMU);
x_val = DIMUreadAccelXAxis10Bit(DIMU);
// Since the axes are constantly 90 degrees apart, we can use the sum of forces law,
// which looks like the pythagorean theorem, to discover the total force along both axes.
Gforce = sqrt(pow(z_val, 2) + pow(x_val, 2));
// Then we divide both values received by the total force to get numbers on the interval [-1,1].
// This way we can input them into the arcsine and arccosine functions.
z_val = z_val/Gforce;
x_val = x_val/Gforce;
pXrads[0] = asin(x_val);
pXrads[1] = PI-pXrads[0]; //other possible X tilt value.
pZrads[0] = acos(z_val);
pZrads[1] = -1*pZrads[0]; //other possible Z tilt value.
normalize();
displayArrow(radiansToDegrees(match()));
// This stops the screen from flashing.
sleep(100);
}
}
task main()
{
setHeadLights(driveTrainReversed);
// Fake Autonomous
startTask(shooter_power_control);
set_shooter_targets(910);
startTask(AutoIntake);
delay(15000);
stopAllTasks();
// User Control
startTask(shooter_power_control);
startTask(controllerPolling);
startTask(driving);
while(true)
{
delay(105000);
}
}
task main () {
string _tmp;
displayCenteredTextLine(0, "HiTechnic");
displayCenteredBigTextLine(1, "Color V2");
displayCenteredTextLine(3, "Test 1");
displayCenteredTextLine(5, "Connect sensor");
displayCenteredTextLine(6, "to S1");
sleep(2000);
// Create struct to hold sensor data
tHTCS2 colorSensor;
// Initialise and configure struct and port
initSensor(&colorSensor, S1);
eraseDisplay();
while (true)
{
// Read the currently detected colour and RGB/HSV data from the sensor
if (!readSensor(&colorSensor)) {
displayTextLine(4, "ERROR!!");
sleep(2000);
stopAllTasks();
}
displayCenteredTextLine(0, "Color: %d", colorSensor.color);
displayCenteredBigTextLine(1, "R G B");
eraseRect(0,10, 99, 41);
fillRect( 0, 10, 30, 10 + (colorSensor.red+1)/8);
fillRect(35, 10, 65, 10 + (colorSensor.green+1)/8);
fillRect(70, 10, 99, 10 + (colorSensor.blue+1)/8);
StringFormat(_tmp, " %3d %3d", colorSensor.red, colorSensor.green);
displayTextLine(7, "%s %3d", _tmp, colorSensor.blue);
sleep(100);
}
}
task main() {
short _chVal = 0;
displayTextLine(3, "MAX127");
displayTextLine(4, "Test prog");
sleep(1000);
eraseDisplay();
while(true) {
eraseDisplay();
for (short i = 0; i < 8; i++) {
_chVal = MAX127readChan(MAX127, MAX127_I2C_ADDR, i);
// if the return value is < 0 then an error occurred
if (_chVal < 0) {
displayTextLine(4, "ERROR!!");
sleep(2000);
stopAllTasks();
}
displayTextLine(i, "CH[%d]: %d", i, _chVal);
}
sleep(100);
}
}
task main()
{
nNxtButtonTask = -2;
displayCenteredTextLine(0, "Codatex");
displayCenteredBigTextLine(1, "RFID");
displayCenteredTextLine(3, "Test 2");
displayCenteredTextLine(5, "Connect sensor");
displayCenteredTextLine(6, "to S1");
sleep(2000);
eraseDisplay();
displayCenteredTextLine(3, "Start single");
displayCenteredTextLine(4, "reading loop");
sleep(2000);
eraseDisplay();
// Set up the sensor for continuous readings.
CTRFIDsetContinuous(CTRFID);
// loop for transponder readings with continuous read function
while(nNxtButtonPressed == kNoButton) {
// read the transponder
if (!CTRFIDreadTransponder(CTRFID, transponderID)) {
eraseDisplay();
displayTextLine(3, "Error reading");
displayTextLine(4, "from sensor!");
sleep(5000);
stopAllTasks();
}
displayCenteredTextLine(3, "Transponder ID:");
displayCenteredTextLine(4, "%s", transponderID);
// Be sure to add about 200ms after each read
// or you end up getting 0000000000 as a transponder address
sleep(200);
}
}
task main () {
displayCenteredTextLine(0, "HiTechnic");
displayCenteredBigTextLine(1, "Accel");
displayCenteredTextLine(3, "Test 1");
displayCenteredTextLine(5, "Connect sensor");
displayCenteredTextLine(6, "to S1");
sleep(2000);
playSound(soundBeepBeep);
while(bSoundActive) sleep(1);
// Create struct to hold sensor data
tHTAC accelerometer;
// Initialise and configure struct and port
initSensor(&accelerometer, S1);
while (true) {
eraseDisplay();
// Read all of the axes at once
if (!readSensor(&accelerometer)) {
displayTextLine(4, "ERROR!!");
sleep(2000);
stopAllTasks();
}
displayTextLine(0,"HTAC Test 1");
displayTextLine(2, " X Y Z");
displayTextLine(3, "%4d %4d %4d", accelerometer.x, accelerometer.y, accelerometer.z);
// Alternatively, you can read them like this:
displayTextLine(4, "%4d %4d %4d", accelerometer.axes[0], accelerometer.axes[1], accelerometer.axes[2]);
sleep(100);
}
}
task main()
{
initializeRobot();
clearTimer(T1);
bool isLaunching = false;
bool isDropping = false;
int mThreshold = 15; // Sets dead zones to avoid unnecessary movement
int aThreshold = 30;
int xVal, yVal; //Stores left analog stick values
float scaleFactor = 40.0 / 127; //Sets max. average motor power and maps range of analog stick to desired range of power
//waitForStart(); // wait for start of tele-op phase
int num = 0;
while (true)
{
getJoystickSettings(joystick); // Retrieves data from the joystick
//4 is forward, 2 is backwards, 7 is left, 8 is right
if(joy1Btn(4) == 1){
if(num == 4) {
motor[motorD] = 20;
motor[motorE] = 20;
} else {
finishAction(num);
num = 4;
}
} else if (joy1Btn(2) == 1){
if(num == 2) {
motor[motorD] = -23;
motor[motorE] = -23;
} else {
finishAction(num);
num = 2;
}
} else if (joy1Btn(1) == 1){
if(num == 1) {
turn(1);
} else {
finishAction(num);
num = 1;
}
}else if (joy1Btn(3) == 1){
if(num == 3) {
turn(-1);
} else {
finishAction(num);
num = 3;
}
//raise scissor lift (positive motor power)
/*
} else if (joy1Btn(9) == 1){
if(num == 9 && flag9) {
flag9 = false;
releaseBallCollector();
//
} else {
finishAction(num);
num = 9;
}
*/
/*} else if (joy1Btn(6) == 1){
if(num == 13 && flag13) {
flag13 = false;
raiseGrabber();
//
} else {
finishAction(num);
num = 13;
}//raise grabber
} else if (joy1Btn(7) == 1){
if(num == 14 && flag14) {
flag14 = false;
lowerGrabber();
//
} else {
finishAction(num);
num = 14;
}
//lower grabber */
}else if (joy1Btn(7) == 1){
clearDebugStream();
writeDebugStreamLine(path);
stopAllTasks();
}else{
if(num != 0) {
finishAction(num);
num = 0;
}
//RECORD CURRENT VARIABLES, THEN RESET EVERYTHING
}
//Controls launcher
//if(joy1Btn(1) == 1 && isLaunching == false){
//isLaunching = true;
//fireLauncher();
//isLaunching = false;
//}
//wait1Msec(1);
}
}
task main () {
bool selected_50hz = true;
displayCenteredTextLine(0, "HiTechnic");
displayCenteredBigTextLine(1, "Color V2");
displayCenteredTextLine(4, "Config operating");
displayCenteredTextLine(5, "frequency to");
displayCenteredTextLine(6, "50 or 60 Hz");
sleep(2000);
eraseDisplay();
displayCenteredTextLine(0, "Use arrow keys");
displayCenteredTextLine(1, "to select a");
displayCenteredTextLine(2, "frequency");
displayCenteredBigTextLine(4, "50 60");
displayCenteredTextLine(6, "[enter] to set");
displayCenteredTextLine(7, "[exit] to cancel");
drawRect(19, 34, 44, 16);
while (true) {
while (nNxtButtonPressed == kNoButton) {
sleep(1);
}
switch (nNxtButtonPressed) {
// if the left button is pressed, set the sensor for 50Hz
case kLeftButton:
if (selected_50hz) {
playSound(soundBlip);
while(bSoundActive) {sleep(1);}
} else {
selected_50hz = true;
eraseRect(55, 34, 80, 16);
displayCenteredBigTextLine(4, "50 60");
drawRect(19, 34, 44, 16);
}
break;
// if the right button is pressed, set the sensor for 60Hz
case kRightButton:
if (!selected_50hz) {
playSound(soundBlip);
while(bSoundActive) {sleep(1);}
} else {
selected_50hz = false;
eraseRect(19, 34, 44, 16);
displayCenteredBigTextLine(4, "50 60");
drawRect(55, 34, 80, 16);
}
break;
// Make the setting permanent by saving it to the sensor
case kEnterButton:
eraseDisplay();
if (selected_50hz)
setHz(HTCS2, 50);
else
setHz(HTCS2, 60);
displayCenteredTextLine(2, "The Sensor is");
displayCenteredTextLine(3, "configured for");
if (selected_50hz)
displayCenteredTextLine(4, "50 Hz operating");
else
displayCenteredTextLine(4, "60 Hz operating");
displayCenteredTextLine(5, "frequency");
for (short i = 5; i > 0; i--) {
displayCenteredTextLine(7, "Exiting in %d sec", i);
sleep(1000);
}
stopAllTasks();
break;
}
// Debounce the button
while (nNxtButtonPressed != kNoButton) {
sleep(1);
}
}
}
task main() {
float temp;
tLEGOTMPAccuracy accuracy;
string strAcc;
displayCenteredTextLine(0, "LEGO");
displayCenteredBigTextLine(1, "Temp");
displayCenteredTextLine(3, "Test 1");
displayCenteredTextLine(5, "Connect sensor");
displayCenteredTextLine(6, "to S1");
sleep(2000);
eraseDisplay();
// Setup the sensor for continuous mode
LEGOTMPsetContinuous(LEGOTMP);
//setting minimum accuracy
accuracy = A_MIN;
if (!LEGOTMPsetAccuracy(LEGOTMP, accuracy)) {
displayTextLine(0, "Error setAccuracy");
sleep(5000);
stopAllTasks();
}
//reads the current accuracy of the sensor
if (!LEGOTMPreadAccuracy(LEGOTMP, accuracy)) {
displayTextLine(0, "Error readAccuracy");
sleep(5000);
stopAllTasks();
}
accuracyToString(accuracy, strAcc);
displayTextLine(0, "Accuracy: %s", strAcc);
//loop to read temp
while (true) {
switch(nNxtButtonPressed) {
// If the left button is pressed, decrease the accuracy
case kLeftButton:
switch(accuracy) {
case A_MIN: accuracy = A_MAX; break;
case A_MEAN1: accuracy = A_MIN; break;
case A_MEAN2: accuracy = A_MEAN1; break;
case A_MAX: accuracy = A_MEAN2; break;
}
if (!LEGOTMPsetAccuracy(LEGOTMP, accuracy)) {
displayTextLine(0, "Error setAccuracy");
sleep(5000);
stopAllTasks();
}
accuracyToString(accuracy, strAcc);
displayTextLine(0, "Accuracy: %s", strAcc);
// debounce the button
while (nNxtButtonPressed != kNoButton) sleep(1);
break;
// If the right button is pressed, increase the accuracy
case kRightButton:
switch(accuracy) {
case A_MIN: accuracy = A_MEAN1; break;
case A_MEAN1: accuracy = A_MEAN2; break;
case A_MEAN2: accuracy = A_MAX; break;
case A_MAX: accuracy = A_MIN; break;
}
if (!LEGOTMPsetAccuracy(LEGOTMP, accuracy)) {
displayTextLine(0, "Error setAccuracy");
sleep(5000);
stopAllTasks();
}
accuracyToString(accuracy, strAcc);
displayTextLine(0, "Accuracy: %s", strAcc);
// debounce the button
while (nNxtButtonPressed != kNoButton) sleep(1);
break;
}
if (!LEGOTMPreadTemp(LEGOTMP, temp)) {
eraseDisplay();
displayTextLine(0, "Temp reading pb");
sleep(100);
stopAllTasks();
}
displayCenteredBigTextLine(2, "Temp:");
switch(accuracy) {
case A_MIN: displayCenteredBigTextLine(4, "%4.1f", temp); break;
case A_MEAN1: displayCenteredBigTextLine(4, "%5.2f", temp); break;
case A_MEAN2: displayCenteredBigTextLine(4, "%6.3f", temp); break;
case A_MAX: displayCenteredBigTextLine(4, "%7.4f", temp); break;
}
}
}
