Superfish.nxc

#include "NXCDefs.h"

#define __NOTETIME   10
#define __WAITTIME   12



inline int Choice()
{

SetSensor(IN_1,SENSOR_TOUCH);

 int btn=0;
 int sens1=SENSOR_1;
 
 NumOut(10, LCD_LINE1, btn);
 
 for(int i=0;i<4;i++)
 {
   Wait(2000);
   sens1=SENSOR_1;
   if(sens1==1)
   {
    btn++;
    NumOut(10, LCD_LINE1, btn);
   }
 }
 return btn;
}

inline void turnR(int speed)//Turns right
{
  	OnFwd(OUT_A, speed);
		OnRev(OUT_C, speed);
}

inline void turnL(int speed)//Turns left
{
  	OnFwd(OUT_C, speed);
		OnRev(OUT_A, speed);
}

inline void move_fwd(int distance)
{
  int i;
  OnFwdSyncEx(OUT_AC, 75, 0, RESET_NONE);
  i = SensorUS(IN_4);
  
  PlayTone(294,4*__NOTETIME);
  Wait(4*__WAITTIME);
  PlayTone(294,4*__NOTETIME);
  Wait(4*__WAITTIME);
  PlayTone(440,4*__NOTETIME);
  Wait(4*__WAITTIME);
  PlayTone(440,4*__NOTETIME);
  Wait(4*__WAITTIME);
  PlayTone(247,4*__NOTETIME);
  Wait(4*__WAITTIME);
  
  NumOut(10, LCD_LINE1, i);
  
 	while(i>distance) //Runs until robot is within distance
 	{
 		i = SensorUS(IN_4);
 	}
 Off(OUT_AC);
 Wait(1000);
 NumOut(10, LCD_LINE2, i);

}

inline void drop()//Book dropoff
{

  OnRev(OUT_B, 30);
  Wait(1500);
  Off(OUT_B);

  OnRevReg(OUT_AC, 100, OUT_REGMODE_SYNC);
  Wait(3000);
  Off(OUT_AC);

  OnFwd(OUT_B, 30);
  Wait(1000);
  Off(OUT_B);
  

}


inline void Find() //Finds the wall and lines up the robot
{
 	int range,right,left;
	range=SensorUS(IN_4);


	while(range>80)//Until wall is found
	{
    turnR(60);
    Wait(200);
	  Off(OUT_AC);
    Wait(200);
		range=SensorUS(IN_4);
	}
	Off(OUT_AC);
	Wait(500);

	left=SensorUS(IN_4); // Sets first point to compare

	turnR(50);
	Wait(150);
	Off(OUT_AC);
	Wait(500);

	right=SensorUS(IN_4); // Sets second point to compare

  if(right>left)
  {
   while(right>left&&right!=left)
   {
   turnL(60);
   Wait(150);
   Off(OUT_AC);
   Wait(500);
   right = left;
   left = SensorUS(IN_4);
   }
  }

  else
  {
    while(left>right&&left!=right)// Runs until right compare point is bigger than left
    {
    turnR(60);
		Wait(150);
		Off(OUT_AC);
		Wait(500);
		left = right;
		right = SensorUS(IN_4);
    }
  }

 	Wait(1000);
  PlayTone(440,4*__NOTETIME);
  Wait(4*__WAITTIME);
  PlayTone(247,4*__NOTETIME);
  Wait(4*__WAITTIME);

}

inline void Packet_1()   // Right 2.5m
{
	RotateMotor(OUT_A, 75, 340); //Turns the robot right 90Degrees
	Off(OUT_A);
	Wait(1000);// To ensure the robot is not interrupted by next task

   OnFwdSyncEx(OUT_AC, 75, 0, RESET_NONE);
   Wait(4000);//Runs the robot 2.5/2m in a line
   Off(OUT_AC);
   Wait(500);
   Find();
 	 RotateMotorEx(OUT_AC, 100, 340, 100, true, true); //Turns the robot right 90Degrees
	 Off(OUT_A);
	 Wait(1000);// To ensure the robot is not interrupted by next task
   OnFwdSyncEx(OUT_AC, 75, 0, RESET_NONE);
   Wait(4000);

  Off(OUT_AC);

  Find();

	move_fwd(20);

	drop();
}

inline void Packet_2()   // Left 2.5m
{
	RotateMotor(OUT_C, 75, 340); //Turns the robot left 90Degrees
	Off(OUT_C);
	Wait(1000);// To ensure the robot is not interrupted by next task

   OnFwdSyncEx(OUT_AC, 75, 0, RESET_NONE);
   Wait(4000);//Runs the robot 2.5/2m in a line
   Off(OUT_AC);
   Wait(500);
   Find();
 	 RotateMotor(OUT_C, 75, 340); //Turns the robot left 90Degrees
	 Off(OUT_C);
	 Wait(1000);// To ensure the robot is not interrupted by next task
	 
   OnFwdSyncEx(OUT_AC, 75, 0, RESET_NONE);
   Wait(4000);

  Off(OUT_AC);

  Find();

	move_fwd(20);

	drop();
}

inline void Packet_3()   // RightAcross 2.5m
{
	RotateMotor(OUT_A, 75, 340); //Turns the robot right 90Degrees
	Off(OUT_A);
	Wait(1000);// To ensure the robot is not interrupted by next task

   OnFwdSyncEx(OUT_AC, 75, 0, RESET_NONE);
   Wait(4000);//Runs the robot 2.5/2m in a line
   Off(OUT_AC);
   Wait(500);
   Find();
 	 RotateMotor(OUT_A, 75, 340); //Turns the robot right 90Degrees
	 Off(OUT_A);
	 Wait(1000);// To ensure the robot is not interrupted by next task
   OnFwdSyncEx(OUT_AC, 75, 0, RESET_NONE);
   Wait(4000);
   Off(OUT_AC);

  Find();
  RotateMotor(OUT_A, 75, 680); //Turns the robot right 180Degrees
	Off(OUT_A);
	Wait(1000);// To ensure the robot is not interrupted by next task
	
	move_fwd(20);

	drop();
}

inline void Packet_4()   // LeftAcross 2.5m
{
	RotateMotor(OUT_C, 75, 340); //Turns the robot left 90Degrees
	Off(OUT_C);
	Wait(1000);// To ensure the robot is not interrupted by next task

   OnFwdSyncEx(OUT_AC, 75, 0, RESET_NONE);
   Wait(4000);//Runs the robot 2.5/2m in a line
   Off(OUT_AC);
   Wait(500);
   Find();
 	 RotateMotor(OUT_C, 75, 340); //Turns the robot left 90Degrees
	 Off(OUT_C);
	 Wait(1000);// To ensure the robot is not interrupted by next task
   OnFwdSyncEx(OUT_AC, 75, 0, RESET_NONE);
   Wait(4000);
   Off(OUT_AC);

  Find();

  RotateMotor(OUT_A, 75, 680); //Turns the robot right 180Degrees
	Off(OUT_A);
	Wait(1000);// To ensure the robot is not interrupted by next task

	move_fwd(20);

	drop();
}

task main()
{

	SetSensorLowspeed(IN_4);

  int btn = Choice();
  
  Stop(btn == 0);
  
  if(btn==3)
  Wait(1000);
  
  if(btn==4)
  Wait(2000);
  
 	Find();  // Finds the wall
 	

 switch(btn)
    {
        case 1:
        Packet_1();
        break;

        case 2:
        Packet_2();
        break;

        case 3:
        Packet_3();
        break;

        case 4:
        Packet_4();
        break;
      }
   
}