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 () {
long encA = 0;
long encB = 0;
long distance = 0;
long mindist = 0;
long maxdist = 0;
int motorSpeed = 0;
eraseDisplay();
MSMMUXinit();
MSMotorStop(mmotor_S1_1);
MSMotorStop(mmotor_S1_2);
wait1Msec(500);
// Reset the encoders. This can be done individually or all at once.
// You should do this at the start of your program.
MSMMotorEncoderResetAll(MSMMUX);
// Read the minimum distance the sensor can "see"
mindist = MSDISTreadMinDist(MSDIST);
// Read the maximum distance the sensor can "see"
maxdist = MSDISTreadMaxDist(MSDIST);
while (true) {
distance = MSDISTreadDist(MSDIST);
// Retrieve the motor-MUX's encoder counts
encA = MSMMotorEncoder(mmotor_S1_1);
encB = MSMMotorEncoder(mmotor_S1_2);
// calculate the motor speed
motorSpeed = ((distance - mindist) * 100) / (maxdist - mindist);
// Tell the motors to start moving.
MSMMotor(mmotor_S1_1, motorSpeed);
MSMMotor(mmotor_S1_2, motorSpeed);
// Display the info.
nxtDisplayTextLine(4, "D: %5d", distance);
nxtDisplayTextLine(5, "A: %5d (%3d)", encA, motorSpeed);
nxtDisplayTextLine(6, "B: %5d (%3d)", encB, motorSpeed);
wait1Msec(20);
}
}
