/*demos*/

void sensorDebug()
{
  	sei();
	timer0_init();
	ADC_init();
	LCD_init();
	delay(500);
	LCD_init();
	printS(4,"IR0 = ");
	printN(4,analogRead(1));
	LCD_setCursor(10,0);
	printS(4,"  IR1 = ");
	printN(4,analogRead(2));
	LCD_setCursor(0,1);
	printS(4,"IR2 = ");
	printN(4,analogRead(3));
	LCD_setCursor(10,1);
	printS(4,"  IR3 = ");
	printN(4,analogRead(4));
	LCD_setCursor(0,2);
	printS(4,"sonar0 = ");
	printN(4,sonar0());
	LCD_setCursor(0,3);
	printS(4,"sonar1 = ");
	printN(4,sonar1());
	  
	while(1)
	{
	  LCD_setCursor(6,0);
	  printS(4,"    ");
	  LCD_setCursor(6,0);
	  printN(4,analogRead(1));
	  
	  LCD_setCursor(16,0);
	  printS(4,"    ");
	  LCD_setCursor(16,0);
	  printN(4,analogRead(2));

	  
	  LCD_setCursor(6,1);
	  printS(4,"    ");
	  LCD_setCursor(6,1);
	  printN(4,analogRead(3));

	  
	  LCD_setCursor(16,1);
	  printS(4,"    ");
	  LCD_setCursor(16,1);
	  printN(4,analogRead(4));

    
      LCD_setCursor(9,2);
	  printS(4,"   ");
      LCD_setCursor(9,2);
	  printN(4,sonar0());

	  
	  LCD_setCursor(9,3);
	  printS(4,"   ");
	  LCD_setCursor(9,3);
	  printN(4,sonar1());
	  


	  delay(100);



	}


}

void mobilityDemo()
{
  sei();
timer0_init();
LCD_init();
delay(500);
LCD_init();
ADC_init();
printS(4,"GatorKS on line");
LCD_setCursor(0,1);
printS(4,"VIN = ");
printD(4,(analogRead(0)+1)/102.4);
delay(3000);
timer1Servo_init();
center();
timer3Servo_init();
int i = 0;
while(1)
{

LCD_clear();
printS(4,"moving forward");
move(90,100);
delay(1000);
LCD_clear();
printS(4,"moving right");
move(0,100);
delay(1000);
LCD_clear();
printS(4,"moving backward");
move(270,100);
delay(1000);
LCD_clear();
printS(4,"moving left");
move(180,100);
delay(1000);

LCD_clear();
printS(4,"circling");
for (i=0;i<360;i++) 
{
  move(i,100);
  LCD_setCursor(10,0);
  printN(4,i);
  delay(10);
}

spinL(100);
LCD_clear();
printS(4,"spinning left");
delay(5000);
spinR(100);
LCD_clear();
printS(4,"spinning right");
delay(5000);

}
}
