// Glove Project
//Karan Khanna & Amit Penmetcha

#include <Mega32.h>
// define a new I/O register for easier programming
sfrw ICR1=0x26;

#include <stdio.h>
#include <delay.h> // delay functions delay_ms
#include <math.h>

#define UDRE    5

//time for task
#define t1 50	//used to do tasks every ms

#define begin {
#define end }

void task1(void);
void puts_int(void);
void gets_int(void);

unsigned char r_buffer[16];
unsigned char r_ready = 0;
unsigned char r_char;
int r_index = 0;

unsigned char t_buffer[20];
char sendData[3];
unsigned char t_ready = 0;
unsigned char t_char;
int t_index = 0;
signed int sign_x = 0;
signed int sign_y = 0;
//timeout counter
unsigned char time1;
unsigned char time2;
unsigned char time3;
int hsIn, hsActive, currPort = 0;
char input, x, y = 0;
char x_calibration, y_calibration = 0;
int flag1, flag2 = 0;

unsigned int flex_sensors[5];
char temp; //temporary variable
int var1, var2 = 0; //used to determine degree
int i;

signed int x_degree = 0; //determines which direction one moves on the x axis and by how much
signed int y_degree = 0; //determines which direction one moves on the y axis and by how much
char serial_data1, serial_data2 = 0;
char buffer = 0b11000000;

int lookup[5];
//use timer overflow interrupt to determine if hall sensor array has been activated.
//PORTB and PORTC will be used for hall effect sensors. Will allow for 14 sensors to be used
//PORTD will communicate with PC on how to move the arm.
interrupt [TIM0_OVF] void timer0_overflow(void)
begin
  //detect which sensor has been activated
  if     (~PINB.0 & 0x01 == 0x01)
    hsIn = 1;
  else if(~PINB.1 & 0x01 == 0x01)
    hsIn = 2;
  else if(~PINB.2 & 0x01 == 0x01)
    hsIn = 3;
  else if(~PINB.3 & 0x01 == 0x01)
    hsIn = 4;
  else if(~PINB.4 & 0x01 == 0x01)
    hsIn = 5;
  else if(~PINB.5 & 0x01 == 0x01)
    hsIn = 6;
  else if(~PINB.6 & 0x01 == 0x01)
    hsIn = 7;
  else if(~PINB.7 & 0x01 == 0x01)
    hsIn = 8;
  else
    hsIn = 0;

  if (time1>0)	--time1;
end

//ADC interrupt to be used for reading accelerometer and flex sensors from ADC
interrupt [ADC_INT] void readADC(void){
  input = ADCH;  //read acceleration from ADC
}

void main(void)
begin
  DDRB = 0x00;          //set PORTB to input
  PORTB = 0;
  DDRD = 0xff;
  PORTD = 0xff;
  DDRC = 0x00;          //set PORTC to input
  PORTC = 0;

  //For Mega32:
  UCSRB = 0b10011000 ; //hex 0x98
  UBRRL = 103 ;        //using a 16 MHz crystal (9600 baud)

  TCNT0 = 0;            //reset counter
  TIMSK = 1;            //turn on timer 0 overflow ISR
  TCCR0 = 0b00000001;   //set prescalar to 1
  ADMUX = 0b01100000;   //read x-axis first
  ADCSR = 0b11101111;   //adc status
  hsActive = 0;
  i = 0;
  for(i=0; i<5; i++)
    flex_sensors[i] = 0;

  i = 0;
  r_ready=0;
  t_ready=1;

  lookup[0] = 0x00;
  lookup[1] = 0x01;
  lookup[2] = 0x02;
  lookup[3] = 0x03;
  lookup[4] = 0x04;

  time1 = t1;
  #asm
    sei
  #endasm

  //main task scheduler loop
  while(1)
  begin
    if (time1 == 0)  task1();
  end
end

//task1 converts the input from the ADC into acceptable input to the Serial Port. We will calculate the angle. The angle determines how fast the snake arm moves.
//rolling to the right gives 250 to 220, starting from 250. Words are right side up. Rolling to the left gives 0 to 36, starting from 0. Words are right side up.
//Tilting up gives 0 to 40, starting at 0. Tilting down gives 255 to 220, starting at 255.
void task1(void)
begin
  time1 = t1;
  if(hsActive == 0 && currPort != 2) //makes sure you are not using the hall effect sensors
  begin
    if(currPort == 0)
    begin
      #asm
        sleep
      #endasm
      x = input; //read ADC
      ADMUX = 0b01100001;  //change to read y axis
      if(flag1 == 0)
      begin
        x_calibration = x;
        flag1 = 1;
      end
      currPort = 1;
    end
    else if(currPort == 1)
    begin
      #asm
        sleep
      #endasm
      y = input; //read ADC
      ADMUX = 0b01100010;  //change to read flex sensor
      if(flag2 == 0)
      begin
        y_calibration = y;
        flag2 = 1;
      end
      currPort = 2;

      var1 = x - x_calibration;
      var2 = y - y_calibration;
      if(var1 > 250 || var1 < 10)
        var1 = 0;
      if(var2 > 250 || var2 < 10)
        var2 = 0;

      if(var1 < 100 && var1 != 0)
        sign_x = 0x80;
      else
        sign_x = 0x00;
      if(var2 > 100)
        sign_y = 0x08;
      else
        sign_y = 0x00;

      serial_data2 = 0; //reset packet of data
      serial_data2 = serial_data2 | sign_x | sign_y;
      //this section formats the 2nd packet of data that is sent serially.
      //first half of the byte contains sign and magnitude of how the arm should move
      //on the horizontal plane
      //the second half of the byte contains the sign and magnitude of how the arm should move
      //on the veritcal plane

      if(var1 == 0)
        serial_data2 = serial_data2 & 0x00; //pointless but more clear
      else if((var1 > 10 & var1 <= 15) || (var1 < 250 && var1 >= 245))
        serial_data2 = serial_data2 | 0x10;
      else if((var1 > 15 & var1 <= 20) || (var1 < 245 && var1 >= 240))
        serial_data2 = serial_data2 | 0x20;
      else if((var1 > 20 & var1 <= 25) || (var1 < 240 && var1 >= 235))
        serial_data2 = serial_data2 | 0x30;
      else if((var1 > 25 & var1 <= 40) || (var1 < 235 && var1 >= 220))
        serial_data2 = serial_data2 | 0x40;
      //otherwise it is out of range


      if(var2 == 0)
        serial_data2 == serial_data2 | 0x00; //pointless but more clear
      else if((var2 > 10 & var2 <= 15) || (var2 < 250 && var2 >= 245))
        serial_data2 = serial_data2 | 0x01;
      else if((var2 > 15 & var2 <= 20) || (var2 < 245 && var2 >= 240))
        serial_data2 = serial_data2 | 0x02;
      else if((var2 > 20 & var2 <= 25) || (var2 < 240 && var2 >= 235))
        serial_data2 = serial_data2 | 0x03;
      else if((var2 > 25 & var2 <= 40) || (var2 < 235 && var2 >= 220))
        serial_data2 = serial_data2 | 0x04;

      //otherwise it is out of range
  
    //testing on the hyperterminal
    //sprintf(t_buffer, "byte2=%d\n\r", serial_data2);
    //puts_int();
    //delay_ms(20);

   
 
    end //end if currPort == 1
  end   //end hsActive
  else if(hsActive == 1)
  begin
   // PORTD = PINB;
    serial_data2 = 0; //reset packet of data.
    serial_data2 = 0xFF & hsIn; //second packet of data contains hall effect sensor ID.
    hsActive = 0; //forces control back to flex sensor, which will set hsActive back high if all are flexed.
  end
  else if(currPort == 2)
  begin
    for(i=0; i<5; i++) { //this determines the array of flex sensors that are active
      #asm
        sleep
      #endasm
      temp = input;
      if (temp <= 30)
        flex_sensors[i] = 1;
      else
        flex_sensors[i] = 0;
      ADMUX = ADMUX + 0x01;
    } //end for loop
    i = 0; //reset i

    //if they are all flexed, then use hall effect sensors
    if(flex_sensors[0] && flex_sensors[1] && flex_sensors[2] && flex_sensors[3] && flex_sensors[4])
    begin
       hsActive = 1;
       currPort = 2;
       ADMUX = 0b01100010; //keep reading flex sensors
    end
    //otherwise use accelerometer
    else
    begin
      hsActive = 0;
      currPort = 0;
      ADMUX = 0b01100000; //begin reading accelerometer
    end

    //first packet of data has contains the array of active flex sensors.
    //it also contains whether or not the hall effect sensors or accelerometer
    //is activated. *NOTE: The top two bits are not used. Only 6 bits are used
    //for data
    serial_data1 = 0;
    serial_data1 =  serial_data1       | (0x01 & flex_sensors[0]); //1st bit of 5 bit array
    serial_data1 = (serial_data1 << 1) | (0x01 & flex_sensors[1]); //2nd bid of 5 bit array
    serial_data1 = (serial_data1 << 1) | (0x01 & flex_sensors[2]); //3rd bit
    serial_data1 = (serial_data1 << 1) | (0x01 & flex_sensors[3]); //4th bit
    serial_data1 = (serial_data1 << 1) | (0x01 & flex_sensors[4]); //5th bit
    serial_data1 = serial_data1 << 1;  //6th bit
    if(hsActive == 1) //put in 6th bit
      serial_data1 = serial_data1 | 0x01;
    else
      serial_data1 = serial_data1 | 0x00;

    sendData[0] = buffer;
    sendData[1] = serial_data1;
    sendData[2] = serial_data2;
    t_index = 0;
    while(t_index < 3) {
    while(!(UCSRA & (1<<UDRE))); //block
      UDR = sendData[t_index];
      t_index++;
    }

  end //end if currPort == 2
end //end Task

//**********************************************************
//UART character-ready ISR
interrupt [USART_RXC] void uart_rec(void)
begin
  r_char=UDR;    //get a char
  UDR=r_char;    //then print it
  //build the input string
  if (r_char != '\r') r_buffer[r_index++]=r_char;
  else
  begin
    putchar('\n');  		//use putchar to avoid overwrite
    r_buffer[r_index]=0x00;    	//zero terminate
    r_ready=1;			//signal cmd processor
    UCSRB.7=0;   		//stop rec ISR
  end
end

/**********************************************************/
//UART xmit-empty ISR
interrupt [USART_DRE] void uart_send(void)
begin
//  used for debugging
//  t_char = t_buffer[++t_index];
//  if (t_char == 0)
//  begin
    //UCSRB.5=0; //kill isr
//    t_ready=1; //transmit done
//  end
  //else	UDR = t_char ;     //send the char
end

void puts_int(void)
begin
  t_ready=0;
  t_index=0;
  if (t_buffer[0]>0)
  begin
    putchar(t_buffer[0]);
    UCSRB.5=1;
  end
end

void gets_int(void)
begin
  r_ready=0;
  r_index=0;
  UCSRB.7=1;
end