// 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();
}
}
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 raw = 0;
short nrm = 0;
nNxtButtonTask = -2;
eraseDisplay();
displayTextLine(0, "Dexter Industries");
displayCenteredBigTextLine(1, "dFlex");
displayCenteredTextLine(3, "Test 1");
displayCenteredTextLine(5, "Connect sensor");
displayCenteredTextLine(6, "to S1");
sleep(2000);
eraseDisplay();
displayTextLine(0, "dFlex Sensor");
while (true) {
displayClearTextLine(5);
displayClearTextLine(6);
// Get the raw value from the sensor
raw = DFLEXvalRaw(DFLEX);
// Get the normalised value from the sensor
nrm = DFLEXvalNorm(DFLEX);
displayTextLine(2, "Raw: %4d", raw);
displayTextLine(4, "Norm: %4d", nrm);
sleep(50);
}
}
task main(){
displayCenteredTextLine(0, "Dexter Ind.");
displayCenteredBigTextLine(1, "IMU");
displayCenteredTextLine(3, "Test 2");
displayCenteredTextLine(5, "Connect sensor");
displayCenteredTextLine(6, "to S1");
sleep(2000);
eraseDisplay();
// Fire up the gyro and initialize it. Only needs to be done once.
//DIMUconfigGyro(DIMU, DIMU_GYRO_RANGE_500);
if (!DIMUconfigAccel(DIMU, DIMU_ACC_RANGE_2G))
playSound(soundException);
if(!DIMUconfigGyro(DIMU, DIMU_GYRO_RANGE_250, true))
playSound(soundException);
for (short i = 0; i < 500; i++){
// Read the GYROSCOPE
// There are 3 ways to do this:
// All at once, very convenient if you need all 3
DIMUreadGyroAxes(DIMU, xvals[i], yvals[i], zvals[i]);
sleep(5);
}
for (short i = 0; i< 500; i++) {
writeDebugStream("%f, %f", xvals[i], yvals[i]);
writeDebugStreamLine(", %f", zvals[i]);
sleep(2);
}
}
// Allow the user to calibrate the scales
void calibrateScales()
{
short calibrateWeight = 0;
eraseDisplay();
displayCenteredTextLine(0, "GlideWheel-AS");
displayCenteredTextLine(2, "Place the object");
displayCenteredTextLine(3, "on the scales");
displayCenteredTextLine(4, "and press");
displayCenteredTextLine(5, "[enter]");
displayCenteredTextLine(6, "to calibrate");
while (nNxtButtonPressed != kEnterButton) sleep(1);
debounce();
eraseDisplay();
calibrateWeight = weighObject();
displayCenteredTextLine(0, "GlideWheel-AS");
displayCenteredTextLine(2, "Enter the weight");
displayCenteredTextLine(3, "in grams");
displayCenteredTextLine(7, "- OK +");
while (true)
{
displayCenteredBigTextLine(5, "%d", calibrateWeight);
switch(nNxtButtonPressed)
{
case kLeftButton: playTone(500,10); calibrateWeight--; calibrateWeight = max2(0, calibrateWeight); break;
case kRightButton: playTone(1000,10); calibrateWeight++; break;
case kEnterButton: playTone(1500,10);gramsPerUnit = (float)calibrateWeight / (float)MSANGreadRaw(MSANG); eraseDisplay(); return;
}
sleep(20);
debounce();
}
}
task main()
{
disableDiagnosticsDisplay();
/*
motor[leftFront] = 50;
wait1Msec(1000);
motor[leftFront] = 0;
motor[rightFront] = 50;
wait1Msec(1000);
motor[rightFront] = 0;
motor[leftBack] = 50;
wait1Msec(1000);
motor[leftBack] = 0;
motor[rightBack] = 50;
wait1Msec(1000);
motor[rightBack] = 0;
*/
//travelDistance(45, dForward);
//gyroTurn(50, 35, dLeft);
forward(75);
wait1Msec(1000);
stopMotors();
displayCenteredTextLine(1, "lf = %i", nMotorEncoder[leftFront]);
displayCenteredTextLine(2, "lb = %i", nMotorEncoder[leftBack]);
displayCenteredTextLine(3, "rf = %i", nMotorEncoder[rightFront]);
displayCenteredTextLine(4, "rb = %i", nMotorEncoder[rightBack]);
wait1Msec(30000);
}
task main () {
nNxtButtonTask = -2;
displayCenteredTextLine(0, "Mindsensors");
displayCenteredBigTextLine(1, "Angle");
displayCenteredTextLine(3, "Test 1");
displayCenteredTextLine(5, "Connect sensor");
displayCenteredTextLine(6, "to S1");
sleep(2000);
eraseDisplay();
displayCenteredTextLine(0, "GlideWheel-AS");
displayTextLine(1, "-------------------");
displayTextLine(5, "-------------------");
while (true) {
// Reset the angle to 0
if (nNxtButtonPressed == kEnterButton)
{
MSANGresetAngle(MSANG);
while (nNxtButtonPressed != kNoButton) sleep(1);
}
// Read the current angle, accumulated angle and RPM and display them
displayTextLine(2, "Ang: %7d deg", MSANGreadAngle(MSANG));
displayTextLine(3, "Raw: %7d", MSANGreadRaw(MSANG));
displayTextLine(4, "RPM: %7d", MSANGreadRPM(MSANG));
displayTextLine(7, " Reset angle");
sleep(50);
}
}
task main(){
// This struct holds all the sensor related data
tDIMC compass;
displayCenteredTextLine(0, "Dexter Ind.");
displayCenteredBigTextLine(1, "dCompass");
displayCenteredTextLine(3, "Test 1");
displayCenteredTextLine(5, "Connect sensor");
displayCenteredTextLine(6, "to S1");
sleep(2000);
eraseDisplay();
// Fire up the compass and initialize it. Only needs to be done once.
if (!initSensor(&compass, DIMC))
playSound(soundException);
sleep(100);
while (true){
// Read the Compass
if (!readSensor(&compass))
playSound(soundException);
displayCenteredBigTextLine(2, "%3.2f", compass.heading);
displayTextLine(5, "%d", compass.axes[0]);
displayTextLine(6, "%d", compass.axes[1]);
displayTextLine(7, "%d", compass.axes[2]);
sleep(50);
}
}
task main () {
displayCenteredTextLine(0, "Mindsensors");
displayCenteredBigTextLine(1, "TMUX");
displayCenteredTextLine(3, "SMUX Test");
displayCenteredTextLine(5, "Connect SMUX to");
displayCenteredTextLine(6, "S1 and TMUX to");
displayCenteredTextLine(7, "SMUX Port 1");
sleep(2000);
while (true) {
eraseDisplay();
displayTextLine(0, "MS Touch MUX");
// Get the raw data from the sensor, this is not processed
// by the driver in any way.
displayTextLine(1, "Raw: %d", 1023 - HTSMUXreadAnalogue(MSTMUX));
// Go through each possible touch switch attached to the TMUX
// and display whether or not is active (pressed)
for (short i = 1; i < 4; i++) {
if (MSTMUXisActive(MSTMUX, i))
displayTextLine(i+2, "Touch %d: on", i);
else
displayTextLine(i+2, "Touch %d: off", i);
}
// Display the binary value of the active touch switches
// 0 = no touch, 1 = touch 1 active, 2 = touch 2 active, etc.
// touch 1 + touch 2 active = 1 + 2 = 3.
displayTextLine(7, "Status: %d", MSTMUXgetActive(MSTMUX));
sleep(50);
}
}
task main(){
short heading;
short x_val, y_val, z_val; // axis values
displayCenteredTextLine(0, "Mindsensors");
displayCenteredBigTextLine(1, "IMU");
displayCenteredTextLine(3, "Test 2");
displayCenteredTextLine(5, "Connect sensor");
displayCenteredTextLine(6, "to S1");
sleep(2000);
eraseDisplay();
while (true){
// Read the Compass
heading = MSIMUreadHeading(MSIMU);
displayTextLine(1, "%d", heading);
// Read the tilt
MSIMUreadTiltAxes(MSIMU, x_val, y_val, z_val);
displayTextLine(5, "%d", x_val);
displayTextLine(6, "%d", y_val);
displayTextLine(7, "%d", z_val);
sleep(50);
}
}
void createTeleopConfigFile(string &sExecutableName)
{
TFileIOResult nIoResult;
TFileHandle hFileHandle;
short nFileSize;
deleteTeleOpConfigFile(); // Erase existing file
// Create the file
nFileSize = strlen(sExecutableName) + 4;
OpenWrite(hFileHandle, nIoResult, sTextFileName, nFileSize);
WriteText(hFileHandle, nIoResult, sExecutableName);
WriteText(hFileHandle, nIoResult, ".rxe");
Close(hFileHandle, nIoResult);
// Display Message
displayTextLine(5, "");
if(nIoResult == ioRsltSuccess)
displayCenteredTextLine(6, "File Created" );
else
displayCenteredTextLine(6, "File Error");
displayTextLine(7, "");
wait1Msec(1250);
return;
}
void autoFloor(bool useLift)
{
int irValue = getIRReading();
strafeDist(40, 100, dRight);
displayCenteredTextLine(1, "IR = %i", irValue);
if (irValue == 2)
{
strategyA(useLift);
}
else
{
irValue = getIRReading();
if (irValue == 2)
{
strategyA(useLift);
}
else
{
gyroTurn(10, 5, dRight);
int irValue1 = getIRReading();
gyroTurn(10, 10, dLeft);
int irValue2 = getIRReading();
eraseDisplay();
displayCenteredTextLine(2, "IR1 = %i", irValue1);
displayCenteredTextLine(3, "IR2 = %i", irValue2);
if (irValue1 == 2 || irValue2 == 2)
{
gyroTurn(10, 5, dRight);
strategyA(useLift);
}
else
{
strafeDist(40, 75, dRight);
gyroTurn(50, 30, dLeft);
resetEncoders();
while (irValue != 6)
{
irValue = getIRReading();
strafe(100);
int enc = abs(nMotorEncoder[leftBack]);
displayCenteredTextLine(3, "distance=%i", enc);
wait1Msec(1);
}
stopMotors();
int enc = abs(nMotorEncoder[leftBack]);
displayCenteredTextLine(1, "enc = %i", enc);
if (enc < 1500)
{
strategyB(useLift);
}
else
{
strategyC(useLift);
}
}
}
}
}
// main task
task main ()
{
displayCenteredTextLine(0, "HiTechnic");
displayCenteredBigTextLine(1, "IR Seekr");
displayCenteredTextLine(3, "Test 1");
displayCenteredTextLine(5, "Press enter to");
displayCenteredTextLine(6, "switch between");
displayCenteredTextLine(7, "600 and 1200 Hz");
sleep(2000);
// Create struct to hold sensor data
tHTIRS2 irSeeker;
// Initialise and configure struct and port
initSensor(&irSeeker, S1);
while(true)
{
// You can switch between the two different DSP modes by pressing the
// orange enter button
playSound(soundBeepBeep);
while(bSoundActive)
{}
eraseDisplay();
// display the current DSP mode
if (irSeeker.mode == DSP_1200)
displayTextLine(0, " DC 1200");
else
displayTextLine(0, " DC 600");
while (true)
{
if (getXbuttonValue(xButtonEnter))
{
// "Enter" button has been pressed. Need to switch mode
irSeeker.mode = (irSeeker.mode == DSP_1200) ? DSP_600 : DSP_1200;
while(getXbuttonValue(xButtonEnter))
{
sleep(1);
}
break;
}
// Read the sensor data.
readSensor(&irSeeker);
displayTextLine(1, "D:%4d %4d 3%d", irSeeker.dcDirection, irSeeker.acDirection, irSeeker.enhDirection);
displayTextLine(2, "0:%4d %d", irSeeker.dcValues[0], irSeeker.acValues[0]);
displayTextLine(3, "0:%4d %4d", irSeeker.dcValues[1], irSeeker.acValues[1]);
displayTextLine(4, "0:%4d %4d %3d", irSeeker.dcValues[2], irSeeker.acValues[2], irSeeker.enhStrength);
displayTextLine(5, "0:%4d %4d", irSeeker.dcValues[3], irSeeker.acValues[3]);
displayTextLine(6, "0:%4d %4d", irSeeker.dcValues[4], irSeeker.acValues[4]);
displayTextLine(7, "Enter to switch");
}
}
}
task main()
{
displayCenteredTextLine(0, "Station");
displayCenteredBigTextLine(1, "RoboTaxi");
displayCenteredTextLine(3, "Using A*");
displayCenteredTextLine(5, "Ashesi");
displayCenteredTextLine(6, "Fall 2015");
sleep(5000);
eraseDisplay();
//store input from the numeric keypad
base_station();
//displays values received as location and destination
displayTextLine(1, "Test Values: ");
displayTextLine(2, "Xval: %d", passenger_location_x);
displayTextLine(3, "Yval: %d", passenger_location_y);
displayTextLine(4, "XDest: %d", passenger_destination_x);
displayTextLine(5, "YDest: %d", passenger_destination_y);
wait1Msec(100);
goal_x = passenger_destination_x;
goal_y = passenger_destination_y;
//find goal with taxi1 in mind
findGoal(taxi1base_x, taxi1base_y, passenger_location_x, passenger_location_y);
//get distance from taxi1 to passenger
int dist1 = get_distance(taxi1base_x, taxi1base_y, passenger_location_x, passenger_location_y);
//find goal with taxi2 in mind
findGoal(taxi2base_x, taxi2base_y, passenger_location_x, passenger_location_y);
//get distance from taxi2 to passenger
int dist2 = get_distance(taxi2base_x, taxi2base_y, passenger_location_x, passenger_location_y);
//compare distances between taxis and passengers. select taxi with shorter distance
if (dist1 < dist2){//if taxi1 is shorter
eraseDisplay();
displayTextLine(1, "taxi 1 ");
wait1Msec(500);
sendMessageWithParm(TAXI1LOC,passenger_location_x,passenger_location_y);//11 is location to taxi1
wait1Msec(500);
displayTextLine(1, "to taxi 1.1 ");
sendMessageWithParm(TAXI1DEST,passenger_destination_x,passenger_destination_y);//12 is destination to taxi1
wait1Msec(500);
displayTextLine(1, "to taxi 1.2 ");
}
else {//if taxi2 is shorter
displayTextLine(1, "taxi 2 ");
wait1Msec(500);
sendMessageWithParm(TAXI2LOC,passenger_location_x,passenger_location_y);//21 is location to taxi2
wait1Msec(500);
displayTextLine(1, "to taxi 2.1 ");
sendMessageWithParm(TAXI2DEST,passenger_destination_x,passenger_destination_y);//22 is destination to taxi2
wait1Msec(500);
displayTextLine(1, "to taxi 2.2 ");
}
}
// Display the instructions to the user
void displayInstructions()
{
displayCenteredTextLine(0, "HiTechnic");
displayCenteredBigTextLine(1, "IR Seekr");
displayCenteredTextLine(3, "Test 1");
displayCenteredTextLine(5, "Press enter to");
displayCenteredTextLine(6, "switch between");
displayCenteredTextLine(7, "600 and 1200 Hz");
sleep(2000);
}
/**
* Show a fancy startup display
*/
void StartupDisplay()
{
displayCenteredBigTextLine(0, "WiFi");
displayCenteredTextLine(3, "Program to test");
displayCenteredTextLine(4, "Network with ");
displayCenteredTextLine(5, "WifiBlock");
sleep(2000);
eraseDisplay();
return;
}
/*
=============================================================================
main task with some testing code
*/
task main() {
// Standard range is set to short range
bool shortrange = true;
tObstacleZone zone = MSSUMO_NONE;
nNxtButtonTask = -2;
eraseDisplay();
displayCenteredTextLine(0, "Mindsensors");
displayCenteredBigTextLine(1, "SUMO Eyes");
displayCenteredTextLine(3, "Test 1");
displayCenteredTextLine(5, "Press enter to");
displayCenteredTextLine(6, "switch between");
displayCenteredTextLine(7, "ranges");
sleep(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);
displayClearTextLine(1);
displayTextLine(1, "Short range");
shortrange = true;
} else {
// set the sensor to long range and display this
MSSUMOsetLongRange(HTMSSUMO);
displayClearTextLine(1);
displayTextLine(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: displayCenteredBigTextLine(4, "FRONT"); break;
case MSSUMO_LEFT: displayCenteredBigTextLine(4, "LEFT"); break;
case MSSUMO_RIGHT: displayCenteredBigTextLine(4, "RIGHT"); break;
case MSSUMO_NONE: displayCenteredBigTextLine(4, "NONE"); break;
}
sleep(50);
}
}
}
task main() {
displayCenteredTextLine(0, "Mindsensors");
displayCenteredBigTextLine(1, "PFMate");
displayCenteredTextLine(3, "Test 1");
sleep(2000);
// Run through each channel for testing.
for (short channel = 1; channel < 5; channel++) {
doTest(channel);
}
}
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);
}
}
task main()
{
// Clear all text from the debugstream window
clearDebugStream();
//sets LED to flash to show that the printer is printing
setLEDColor(ledRed);
//credits
displayCenteredTextLine(2, "Made by Xander Soldaat");
displayCenteredTextLine(4, "and Cyrus Cuenca");
//verion number
displayCenteredTextLine(6, "Version 1.1");
//GitHub link
displayCenteredTextLine(8, "http://github.com/cyruscuenca/g-pars3");
//supported commands
displayCenteredTextLine(10, "Supported commands: G1");
float x, y, z, e, f = 0.0;
long fd = 0;
char buffer[128];
long lineLength = 0;
string gcmd = "G1"; // always a g1
fd = fileOpenRead(fileName); // fileName = gcode.rtf
if (fd < 0) // if file is not found/cannot open
{
writeDebugStreamLine("Could not open %s", fileName);
return;
}
while (true)
{
lineLength = readLine(fd, buffer, 128);
if (lineLength > 0)
{
readNextCommand(buffer, lineLength, x, y, z, e, f);
executeCommand(gcmd, x, y, z, e, f);
}
else
// we're done, no lines left to read from the file
return;
// Wipe the buffer by setting its contents to 0
memset(buffer, 0, sizeof(buffer));
//LED turns green to show that the print is done
setLEDColor(ledGreen);
}
}
task main()
{
int optSel1 = GetOptVal_StartPosition();
int optSel2 = GetOptVal_PowerSettings_SpanScreens();
int optSel3 = GetOptVal_IR_Approach();
eraseDisplay();
displayCenteredTextLine(1, "Option 1: %2d", optSel1);
displayCenteredTextLine(2, "Option 2: %2d", optSel2);
displayCenteredTextLine(3, "Option 3: %2d", optSel3);
WAIT_SCREEN ws;
ws.line6 = "Last Screen";
ws.line7 = "...";
WaitForButtonPress(ws);
}
void strategyY() //use if in position 2, from ramp
{
displayCenteredTextLine(1, "strategy Y"); //display which strategy it chose
travelDistance(distanceY1, dForward); //travel forward a certain distance
gyroTurn(30, 45, dLeft); //turn left 45 degrees
travelDistance(distanceY1, dForward); //travel forward a certain distance
}
//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 () {
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 soundraw = 0;
short soundnorm = 0;
bool dba = false;
nNxtButtonTask = -2;
displayCenteredTextLine(0, "Lego");
displayCenteredBigTextLine(1, "Sound");
displayCenteredTextLine(3, "SMUX Test");
displayCenteredTextLine(5, "Connect SMUX to");
displayCenteredTextLine(6, "S1 and snd sensor");
displayCenteredTextLine(7, "to SMUX Port 1");
sleep(2000);
eraseDisplay();
displayTextLine(0, "Lego Sound Sensor");
displayTextLine(6, "[enter] to switch");
displayTextLine(7, "dB and dBA mode");
// Set the sensor to dB mode.
SNDsetDB(LEGOSND);
displayCenteredTextLine(1, "dB mode");
while(true) {
// The enter button has been pressed, switch
// to the other mode
if (nNxtButtonPressed == kEnterButton) {
dba = !dba;
if (!dba) {
// set the sensor to DB mode
SNDsetDB(LEGOSND);
displayCenteredTextLine(1, "dB mode");
} else {
// set the sensor to dBA mode.
SNDsetDBA(LEGOSND);
displayCenteredTextLine(1, "dBA mode");
}
// wait 500ms to debounce the switch
sleep(500);
}
// Read the normalised value of the sensor
soundraw = SNDreadRaw(LEGOSND);
// Display the raw and normalised values
soundnorm = SNDreadNorm(LEGOSND);
// display the info from the sensor
displayTextLine(3, "Raw: %3d", soundraw);
displayTextLine(4, "Norm: %3d", soundnorm);
sleep(50);
}
}
task main () {
// declare and initialise the sensor
tTIR tir;
initSensor(&tir, S1);
displayCenteredTextLine(0, "Dexter Industries");
displayCenteredTextLine(1, "Thermal Infrared");
displayCenteredTextLine(3, "Test 1");
displayCenteredTextLine(5, "Connect sensor");
displayCenteredTextLine(6, "to S1");
sleep(2000);
eraseDisplay();
// set emissivity for light skin
setEmissivity(&tir, TIR_EM_SKIN_LIGHT);
sleep(200);
displayCenteredTextLine(0, "Dexter Industries");
displayCenteredTextLine(1, "Thermal Infrared");
while (true) {
// Read the currently detected ambient and object temp from the sensor
readSensor(&tir);
displayTextLine(3, "A: %3.2f", tir.ambientTemp);
displayTextLine(4, "O: %3.2f", tir.objectTemp);
sleep(100);
}
}
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);
}
}
void strategyZ() //use if in position 1, from ramp
{
displayCenteredTextLine(1, "strategy Z"); //display which strategy it chose
travelDistance(distanceZ1, dForward); //
gyroTurn(30, 90, dLeft);
travelDistance(distanceZ2, dForward);
gyroTurn(30, 90, dRight);
travelDistance(distanceZ3, dForward);
}
/**
* 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()
{
int timeToWait = requestTimeToWait();
initializeRobot();
waitForStart();
disableDiagnosticsDisplay();
//Initialize the gyro and turning
GyroInit(g_Gyro, gyro, 0);
PidTurnInit(g_PidTurn, leftDrive, rightDrive, MIN_TURN_POWER, g_Gyro, TURN_KP, TURN_TOLERANCE);
countdown(timeToWait);
//Start actual movement code
moveForwardInches(60,43);//initial forwards
turn(g_PidTurn,45,20);
clearEncoders();
wait1Msec(50);
const int totalTics = 6806;//total tics from before IR to end- CHANGED FOR LESSENED AMOUNT OF FORWARDS
const int ticsToCenter = 3663;//tics from start to central beam
const int ticsToSubtract = 1665;//failsafe, may still need testing
//finding IR
while(HTIRS2readACDir(IR) != 4 && (abs(nMotorEncoder[leftDrive]) < totalTics - ticsToSubtract)){ //finds the beacon zone 4 (rough)
//nxtDisplayCenteredTextLine(5,"Direction:%d",HTIRS2readACDir(IR));
startBackward(27);
}
stopDrive();
wait1Msec(300);
while(HTIRS2readACDir(IR) != 5 && (abs(nMotorEncoder[leftDrive]) < totalTics - ticsToSubtract)){ //slow down to look for basket (fine)
startForward(15);
}
int currentPosition = abs(nMotorEncoder[leftDrive]);
if (currentPosition > ticsToCenter)//check where we are
moveForwardInchesNoReset(20, 6);//move forwards 5 inches (buckets 1 and 2)
else
moveForwardInchesNoReset(20, 3);//forwards 3 inches (buckets 3 and 4)
stopDrive();//stops robot
servo[dumper] = 30;//dumps the block
motor[lift]= 50;//starts the lift up
wait1Msec(700);
motor[lift]= 0;//stops lift
servo[dumper] = servoRestPosition;//resets servo
int ticsToMove= totalTics - abs(nMotorEncoder[leftDrive]);//tics left after IR
displayCenteredTextLine(0,"TTM: %d", ticsToMove);
moveBackwardTics(90, ticsToMove); //move to end after IR
turn(g_PidTurn, -87,40); //turn to go towards ramp
moveForwardInches(90, 44, false, LEFTENCODER); //forwards to ramp
turn(g_PidTurn, 95, 40); //turn to face ramp
moveForwardInches(90, 45, false, LEFTENCODER);//onto ramp
motor[lift]= -50;//starts the lift down
wait1Msec(500);
motor[lift]= 0;//stops lift
while(true){}
}
