/*  Coded by Jack Buffington 2011.   

    This isn't supposed to be a functional program in the respect that it should perform any particular
	task.  Because of that, it is relatively disorganized.  I have tried to comment things well enough 
	that you could use this code to jump off of to start making your own programs.   In the main(), I 
	have various sections commented out that demo different things.   Just uncomment them to play around 
	with that functionality.  

*/


#include <pololu/3pi.h>
#include <avr/io.h>
#include <avr/interrupt.h>



// function prototypes
void setupPWMlockAntiphase();
void setPWMs(int leftMotor, int rightMotor);
void setupADC();
int readADC();
long readLineSensor(int scaleOutput, long minimum, long maximum, int whichSensor);
void setupLineSensor();
void startTimer1interrupt();
void stopTimer1interrupt();
void setTimer1match(short theValue);



ISR(TIMER1_COMPA_vect)
	{// this interrupt happens when timer1's compare A is matched
	 // note that anything that disables interrupts will mess with your frequencies.  For example,
	 // Pololu's LCD driver seems to disable interrupts while updating the display.
    PORTB ^= 1<<2; // toggle the beeper
	}

//------------------------------------------------------------------------
int main()
	{


	print("Hackaday");
	DDRD=0xFF; //set port D pins as outputs
	delay_ms(500);

	
	
	 // Do a tone on the beeper using interrupts
	DDRB |= 1 << 2; // make the beeper be an output.
	startTimer1interrupt();
	setTimer1match(555);
	
	delay_ms(500);
	setTimer1match(355);
	delay_ms(500);

	stopTimer1interrupt();

	while(1)
		{}



	/* Drive the motors using PWM
	setupPWMlockAntiphase();

    setPWMs(192,192);
	delay_ms(300);

	setPWMs(128,128);
	delay_ms(200);

	setPWMs(64,64);
	delay_ms(300);

	setPWMs(128,128);
	delay_ms(200);
	

	setPWMs(100,156);
	delay_ms(200);

	setPWMs(156,100);
	delay_ms(400);

	setPWMs(100,156);
	delay_ms(200);


	setPWMs(128,128);*/

	

	/*	
	
	// Calibrate the line sensor and then display a reading in the range of 0->255 
	long tempLong;
	int calibrationCount = 50;
	long sensorMax = 0;
	long sensorMin = 50000;

	// calibration step
	while(calibrationCount--)
		{
		tempLong = readLineSensor(0, 0, 0, 0);
		if(tempLong > sensorMax)			// try to find the range of possible values 
			sensorMax = tempLong;			// so that the sensors can be calibrated
		else if(tempLong < sensorMin)
			sensorMin = tempLong;
		clear();
		print_long(tempLong);
		lcd_goto_xy(0,1);
		print_long(calibrationCount);
		delay_ms(100);
		}


	
	// step that reads the line sensor and displays a value in the range of 0-255
	while(1)
		{
		tempLong = readLineSensor(1, sensorMin, sensorMax, 0);
		clear();
		print_long(tempLong);
		delay_ms(100);
		} */
	

/*	read the ADC and display it on the LCD
	while(1)
		{
		tempInt = readADC();
		clear();
		print_long(tempInt);
		
		lcd_goto_xy(0,1);
		print_long(tempLong++);
		delay_ms(100);
		}*/


/*	play with the buttons LEDs
	while(1) // turn off LED if button is pushed (buttons have inverted logic)
		{

		if (!(PINB & 2))
			{
			clear();
			print("left");
			PORTD = 2;
			delay_ms(50);
			}			
		else if(!(PINB & 16))
			{
			clear();
			print("middle");
			delay_ms(50);
			PORTD = 2 + 128;
			}
		else if(!(PINB & 32))
			{
			clear();
			print("right");
			PORTD = 128;
			delay_ms(50);
			}
		else
			{
			clear();
			print("nothing");
			delay_ms(50);
			PORTD = 0;
			}
		}*/

/*	
	// play with the beeper without using interrupts
	DDRB |= 0b00000100; // set buzzer pin to be an output.
	while(1)
		{
		if (!(PINB & 2))
			{
			PORTB = 4;
			delay_ms(1);
			PORTB = 0;
			delay_ms(1);
			}			
		else if(!(PINB & 16))
			{
			PORTB = 4;
			delay_ms(2);
			PORTB = 0;
			delay_ms(2);
			}
		else if(!(PINB & 32))
			{
			PORTB = 4;
			delay_ms(3);
			PORTB = 0;
			delay_ms(3);
			}
		else
			{
			clear();
			print("nothing");
			delay_ms(50);
			PORTD = 0;
			}
		}*/

	return 0;
	}

//----------------------------------------------------------------------
void setupPWMlockAntiphase()
	{
	DDRD = 0;
	DDRC=0xFF;
	PORTC = 0;
	DDRB = 0;

	DDRD = 0b01101000; // set pins 3,5,6 to be outputs
	DDRB = 0b00001000; // set pin 3 to be an output

	// Left motor stuff	
	OCR0A = 128; 	// set PWM for N/255 duty cycle
	OCR0B = 128;	// 128 sets the motors to be stopped

	TCCR0A = (1 << WGM01) | (1 << WGM00); // set fast PWM Mode that counts to 255

	// makes the A pin be normal polarity PWM
	TCCR0A |= (1 << COM0A1);
	
	// makes the B pin be inverted polarity PWM
	TCCR0A |= (1 << COM0B1) | (1 << COM0B0);   

	// set prescaler to 8 and starts PWM
    TCCR0B |= (1 << CS01);



	// Right motor stuff
	OCR2A = 128; 	// set PWM for N/255 duty cycle
	OCR2B = 128;

	TCCR2A = (1 << WGM21) | (1 << WGM20); // set fast PWM Mode that counts to 255

	// makes the A pin be normal polarity PWM
	TCCR2A |= (1 << COM2A1);
	
	// makes the B pin be inverted polarity PWM
	TCCR2A |= (1 << COM2B1) | (1 << COM2B0);   

	// set prescaler to 8 and starts PWM
    TCCR2B |= (1 << CS21);
	}

//-----------------------------------------------------------------------------------
void setPWMs(int leftMotor, int rightMotor)
	{// sets the speed and direction of the motors.
	 // valid values are 0->255
	 // 128 is stopped
	 // higher numbers are forward
	 // lower numbers are backward


	OCR0A = leftMotor; 	// set PWM for N/255 duty cycle
	OCR0B = leftMotor;

	OCR2A = rightMotor; 	
	OCR2B = rightMotor;


	}
//-----------------------------------------------------------------------------------
void setupADC()
	{// sets up the ADC to read 8-bit values on channel 7

	//ADMUX = 96; // use Aref pin, left justify the result, select channel 0
	//ADCSRA = 151; // ADC on, clear conversion flag, no interrupt, divide clock by 128	
	
	ADCSRA = 0;
	ADMUX = 0;

	ADMUX += 7; //set ADC channel (7 for potentiometer) 
	ADMUX |= (1 << ADLAR); // left justify the result
	ADMUX |= (1 << REFS0); // set the reference to AVcc

	ADCSRA |= (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // set prescaler to 128
	ADCSRA |= (1 << ADEN); // enable the ADC
	}

//-----------------------------------------------------------------------------------
int readADC()
	{// returns an 8-bit result
	// turn on the sensors' LEDs
	PORTC &= 0b11011111;

	ADCSRA |= (1 << ADSC); // start conversion. 

	while(ADCSRA & (1 << ADSC)) // This bit will go to zero when the conversion is done
		{
		}
	
	return ADCH;  // I have set things to left justify the value so this just gives me an 8-bit result
	}
//------------------------------------------------------------------------------------
long readLineSensor(int scaleOutput, long minimum, long maximum, int whichSensor)
	{/*  This routine does the following:  
	* It calculates a sensor mask to drive the appropriate sensor. whichSensor: 0->4
	* It sets the sensor pin to be an output and drives it high.  
	* It delays for a bit of time for the sensor's capacitor to charge up.
	* It turns on the sensor's LED.
	* It then makes the sensor pin an input and times how long it takes to go low again.
	* It turns off the sensor's LED.
	* If scaleOutput is 1 then it will scale the time to to the range of 0->255 based on 
	  the minimum and maximum values.   

	*/

	// calculate the pin mask for selecting the sensors
	int sensorMask = 1;
	sensorMask <<= whichSensor;

	//set the sensor pin to be an output and drive it high. 
	DDRC |= sensorMask;		// make it an output
	PORTC |= sensorMask;    // drive it high
	
	//delay for a bit of time for the sensor's capacitor to charge up.
	delay_us(10);

	// turn on the sensor's LED
	PORTC |= 1 << 5;
	
	// make the sensor pin an input and times how long it takes to go low again.
	DDRC ^= sensorMask;		// make it an input 
	PORTC ^= sensorMask;    // disable the internal pull up

	long sensorTime = 0;

	while(PINC & sensorMask)
		{ // do nothing while it is still high
		sensorTime++;
		}

	

	//turn off the sensor's LED.
	PORTC ^= 1 << 5;

	// If scaleOutput is 1 then it will scale the time to to the range of 0->255 based on 
	// the minimum and maximum values.
	if(scaleOutput)
		{
		if(sensorTime <= minimum)
			sensorTime = 0;
		else if(sensorTime >= maximum)
			sensorTime = 255;
		else
			{
			sensorTime -= minimum;
			sensorTime *= 255;
			sensorTime /= maximum;
			}
		}
	return sensorTime;
	}

//-----------------------------------------------------------------------------------------
void setupLineSensor()
	{// makes the LED pin be an output and sets it low
	DDRC |= 0b00100000;
	PORTC &= 0b11011111;  // set the pin low
	}

//-----------------------------------------------------------------------------------------
void setTimer1match(short theValue)
	{/* since timer1 is a 16-bit timer, you can't just set the match registers without
	    diabling the interrupts since an interrupt could happen between writing the two 
	    8-bit registers.
	
	   According to the data sheet the timer will interrupt at: clock/(2 * divider * OCR1A)

	   I am finding that the resulting frequency is roughly 20,000,000/(2 * 64 * (theValue + 1))  
	   I suspect that the +1 is due to interrupt latency.
	   I am not sure why the frequency is twice what it should be.  I'm feeling lazy though so I'm not going to figure it out.
	   A quick formula to find 'theValue' for a particular frequency is: (156250 / frequency) - 1

	
	   For reference, these are the "theValue" for the notes on the piano keyboard:  If you are using a lot of the higher 
	   notes, you should probably decrease the divider so that you can get more accurate frequencies for those notes.
	   	C0		9556
		C#0/Db0	9020
		D0		8514
		D#0/Eb0	8032
		E0		7584
		F0		7157
		F#0/Gb0	6757
		G0		6377
		G#0/Ab0	6018
		A0		5681
		A#0/Bb0	5361
		B0		5061
		C1		4777
		C#1/Db1	4508
		D1		4255
		D#1/Eb1	4017
		E1		3791
		F1		3579
		F#1/Gb1	3377
		G1		3188
		G#1/Ab1	3009
		A1		2840
		A#1/Bb1	2680
		B1		2530
		C2		2388
		C#2/Db2	2254
		D2		2127
		D#2/Eb2	2008
		E2		1895
		F2		1789
		F#2/Gb2	1688
		G2		1593
		G#2/Ab2	1504
		A2		1419
		A#2/Bb2	1340
		B2		1264
		C3		1193
		C#3/Db3	1126
		D3		1063
		D#3/Eb3	1003
		E3		 947
		F3		 894
		F#3/Gb3	 844
		G3		 796
		G#3/Ab3	 751
		A3		 709
		A#3/Bb3	 669
		B3		 632
		C4		 596
		C#4/Db4	 563
		D4		 531
		D#4/Eb4	 501
		E4		 473
		F4		 446
		F#4/Gb4	 421
		G4		 398
		G#4/Ab4	 375
		A4		 354
		A#4/Bb4	 334
		B4		 315
		C5		 298
		C#5/Db5	 281
		D5		 265
		D#5/Eb5	 250
		E5		 236
		F5		 223
		F#5/Gb5	 210
		G5		 198
		G#5/Ab5	 187
		A5		 177
		A#5/Bb5	 167
		B5		 157
		C6		 148
		C#6/Db6	 140
		D6		 132
		D#6/Eb6	 125
		E6		 118
		F6		 111
		F#6/Gb6	 105
		G6		  99
		G#6/Ab6	  93
		A6		  88
		A#6/Bb6	  83
		B6		  78
		C7		  74
		C#7/Db7	  69
		D7		  66
		D#7/Eb7	  62
		E7		  58
		F7		  55
		F#7/Gb7	  52
		G7	 	  49
		G#7/Ab7	  46
		A7	      43
		A#7/Bb7	  41
		B7		  39
		C8		  36
		C#8/Db8	  34
		D8		  32
		D#8/Eb8	  30

	*/


	int sreg;
	sreg = SREG; /* Save global interrupt flag */
	cli();  //Disable interrupts 

	// I am setting 16-bits here even though it looks like I should only be setting eight	
	OCR1A = theValue;  

	SREG = sreg; //Restore global interrupt flag 

	
	

	}

//-----------------------------------------------------------------------------------------
void startTimer1interrupt()
	{
	 
	TIMSK1 |= 1 << OCIE1A; // turn on the A match interrupt
	TCCR1B = 12; // clear the timer on compare 
	TCCR1B &= 0b11111000;   // clear the prescaler bits
	TCCR1B |= 3; // set the prescaler to /64.
	sei();
	}

//-----------------------------------------------------------------------------------------
void stopTimer1interrupt()
	{
	TIMSK1 ^= 1 << OCIE1A;   // turn off the A match interrupt
	}

//-----------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------

