/* Version Information:



Newest version - version 4.1, added I2C master support, print long ints



Changes made by version 3- this has a change in 

ser_rcv_nonblock so that it deals with overflows and framing errors

correctly-- the old nonblock failed when overflows occurred, freezing up

the cerebellum on blocking serial receives thereafter



Changes made by Version 2:  For new Cerebellum in which Btn1 is connected to B0

and the motor direction is now B1 instead of B0.



*/



/* Cereb.c is the main helper file for Cerebellum Programming. 

   See the API for details of how to use this stuff.

	Much of original code is courtesy of Kwanjee Ng

	Illah Nourbakhsh 2002 ; 2003
	Rashmi Patel, Tom Lauwers 2003

*/



/* things to add later -illah- :







*/



// API provided for cereb //////////////////////

//

// call init_cerebellum() first in your main

// turn on an led with set_bit(PORTB, (GREEN|YELLOW)); turn off with clear_bit

// (PORTB & BTNn) is true if button n is depressed and false otherwise

//

// To output on a digital pin on portD, use set_bit(PORTD, n); where n is 0-7 (and clear_bit)

// These are the pins to generally use for digital output.

// But if you're using the Servoes, you will have to set the mask for the servoes

// appropriately so that you can preserve some of the pins for your own digital out

// purposes.  Read the Servoes section below.

// 

// call ser_init(SER_115200) to set up hardware serial port

// ser_tx(char c) sends a character out over the serial line

// ser_putstring(const char *text) writes a CONST string out the serial line

// ser_writechar(char c) writes out a character in decimal (good for debugging!)
// ser_writelong(long data) writes out a long readably out the serial port

// ser_rcv() is a blocking receive char call that returns a char

//		note that if the input buffer has overflowed it dumps the buffer

//		and continues waiting for a new character to return!

// ser_rcv_nb() is a nonblocking receive char call that returns a char

//          or if there is nothing waiting, returns a 0. Does NOT check for

//		frame error and overflow conditions!!!

// to save memory, if not using the serial port, don't call ser_init() and then

// don't use any of the above functions.

// 

// to use servoes, begin by calling servo_init() in your main.

// then before expecting servoes to get used make servo_state=1;

// servoes use interrupts, so make sure you're starting interrupts:

//		set_bit(INTCON, GIE);

// then command a servo by commanding servo_pos[n]=p. This will drive

// the servo on port Dn to position p.  If p is zero, it deactivates the servo.

// to save memory, if not using servoes don't call servo_init(), do not

// put "set_bit(INTCON, GIE);" in your code, and also

// go to cereb.c (this file) and find void interrupt(void) and comment out the line that

// calls do_servo().

//

// to use some lines as servoes and other portD lines for direct digital outputs, note

// the variable servo_mask.  This defaults to 11111111b, which means all pins

// on portD are configured for servo output use.  If you want to have D0,D1

// and D3 be servo pins and the rest controlled directly by you, using set_bit,

// then in Main() you would include the line: "servo_mask = 00000111b;"

// Then the servo controller would ignore pins D3 - D7 and you can set them

// as desired. For example you could set pin3 to be high: "set_bit(PORTD,3);"

// Note the tristate is still configuring ALL of portD for digital output (not input).

//

// to use the analog-digital input lines, first in main you must initialize

// with the command: ADCON1=0;

// to read analog input channel ch, use adc_read(ch) which returns a char

// ch should be 0-7 inclusive. numbering on cerebellum goes from A0 as channel

// zero, left-to-right and top-to-bottom, so that E2 is analog channel 7

//

// to use the motor PWM outputs, first call pwm_init() in your code.

// if both commands are in there, call ser_init() first. The motor command

// is pwm_setvel8(n,dir,duty) where n=0 or 1 (0 means Motor1 on Cerebellum

// and 1 means Motor2 on Cerebellum). dir is direction (0 or 1) and duty is

// a char, with 0 meaning off and 255 meaning full-on.

// To save memory, if you are not using motor PWM then comment out pwm_init() in

// main().

// // 

//

// to use the I2C functions you need to be aware of the order in which functions need

// to be called.  The function i2c_init() needs to be called first, and only once.  

// this function should be called at the beginning of the program after the other init

// functions.  In order to send or receive with i2c you need to first start i2c communication.

// After a certain amount of data is sent, you must then stop communication.  Therefore,

// in order to send, for example three characters, you must call the i2c functions in

// the following order:

// i2c_start();

// i2c_send(address);*

// i2c_send(char1);

// i2c_send(char2);

// i2c_send(char3);

// i2c_stop();

//

// *because i2c accepts multiple devices on the bus, you have to specify 

// address of the device before the rest of the characters are sent.  

//

// Receiving requires several more functions, because after each character that is received,

// you must send an acknowledgement, requiring you to call a special i2cack(); function.  

// HOWEVER, after the last byte is sent, you must send a special non-acknowledgement, and call

// i2c_nak(); instead of i2cack();  As an example, assume you expect three bytes from a device.

// You would call:

// i2c_start();

// i2c_send(address+1);*

// char1 = i2c_receive();

// i2c_ack();

// char2 = i2c_receive();

// i2c_ack();

// char3 = i2c_receive();

// i2c_nak();   !note the special nak function here

// i2c_stop();

//

// * In order to tell a device that you are accepting information instead of sending it, the i2c

// standard requires you to add 1 to that device's address.  Therefore, all i2c devices have

// even numbered addresses.

//

// If you wish to send data, and then receive data immediately after, you may substitute an 

// i2c_restart() command for the i2c_stop()/i2c_start() commands.  Please refer to the sample

// i2c program to see this implementation.
////////////////////

// 




// Below code is for implementing the hardware serial port //

char ser_tmp;

char SSPCON@0x14;

char SSPCON2@0x91;

char SSPADD@0x93;

char SSPSTAT@0x94;

char SSPBUF@0x13;

char PIR1@0x0c;

char PIE1@0x8c;

char TRISC@0x87;

char TRISD@0x88;

char TXSTA@0x98;

char RCSTA@0x18;

char SPBRG@0x99;

char RCREG@0x1a;

char TXREG@0x19;

char PORTD@0x08;

char myOption_Reg@0x81;



// call ser_init to initial hardware serial as: ser_init(SER_115200) //

void ser_init(char spbrg_val) {

	

	SSPCON = 0;

	PIR1 = 0;  // this ensures also RCIF = TXIF = 0;

	clear_bit(PIE1,5); //RCIE = 0;

	clear_bit(PIE1,4); //TXIE = 0;

	TRISC = TRISC | 0xC0; // setup TRISC for USART use

	SPBRG = spbrg_val;

    	TXSTA = 000100100b; // txen, brgh

	RCSTA = 010010000b; // spen and cren, the rest all off

	ser_tmp = RCREG; // flush the rx buffer

	ser_tmp = RCREG;

	ser_tmp = RCREG;

}



// low-level call to send a character serially.

void ser_tx(char c) {



	//wait for txif to go hi

	while(!(PIR1 & 16)); //while (!TXIF); 

	//disable_interrupt(GIE); //?

	TXREG = c;

	//enable_interrupt(GIE);  //?

}



void ser_putstring(const char* text) {

    char i = 0;

    while( text[i] != 0 )

        ser_tx( text[i++] ); 

}



// writes a character out the serial port as a 1-3 digit number



void ser_writechar(char data) {

  if(data >= 100)

    ser_tx( '0' + data/100 );

  if(data >= 10)

    ser_tx( '0' + (data%100)/10 );

  ser_tx( '0' + data%10 );

}



// write a long int out to the serial

void ser_writelong(long data){



  if(data >= 10000)

    ser_tx('0' +   data/10000);

  if(data >= 1000)

    ser_tx( '0' + (data%10000)/ 1000 );

  if(data >= 100)

    ser_tx( '0' + (data%1000)/100 );

  if(data >= 10)

    ser_tx( '0' + (data%100)/10 );

  ser_tx( '0' + data%10 );

}





// this is a blocking receive //

char ser_rcv(void) {

	while (1) {

		if (RCSTA & 2) { // RCSTA bit 1 is overflow error

			// overflow error

			clear_bit(RCSTA,CREN); // CREN is RCStatus bit 4 //

			ser_tmp = RCREG; // flush the rx buffer

			ser_tmp = RCREG;

			ser_tmp = RCREG;

			set_bit(RCSTA,CREN); // CREN = 1;

		}

		else if (RCSTA & 4) { // RCSTA bit 2 is framing error

			// framing error

			ser_tmp = RCREG;

		}

		else if (PIR1 & 32) { // PIR1 bit 5 is RCIF

			ser_tmp = RCREG;

			return ser_tmp;

		}

	}

}



// nonblocking receive returns 0 for any error, including nothing to read

char ser_rcv_nb(void) {



	int problem;

	problem = 1;

	

	while (problem == 1) {

		problem = 0;

		if (RCSTA & 2) { // RCSTA bit 1 is overflow error

			// overflow error

			clear_bit(RCSTA,CREN); // CREN is RCStatus bit 4 //

			ser_tmp = RCREG; // flush the rx buffer

			ser_tmp = RCREG;

			ser_tmp = RCREG;

			set_bit(RCSTA,CREN); // CREN = 1;

			problem = 1;

		}

		else if (RCSTA & 4) { // RCSTA bit 2 is framing error

			// framing error

			ser_tmp = RCREG;

			problem = 1;

		}

		else if (PIR1 & 32) {

		ser_tmp = RCREG;

		return ser_tmp;

		}

	} // end while (problem == 1) //

	return 0; // no byte to receive and no errors; return zero

}



// this next section is for servo control /////////////



char servo_state = 0;

char servo_curr = 0;

char servo_mask = 11111111b; // default that all of portD will be

					// used for servo operations rather 

					// than direct digital output twiddling

char servo_switch = 1;

char servo_pos[8] = {0,0,0,0,0,0,0,0};

char _servo_free = 0;



void servo_init(void) {

    OPTION_REG = 132; // bit 7 = 1 for portB pull-up and bit2 is for prescaler

				// TMR0 @ 1:32, prescaler to timer0 (for servo control)

    TRISD = 0; // port D as outputs	for servo commands!

	// in fact we already do this in init_cerebellum(); we do it again for //

	// redundancy only here //

    set_bit(INTCON, T0IE); //T0IE = 1 ie enable TMR0 overflow bit - T0IE is bit 5

    delay_ms(1);   

}



void do_servo(void) {

	clear_bit(_servo_free,0); // the bit 0 of servo_free = 0;

	if (servo_state == 1) {

		// next intr in 0.6 ms

		TMR0 = 153;

		//TMR0 = 145;

		if ((servo_pos[servo_curr] != 0) && ((servo_mask & servo_switch) != 0))

			PORTD |= servo_switch; // output hi to specific servo pin out

		servo_state = 2;

                set_bit(_servo_free,0); // the bit servo_free = 1;

	}

	else if (servo_state == 2) {

		TMR0 = 255 - servo_pos[servo_curr];

		servo_state = 3;

	}

	else if (servo_state == 3) {

		if ((servo_mask & servo_switch) != 0)

			PORTD &= ~servo_switch; // output lo to this servo bit, but

							// preserve all other digital hi's on port D

		TMR0 = servo_pos[servo_curr];