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 ");
}
}
task main(){
checkBTLinkConnected();
dist = 0;
time = 0;
nMotorEncoder[motorA] = 0;
while(true){
wait1Msec(1);
dist = nMotorEncoder[motorA];
if(time == 100){
if(dist != 0){
sendMessageWithParm(dist);
}
time = 0;
}
nxtDisplayTextLine(1,"dist = %d", dist);
if(nNxtButtonPressed == kEnterButton) {
nMotorEncoder[motorA] = 0;
}
++time;
}
}
// Go through the vector, set bricks, and move conveyor after each row
void writeLetter(char* letter, int size)
{
for(int i=0; i<size; i++)
{
// move the conveyor
if (i!=0 && i%5==0){
sendMessageWithParm(CONVEYOR, CONVEYOR_MOVE, 1);
PlaySound(soundBeepBeep);
wait1Msec(500);
}
if (letter[i]=='1'){
sendMessageWithParm(WEBSERVER, STT_PRINTING, i);
setBrick(i%5,0);
// Calibrate after each brick placement
moveToOrigin();
}
}
}
task main()
{
while (true) // We want the function to run all the time
{
wait1Msec(50);
if(nNxtButtonPressed == 1) // If we press the Right Arrow
{
buttonTest = 1; // We send "1"
nxtDisplayTextLine(4,"Button: %d",buttonTest); // We present the result on the screen
}
else if (nNxtButtonPressed == 2) // If we press the Left Arrow
{
buttonTest= 2; // We send "2"
nxtDisplayTextLine(4,"Button: %d",buttonTest);
}
else if (nNxtButtonPressed == 3) // If we press the Orange Square
{
buttonTest = 3; // We send "3"
nxtDisplayTextLine(4,"Button: %d",buttonTest);
}
else if (SensorValue [touchSensor] == 1) // If the SensorTouch is pressed
{
buttonTest = 4; // We send "4"
nxtDisplayTextLine(4,"Button: %d",buttonTest);
}
else // If Nothing is pressed
{
buttonTest = 0; // We send "0"
nxtDisplayTextLine(4,"No Button Pressed");
}
measureRPM(); // We call the function to measure the speed of dial (motorC)
sendMessageWithParm(buttonTest, speed, testMessage);
wait1Msec(50);
}
}
// Pick a brick from warehouse and plug it in
void setBrick(int i, int j)
{
// how deep should it go
const float down = 3;
driveNipple(5.2,30,MoveMotor);
// If no brick wait 2,5 sec and check again
while (!haveBrick()){
// send to server error message
sendMessageWithParm(WEBSERVER, ERR_NO_BRICKS, 0);
wait1Msec(2500);
}
driveNipple(5.2,-30,MoveMotor);
// loading
driveGear(down,15,LiftMotor, LiftGear);
wait1Msec(500);
driveGear(down,-30,LiftMotor, LiftGear);
wait1Msec(500);
moveToOrigin(); // calibrate
// move to plate
driveNipple(i+5 ,20,MoveMotor);
driveGear(0.8,10,MoveMotor, MoveGear);
//driveGear(1,-20,MoveMotor, MoveGear);
wait1Msec(500);
// printing
//driveGear(down,15,LiftMotor, LiftGear);
//wait1Msec(10);
plugInBrick(down);
wait1Msec(500);
driveGear(down,-30,LiftMotor, LiftGear);
wait1Msec(500);
//haveBrick();
}
// Plug the brick to the plate
void plugInBrick(float down){
// loading
driveGear(down,15,LiftMotor, LiftGear);
wait1Msec(500);
driveGear(down,-30,LiftMotor, LiftGear);
wait1Msec(500);
driveNipple(5.2,30,MoveMotor);
bool have = haveBrick();
driveNipple(5.2,-30,MoveMotor);
if(!have){
sendMessageWithParm(WEBSERVER, ERR_BRICK_NOT_PLUGGED, 0);
// loading
driveGear(down,15,LiftMotor, LiftGear);
wait1Msec(500);
driveGear(down,-30,LiftMotor, LiftGear);
wait1Msec(500);
driveNipple(5.2,30,MoveMotor);
bool have = haveBrick();
if(!have){
// find another way
}
driveNipple(5.2,-30,MoveMotor);
}
// Try few times to make sure it is plugged in firmly
//driveGear(down,15,LiftMotor, LiftGear);
//float vibr = 1;
//int speed = 100;
//driveGear(vibr,-speed,LiftMotor, LiftGear);
//wait1Msec(50);
//driveGear(vibr,speed,LiftMotor, LiftGear);
//driveGear(vibr,-speed,LiftMotor, LiftGear);
//wait1Msec(50);
//driveGear(vibr,speed,LiftMotor, LiftGear);
}
void moteToPrinterAndSendJob(int letter){
// move to printer
driveNipple(27.3, -20, TransportMotor);
// tell printer what should be done
sendMessageWithParm(PRINTER, PRINTER_PRINT, letter);
}
task main() {
int first = 0;
ubyte dataToSend[3] = {1,2,3};
while (true) {
wait1Msec(400); // Don’t send messages too frequently.
//if (first){
/* cCmdMessageWriteToBluetooth(1, dataToSend, 3, mailbox1);
wait1Msec(400);
sendMessageWithParm(SensorValue[S1],0, 0);
first = 1;
// }
// else
// {
cCmdMessageWriteToBluetooth(2, dataToSend, 3, mailbox2);
wait1Msec(400);*/
// sendMessageWithParm(SensorValue[S1],SensorValue[S1], 0);
// wait10Msec(20);
//
sendMessageWithParm(SensorValue[S1],SensorValue[S1], 0);
first = nBTCurrentStreamIndex;
cCmdMessageWriteToBluetooth(2, dataToSend, 3, mailbox1);
first = nBTCurrentStreamIndex;
wait10Msec(20);
while (true) {
sendMessageWithParm(SensorValue[S1],SensorValue[S1], 0);
cCmdMessageWriteToBluetooth(2, dataToSend, 3, mailbox1);
wait10Msec(5);
}
first = nBTCurrentStreamIndex;
cCmdMessageWriteToBluetooth(1, dataToSend, 3, mailbox1);
first = nBTCurrentStreamIndex;
wait10Msec(20);
// wait10Msec(20);
//
// first = 0;
// }
/* cCmdMessageWriteToBluetooth(nStream, pData, nLength, nQueueID);
Writes a Bluetooth message to 'nQueueID'. The message will be written to the specified 'port' or
'nStream' which should be in the range of 0 to 3.
This command is only useful when multiple ports are simultaneously open on the NXT;
a configuration that is not recommended because of the much slower communications when multiple
ports are in use. The message data is taken from the buffer at 'pData'.
'nLength' is the number of bytes in the message; the maximum length is 58 bytes.
*/
}
}
