/* place your #defines and global variable declarations here */

#define clawspeed 80      /* what % speed to run the claw at */
#define clawmotor 1       /* which motor port the claw motor is plugged into*/
#define clawopensensor 14 /* which digital port the "claw open" sensor is in */

void program()
{

  /* declare your variables here */

  /* your program goes here */

  printf("Start= open claw Stop = close\n");     /* print a helpful message */
                                                /* at the beginning */

  while(1==1) {                          /* loop through the program */
                                         /* while 1=1, that is, forever */

    if (start_button() == 1) {           /* if the START button has been */
                                         /* pressed, then do all the */
                                         /* following stuff: */
      
      printf("Opening...\n");                   /* print a message */

      OpenClaw();               /* run the "Open Claw" subroutine that */
                                /* should open the claw... see further down */
                                /* below for details of this subroutine */

      printf("Done.\n");                        /* print a message */

    }

    /********************************************************************/
    /* your code here that closes the claw if you press the STOP button */
    /********************************************************************/

  }   

}

/* put your functions and subroutines here */

      
void OpenClaw()         /* This subroutine opens the claw until it */
                        /* is all the way open, then stops opening */
                        /* it and returns to the main program */
{

      motor(clawmotor,clawspeed);               /* turn on the claw motor */

      while (IsClawFullyOpen() == 0) { }        /* wait here and do nothing */
                                                /* while the function IsClaw */
                                                /* FullyOpen = 0 (not fully */
                                                /* open)  */

      /* if the program gets to here, the function IsClawFullyOpen does */
      /*  NOT = 0, so the claw IS fully open, so: */

      motor(clawmotor,0);                       /* turn off the claw motor */

}


int IsClawFullyOpen()     /* This function returns 1 if the claw is */
			  /*  fully open, and 0 if it is not fully open */
{
  if (digital(clawopensensor) == 0) {     /* if this digital sensor = 0 */
                                          /* (not pressed), then the claw */
                                          /* is NOT fully open */
    return 0;             /* so return 0 */
  }
  else {                  /* otherwise, it IS fully open... */
    return 1;             /* so return 1 */
  }
}



/*********************************************************************/
/********* FROM HERE ON ARE MENU ROUTINES: LEAVE AS IS ***************/
/*********************************************************************/


void main()
{
  while (1==1) {
    ao();
    menu();  
    ao();
    program();
  }

}

char menu_choices[5][32] = {"Any button = run user program","MOTOR: Start=FWD  0  : Stop=RVS","MOTOR: Start=FWD  1  : Stop=RVS","MOTOR: Start=FWD  2  : Stop=RVS","MOTOR: Start=FWD  3  : Stop=RVS"};

int menu_motorstate = 0;					      
int menu_choice = 9;						      
int menu_motorpower = 80;					      

void menu() {

  while (1) {

    while (stop_button() || start_button()) {}
    sleep(0.05);
    while (stop_button() || start_button()) {}
    sleep(0.05);
    while (stop_button() || start_button()) {}
    sleep(0.05);
    while (stop_button() || start_button()) {}
    
    menu_choice = select_string(menu_choices, 5);

    if ((menu_choice == -1) || (menu_choice == 0)) {
      printf("\n\n");
      return;
    }
    else {
      if (menu_choice == 1) {
	motor(0, menu_motorpower);
      }
      if (menu_choice == 2) {
	motor(0, -menu_motorpower);
      }
      if (menu_choice == 3) {
	motor(1, menu_motorpower);
      }
      if (menu_choice == 4) {
	motor(1, -menu_motorpower);
      }
      if (menu_choice == 5) {
	motor(2, menu_motorpower);
      }
      if (menu_choice == 6) {
	motor(2, -menu_motorpower);
      }
      if (menu_choice == 7) {
	motor(3, menu_motorpower);
      }
      if (menu_choice == 8) {
	motor(3, -menu_motorpower);
      }

      while (stop_button() || start_button()) {}
      
      ao();

    }
    
  }

}

int select_string(char choices[][],int n)
{

  int selection,last_selection=-1,coarseness;
  if(n>_array_size(choices))
    n=_array_size(choices);

  coarseness=255/(n-1);

  while(!(stop_button() || start_button()))
    {
      selection=(knob())/coarseness;
      if(selection!=last_selection)
	{
	  printf("%s\n",choices[selection]);
	  sleep(0.1);
	  last_selection=selection;
	}
    }
  if (start_button()) {
    return selection * 2 - 1;
  }
  else {
    return selection * 2;
  }
}