#define F_CPU 20000000
#define BAUD 115200
#define BRC 10//((F_CPU/16/BAUD)-1)
#define MPU6050 0x68
#define interval 10
#define sensormotor 100

#include <avr/io.h>
#include <stdio.h> // for serial
#include <avr/interrupt.h>
#include <util/delay.h>
#include <util/setbaud.h>
#include <util/twi.h>
#include <inttypes.h> // for printf uint16_t
#include <avr/pgmspace.h>

void uart_init(void){
	UBRR0L = BRC;
	UBRR0H = (BRC >> 8); //Baud rate setting
	UCSR0B |= (1 << RXEN0) | (1 << TXEN0); //RxTx enable
	
	UCSR0B |= (1 << RXCIE0); // rx complete interrupt enable
	   
	UCSR0C = (1 << UCSZ01) | (1 << UCSZ00); //8bit char

	UCSR0B |= (1 << UCSZ02); //9bit serial
}

void uart_setchar(char c) {
    loop_until_bit_is_set(UCSR0A, UDRE0); /* Wait until data register empty. */
    UDR0 = c;
} 

char uart_getchar(void) {
    loop_until_bit_is_set(UCSR0A, RXC0); /* Wait until data exists. */
    return UDR0;
}

int usart_putchar_printf(char var, FILE *stream) {
    //if (var == '\n') usart_putchar('\r');
    uart_setchar(var);
    return 0;
}

static FILE mystdout = FDEV_SETUP_STREAM(usart_putchar_printf, NULL, _FDEV_SETUP_WRITE);//define stream


///////////////////////////////////////////////////////////////////////////////////////////
//sinosoidal wave
/* const uint8_t M1[3][1024] PROGMEM = {
	{127,128,129,129,130,131,132,132,133,134,135,135,136,137,138,138,139,140,141,142,142,143,144,145,145,146,147,148,148,149,150,151,151,152,153,154,154,155,156,157,157,158,159,160,160,161,162,163,163,164,165,165,166,167,168,168,169,170,171,171,172,173,173,174,175,176,176,177,178,178,179,180,180,181,182,183,183,184,185,185,186,187,187,188,189,189,190,191,191,192,193,193,194,195,195,196,196,197,198,198,199,200,200,201,201,202,203,203,204,205,205,206,206,207,207,208,209,209,210,210,211,212,212,213,213,214,214,215,215,216,216,217,218,218,219,219,220,220,221,221,222,222,223,223,224,224,225,225,226,226,226,227,227,228,228,229,229,230,230,231,231,231,232,232,233,233,233,234,234,235,235,235,236,236,237,237,237,238,238,238,239,239,239,240,240,240,241,241,241,242,242,242,242,243,243,243,244,244,244,244,245,245,245,245,246,246,246,246,247,247,247,247,247,248,248,248,248,248,249,249,249,249,249,249,250,250,250,250,250,250,250,251,251,251,251,251,251,251,251,251,251,251,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,251,251,251,251,251,251,251,251,251,251,251,250,250,250,250,250,250,250,249,249,249,249,249,249,248,248,248,248,248,247,247,247,247,247,246,246,246,246,245,245,245,245,244,244,244,244,243,243,243,242,242,242,242,241,241,241,240,240,240,239,239,239,238,238,238,237,237,237,236,236,235,235,235,234,234,233,233,233,232,232,231,231,231,230,230,229,229,228,228,227,227,226,226,226,225,225,224,224,223,223,222,222,221,221,220,220,219,219,218,218,217,216,216,215,215,214,214,213,213,212,212,211,210,210,209,209,208,207,207,206,206,205,205,204,203,203,202,201,201,200,200,199,198,198,197,196,196,195,195,194,193,193,192,191,191,190,189,189,188,187,187,186,185,185,184,183,183,182,181,180,180,179,178,178,177,176,176,175,174,173,173,172,171,171,170,169,168,168,167,166,165,165,164,163,163,162,161,160,160,159,158,157,157,156,155,154,154,153,152,151,151,150,149,148,148,147,146,145,145,144,143,142,142,141,140,139,138,138,137,136,135,135,134,133,132,132,131,130,129,129,128,127,126,125,125,124,123,122,122,121,120,119,119,118,117,116,116,115,114,113,112,112,111,110,109,109,108,107,106,106,105,104,103,103,102,101,100,100,99,98,97,97,96,95,94,94,93,92,91,91,90,89,89,88,87,86,86,85,84,83,83,82,81,81,80,79,78,78,77,76,76,75,74,74,73,72,71,71,70,69,69,68,67,67,66,65,65,64,63,63,62,61,61,60,59,59,58,58,57,56,56,55,54,54,53,53,52,51,51,50,49,49,48,48,47,47,46,45,45,44,44,43,42,42,41,41,40,40,39,39,38,38,37,36,36,35,35,34,34,33,33,32,32,31,31,30,30,29,29,28,28,28,27,27,26,26,25,25,24,24,23,23,23,22,22,21,21,21,20,20,19,19,19,18,18,17,17,17,16,16,16,15,15,15,14,14,14,13,13,13,12,12,12,12,11,11,11,10,10,10,10,9,9,9,9,8,8,8,8,7,7,7,7,7,6,6,6,6,6,5,5,5,5,5,5,4,4,4,4,4,4,4,3,3,3,3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,3,3,3,3,4,4,4,4,4,4,4,5,5,5,5,5,5,6,6,6,6,6,7,7,7,7,7,8,8,8,8,9,9,9,9,10,10,10,10,11,11,11,12,12,12,12,13,13,13,14,14,14,15,15,15,16,16,16,17,17,17,18,18,19,19,19,20,20,21,21,21,22,22,23,23,23,24,24,25,25,26,26,27,27,28,28,28,29,29,30,30,31,31,32,32,33,33,34,34,35,35,36,36,37,38,38,39,39,40,40,41,41,42,42,43,44,44,45,45,46,47,47,48,48,49,49,50,51,51,52,53,53,54,54,55,56,56,57,58,58,59,59,60,61,61,62,63,63,64,65,65,66,67,67,68,69,69,70,71,71,72,73,74,74,75,76,76,77,78,78,79,80,81,81,82,83,83,84,85,86,86,87,88,89,89,90,91,91,92,93,94,94,95,96,97,97,98,99,100,100,101,102,103,103,104,105,106,106,107,108,109,109,110,111,112,112,113,114,115,116,116,117,118,119,119,120,121,122,122,123,124,125,125,126},
	{235,235,234,234,234,233,233,232,232,232,231,231,230,230,229,229,229,228,228,227,227,226,226,225,225,224,224,223,223,222,222,221,221,220,220,219,219,218,218,217,217,216,216,215,215,214,214,213,212,212,211,211,210,210,209,208,208,207,207,206,206,205,204,204,203,202,202,201,201,200,199,199,198,198,197,196,196,195,194,194,193,192,192,191,190,190,189,188,188,187,186,186,185,184,184,183,182,182,181,180,180,179,178,177,177,176,175,175,174,173,172,172,171,170,170,169,168,167,167,166,165,165,164,163,162,162,161,160,159,159,158,157,156,156,155,154,153,153,152,151,150,150,149,148,147,147,146,145,144,144,143,142,141,141,140,139,138,137,137,136,135,134,134,133,132,131,131,130,129,128,128,127,126,125,124,124,123,122,121,121,120,119,118,118,117,116,115,114,114,113,112,111,111,110,109,108,108,107,106,105,105,104,103,102,102,101,100,99,99,98,97,96,96,95,94,93,93,92,91,90,90,89,88,88,87,86,85,85,84,83,82,82,81,80,80,79,78,78,77,76,75,75,74,73,73,72,71,71,70,69,69,68,67,67,66,65,65,64,63,63,62,61,61,60,59,59,58,57,57,56,55,55,54,54,53,52,52,51,50,50,49,49,48,48,47,46,46,45,45,44,43,43,42,42,41,41,40,40,39,38,38,37,37,36,36,35,35,34,34,33,33,32,32,31,31,30,30,29,29,28,28,27,27,26,26,26,25,25,24,24,23,23,23,22,22,21,21,20,20,20,19,19,18,18,18,17,17,17,16,16,16,15,15,15,14,14,14,13,13,13,12,12,12,11,11,11,11,10,10,10,9,9,9,9,8,8,8,8,8,7,7,7,7,6,6,6,6,6,6,5,5,5,5,5,5,4,4,4,4,4,4,4,3,3,3,3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,3,3,3,4,4,4,4,4,4,4,4,5,5,5,5,5,5,6,6,6,6,6,7,7,7,7,7,8,8,8,8,9,9,9,9,10,10,10,10,11,11,11,12,12,12,13,13,13,13,14,14,14,15,15,15,16,16,17,17,17,18,18,18,19,19,20,20,20,21,21,22,22,22,23,23,24,24,25,25,25,26,26,27,27,28,28,29,29,30,30,31,31,32,32,33,33,34,34,35,35,36,36,37,37,38,38,39,39,40,40,41,42,42,43,43,44,44,45,46,46,47,47,48,48,49,50,50,51,52,52,53,53,54,55,55,56,56,57,58,58,59,60,60,61,62,62,63,64,64,65,66,66,67,68,68,69,70,70,71,72,72,73,74,74,75,76,77,77,78,79,79,80,81,82,82,83,84,84,85,86,87,87,88,89,89,90,91,92,92,93,94,95,95,96,97,98,98,99,100,101,101,102,103,104,104,105,106,107,107,108,109,110,110,111,112,113,113,114,115,116,117,117,118,119,120,120,121,122,123,123,124,125,126,126,127,128,129,130,130,131,132,133,133,134,135,136,136,137,138,139,140,140,141,142,143,143,144,145,146,146,147,148,149,149,150,151,152,152,153,154,155,155,156,157,158,158,159,160,161,161,162,163,164,164,165,166,166,167,168,169,169,170,171,172,172,173,174,174,175,176,176,177,178,179,179,180,181,181,182,183,183,184,185,185,186,187,187,188,189,190,190,191,191,192,193,193,194,195,195,196,197,197,198,199,199,200,200,201,202,202,203,204,204,205,205,206,206,207,208,208,209,209,210,211,211,212,212,213,213,214,214,215,216,216,217,217,218,218,219,219,220,220,221,221,222,222,223,223,224,224,225,225,226,226,227,227,228,228,228,229,229,230,230,231,231,231,232,232,233,233,234,234,234,235,235,236,236,236,237,237,237,238,238,238,239,239,239,240,240,240,241,241,241,242,242,242,243,243,243,243,244,244,244,245,245,245,245,246,246,246,246,246,247,247,247,247,248,248,248,248,248,248,249,249,249,249,249,249,250,250,250,250,250,250,250,251,251,251,251,251,251,251,251,251,251,251,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,251,251,251,251,251,251,251,251,251,251,250,250,250,250,250,250,250,250,249,249,249,249,249,249,248,248,248,248,248,247,247,247,247,247,246,246,246,246,245,245,245,245,244,244,244,244,243,243,243,242,242,242,241,241,241,241,240,240,240,239,239,239,238,238,237,237,237,236,236,236},
	{19,18,18,18,17,17,17,16,16,15,15,15,14,14,14,13,13,13,13,12,12,12,11,11,11,10,10,10,10,9,9,9,9,8,8,8,8,7,7,7,7,7,6,6,6,6,6,5,5,5,5,5,5,4,4,4,4,4,4,4,4,3,3,3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,3,3,3,3,4,4,4,4,4,4,4,5,5,5,5,5,5,6,6,6,6,6,6,7,7,7,7,8,8,8,8,8,9,9,9,9,10,10,10,11,11,11,11,12,12,12,13,13,13,14,14,14,15,15,15,16,16,16,17,17,17,18,18,18,19,19,20,20,20,21,21,22,22,23,23,23,24,24,25,25,26,26,26,27,27,28,28,29,29,30,30,31,31,32,32,33,33,34,34,35,35,36,36,37,37,38,38,39,40,40,41,41,42,42,43,43,44,45,45,46,46,47,48,48,49,49,50,50,51,52,52,53,54,54,55,55,56,57,57,58,59,59,60,61,61,62,63,63,64,65,65,66,67,67,68,69,69,70,71,71,72,73,73,74,75,75,76,77,78,78,79,80,80,81,82,82,83,84,85,85,86,87,88,88,89,90,90,91,92,93,93,94,95,96,96,97,98,99,99,100,101,102,102,103,104,105,105,106,107,108,108,109,110,111,111,112,113,114,114,115,116,117,118,118,119,120,121,121,122,123,124,124,125,126,127,128,128,129,130,131,131,132,133,134,134,135,136,137,137,138,139,140,141,141,142,143,144,144,145,146,147,147,148,149,150,150,151,152,153,153,154,155,156,156,157,158,159,159,160,161,162,162,163,164,165,165,166,167,167,168,169,170,170,171,172,172,173,174,175,175,176,177,177,178,179,180,180,181,182,182,183,184,184,185,186,186,187,188,188,189,190,190,191,192,192,193,194,194,195,196,196,197,198,198,199,199,200,201,201,202,202,203,204,204,205,206,206,207,207,208,208,209,210,210,211,211,212,212,213,214,214,215,215,216,216,217,217,218,218,219,219,220,220,221,221,222,222,223,223,224,224,225,225,226,226,227,227,228,228,229,229,229,230,230,231,231,232,232,232,233,233,234,234,234,235,235,236,236,236,237,237,237,238,238,239,239,239,240,240,240,241,241,241,241,242,242,242,243,243,243,244,244,244,244,245,245,245,245,246,246,246,246,247,247,247,247,247,248,248,248,248,248,249,249,249,249,249,249,250,250,250,250,250,250,250,250,251,251,251,251,251,251,251,251,251,251,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,251,251,251,251,251,251,251,251,251,251,251,250,250,250,250,250,250,250,249,249,249,249,249,249,248,248,248,248,248,248,247,247,247,247,246,246,246,246,246,245,245,245,245,244,244,244,243,243,243,243,242,242,242,241,241,241,240,240,240,239,239,239,238,238,238,237,237,237,236,236,236,235,235,234,234,234,233,233,232,232,231,231,231,230,230,229,229,228,228,228,227,227,226,226,225,225,224,224,223,223,222,222,221,221,220,220,219,219,218,218,217,217,216,216,215,214,214,213,213,212,212,211,211,210,209,209,208,208,207,206,206,205,205,204,204,203,202,202,201,200,200,199,199,198,197,197,196,195,195,194,193,193,192,191,191,190,190,189,188,187,187,186,185,185,184,183,183,182,181,181,180,179,179,178,177,176,176,175,174,174,173,172,172,171,170,169,169,168,167,166,166,165,164,164,163,162,161,161,160,159,158,158,157,156,155,155,154,153,152,152,151,150,149,149,148,147,146,146,145,144,143,143,142,141,140,140,139,138,137,136,136,135,134,133,133,132,131,130,130,129,128,127,126,126,125,124,123,123,122,121,120,120,119,118,117,117,116,115,114,113,113,112,111,110,110,109,108,107,107,106,105,104,104,103,102,101,101,100,99,98,98,97,96,95,95,94,93,92,92,91,90,89,89,88,87,87,86,85,84,84,83,82,82,81,80,79,79,78,77,77,76,75,74,74,73,72,72,71,70,70,69,68,68,67,66,66,65,64,64,63,62,62,61,60,60,59,58,58,57,56,56,55,55,54,53,53,52,52,51,50,50,49,48,48,47,47,46,46,45,44,44,43,43,42,42,41,40,40,39,39,38,38,37,37,36,36,35,35,34,34,33,33,32,32,31,31,30,30,29,29,28,28,27,27,26,26,25,25,25,24,24,23,23,22,22,22,21,21,20,20,20,19},
}; */

const uint8_t M3[3][1024] PROGMEM = {
	{127,134,141,148,152,154,156,157,158,159,160,160,161,162,163,163,164,165,166,166,167,167,168,168,169,169,170,170,171,171,172,172,173,173,173,174,174,175,175,175,176,176,176,177,177,178,178,179,179,179,180,180,180,181,181,181,182,182,182,182,183,183,184,184,184,185,185,185,186,186,186,187,187,187,188,188,188,189,189,189,190,190,190,191,191,191,192,192,192,193,193,193,194,194,194,195,195,196,196,196,197,197,197,198,198,199,199,199,200,200,200,201,201,201,202,202,202,203,203,203,204,204,204,205,205,205,205,206,206,206,207,207,208,208,208,209,209,209,210,210,211,211,211,212,212,213,213,213,214,214,215,215,216,216,217,217,217,218,218,219,220,220,221,222,223,225,228,232,236,240,241,245,245,246,246,246,246,247,247,247,247,248,248,248,248,248,248,249,249,249,249,249,249,249,250,250,250,250,250,250,250,250,250,251,251,251,251,251,251,251,251,251,251,251,251,251,251,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,251,251,251,251,251,251,251,251,251,251,251,251,251,251,250,250,250,250,250,250,250,250,250,250,249,249,249,249,249,249,249,249,248,248,248,248,248,248,248,247,247,247,247,247,247,247,246,246,246,246,245,244,243,241,239,237,235,231,229,227,225,224,224,223,222,222,222,221,221,220,220,219,219,218,218,218,217,217,217,216,216,216,215,215,215,214,214,214,213,213,213,212,212,212,212,212,211,211,211,210,210,209,209,209,208,208,208,207,207,207,206,206,205,205,205,204,204,204,204,203,203,203,202,202,201,201,201,200,200,199,199,199,198,198,198,197,197,197,196,196,196,195,195,194,194,194,193,193,193,192,192,191,191,191,190,190,190,189,189,189,188,188,187,187,187,186,186,185,185,185,184,184,183,183,182,182,181,181,181,180,180,179,179,178,178,177,177,177,176,175,175,175,174,174,173,173,172,172,171,171,170,170,169,169,168,168,167,167,166,166,166,165,165,164,164,163,163,162,162,162,161,161,160,160,159,159,158,158,158,157,156,156,155,155,154,153,153,152,151,151,150,149,148,148,147,146,145,144,142,139,135,129,124,115,106,99,98,97,96,95,94,93,93,92,91,91,90,90,89,89,88,88,88,87,87,87,86,86,85,85,84,84,84,83,83,82,82,81,81,80,80,80,79,79,78,78,78,77,77,76,76,76,75,75,75,74,74,74,73,73,73,72,72,72,71,71,71,70,70,69,69,69,68,68,68,67,67,66,66,65,65,65,64,64,63,63,63,62,62,61,61,61,60,60,60,59,59,58,58,57,57,57,56,56,55,55,55,54,54,53,53,53,52,52,52,51,51,50,50,50,49,49,49,48,48,47,47,47,46,46,46,45,45,45,45,44,44,43,43,43,43,42,42,42,41,41,40,40,40,39,39,38,38,37,37,37,36,36,35,35,34,34,33,33,32,31,29,25,22,19,15,12,11,9,9,8,8,8,7,7,7,6,6,6,6,6,5,5,5,5,5,5,5,4,4,4,4,4,4,4,4,4,4,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,4,4,4,4,4,4,4,4,4,4,4,4,5,5,5,5,5,5,5,5,6,6,6,6,6,6,7,7,7,7,8,8,9,10,11,13,16,19,23,26,29,29,30,30,31,31,32,32,32,33,33,34,34,34,35,35,36,36,36,37,37,37,38,38,38,39,39,40,40,40,41,41,42,42,42,43,43,43,44,44,45,45,46,46,46,47,47,47,48,49,49,49,50,50,50,51,51,51,52,52,52,53,53,53,54,54,54,55,55,55,55,56,56,56,57,57,57,58,58,58,58,59,59,59,60,60,60,61,61,61,62,62,62,62,63,63,63,64,64,64,65,65,65,66,66,67,67,67,68,68,69,69,69,70,70,71,71,72,72,72,73,73,73,74,74,75,75,76,76,77,77,78,78,79,79,79,80,80,81,81,81,82,82,83,83,84,84,85,85,86,86,87,87,88,88,88,89,90,90,91,92,92,93,93,93,94,95,95,96,96,97,97,98,99,99,100,100,101,101,102,103,103,104,105,105,106,107,108,108,110,111,114,121},
	{235,231,228,223,220,219,218,217,217,216,215,215,214,213,213,212,212,211,211,210,210,209,209,209,208,208,207,207,207,206,206,205,205,205,204,204,204,203,203,203,202,202,202,201,201,200,200,200,199,199,199,199,198,198,198,197,197,197,197,196,196,196,195,195,195,194,194,194,193,193,193,192,192,192,191,191,191,190,190,190,189,189,189,188,188,188,187,187,187,186,186,186,185,185,184,184,184,183,183,182,182,182,181,181,180,180,180,179,179,178,178,178,177,177,176,176,176,175,175,175,174,174,173,173,173,173,172,172,171,171,170,170,169,169,168,168,168,167,167,166,166,165,165,164,164,163,163,162,161,161,160,160,159,158,158,157,156,156,155,154,153,152,151,150,148,145,140,133,126,117,113,105,103,102,101,100,99,98,97,97,96,95,94,94,93,93,92,91,91,90,89,89,88,87,87,86,86,85,85,84,83,83,83,82,81,81,80,80,79,79,78,77,77,76,76,75,75,74,74,73,73,72,72,71,71,70,70,70,69,69,68,68,67,67,66,66,66,65,65,64,64,63,63,62,62,62,61,61,60,60,59,59,58,58,58,57,57,56,56,55,55,54,54,53,53,52,52,51,51,51,50,50,49,49,48,48,47,47,46,46,46,45,45,44,44,44,43,43,42,42,42,41,41,40,40,39,39,39,38,38,38,37,37,36,36,36,35,35,34,34,33,31,29,26,23,21,18,15,13,12,11,10,10,10,9,9,9,9,8,8,8,8,8,7,7,7,7,7,7,7,6,6,6,6,6,6,6,6,5,5,5,5,5,5,5,5,5,5,5,5,4,4,4,4,4,4,4,4,4,4,4,4,4,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,4,4,4,4,4,4,4,4,4,4,4,5,5,5,5,5,5,5,5,5,5,6,6,6,6,6,6,6,6,7,7,7,7,7,7,7,8,8,8,8,8,9,9,9,9,10,10,10,11,11,11,12,13,15,18,20,25,31,36,36,37,38,38,39,39,40,41,41,41,42,42,43,43,43,44,44,44,44,45,45,45,46,46,47,47,47,48,48,48,49,49,49,50,50,51,51,51,52,52,52,53,53,53,54,54,54,55,55,55,55,56,56,56,57,57,57,58,58,58,59,59,59,60,60,60,61,61,62,62,62,63,63,64,64,64,65,65,66,66,66,67,67,68,68,68,69,69,70,70,70,71,71,72,72,73,73,74,74,75,75,75,76,76,77,77,78,78,78,79,79,80,80,81,81,81,82,82,83,83,84,84,85,85,85,86,86,86,87,87,88,88,89,89,89,90,90,91,91,92,92,93,93,94,94,95,96,96,97,97,98,99,99,100,101,102,102,103,104,105,108,115,120,126,136,141,146,149,152,153,154,155,156,157,158,159,160,160,161,162,162,163,164,164,165,166,166,167,167,168,168,169,170,170,171,171,172,172,173,173,174,174,175,175,176,176,177,177,178,178,179,179,179,180,180,181,181,181,182,182,182,183,183,184,184,184,185,185,185,186,186,187,187,187,188,188,188,189,189,190,190,191,191,192,192,192,193,193,194,194,194,195,195,196,196,197,197,198,198,199,199,200,200,201,201,201,202,202,203,203,203,204,204,205,205,206,206,207,207,207,207,208,208,209,209,209,210,210,210,211,211,212,212,213,213,214,214,214,215,215,216,216,217,217,218,219,220,221,223,225,228,232,235,239,241,243,243,244,244,244,245,245,245,245,245,246,246,246,246,246,246,247,247,247,247,247,247,247,248,248,248,248,248,248,248,248,249,249,249,249,249,249,249,249,250,250,250,250,250,250,250,250,250,250,250,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,250,250,250,250,250,250,250,250,250,250,250,249,249,249,249,249,249,249,249,248,248,248,248,248,248,248,247,247,247,247,247,247,246,246,246,246,246,246,245,245,245,245,244,244,244,244,243,243,242,241,238},
	{19,15,13,10,8,8,7,7,7,6,6,6,6,6,5,5,5,5,5,5,4,4,4,4,4,4,4,4,4,4,4,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,4,4,4,4,4,4,4,4,4,4,4,4,5,5,5,5,5,5,5,5,6,6,6,6,6,6,7,7,7,7,7,7,8,8,8,9,9,10,11,13,16,19,24,26,31,33,33,34,35,35,36,37,37,38,39,39,39,40,40,41,41,42,42,42,43,43,44,45,45,45,46,46,47,47,48,48,49,49,50,50,51,51,51,52,53,53,53,54,54,55,55,56,56,57,57,57,58,58,59,59,60,60,60,61,61,62,62,63,63,63,64,64,65,65,66,66,67,67,68,68,68,69,69,70,70,71,71,72,72,73,73,74,74,75,75,76,76,77,77,78,79,79,80,80,81,81,82,82,83,84,84,84,85,85,86,87,87,88,88,89,89,90,90,91,91,92,92,93,94,94,95,95,96,96,97,97,98,98,99,99,100,101,101,103,106,110,114,119,124,128,134,139,143,145,146,147,148,149,150,150,151,152,152,153,154,155,155,156,156,157,157,158,158,159,159,159,160,160,161,161,162,162,163,163,163,164,164,164,165,165,165,166,166,167,167,168,168,169,169,169,170,170,171,171,172,172,172,173,173,174,174,174,175,175,175,176,176,177,177,178,178,179,179,179,180,180,180,181,181,182,182,182,183,183,184,184,185,185,185,186,186,186,187,187,188,188,188,189,189,189,190,190,190,191,191,192,192,192,193,193,193,194,194,195,195,196,196,196,197,197,198,198,198,199,199,200,200,200,201,201,202,202,203,203,203,204,204,204,205,205,206,206,206,207,207,208,208,208,209,209,210,210,210,211,211,211,212,212,212,213,213,213,214,214,214,215,215,215,216,216,216,217,217,217,218,218,219,219,220,220,221,221,222,222,223,223,223,224,225,225,226,227,229,231,234,237,241,244,247,247,247,247,248,248,248,248,248,249,249,249,249,249,249,249,249,249,250,250,250,250,250,250,250,250,250,250,250,250,250,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,252,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,251,250,250,250,250,250,250,250,250,250,250,250,250,250,249,249,249,249,249,249,249,249,249,249,248,248,248,248,248,248,247,247,247,247,247,247,246,246,246,246,245,245,245,245,243,241,239,236,231,227,225,222,221,220,219,219,218,217,216,216,215,215,214,214,213,213,212,212,211,211,210,210,209,209,208,208,207,207,207,206,206,205,205,205,204,204,203,203,202,202,201,201,201,200,200,199,199,199,198,198,198,197,197,197,196,196,196,195,195,195,194,194,193,193,193,192,192,192,191,191,191,190,190,189,189,188,188,187,187,187,186,186,185,185,184,184,184,183,183,182,181,181,180,180,179,179,178,178,177,177,176,176,175,175,175,174,174,173,172,172,171,171,170,170,170,169,169,168,168,167,167,166,166,165,165,164,164,163,162,162,161,161,160,159,159,158,157,156,155,154,153,152,148,145,140,133,127,119,114,110,108,108,107,106,105,105,104,103,103,102,102,101,101,100,99,99,98,98,97,97,96,96,95,95,94,94,93,93,92,92,91,91,90,90,89,89,88,88,87,87,86,86,85,85,84,84,83,83,82,82,81,81,80,80,80,79,79,78,78,78,77,77,76,76,76,75,75,75,74,74,74,73,73,73,72,72,72,71,71,71,70,70,70,69,69,69,68,68,68,67,67,67,67,66,66,66,65,65,65,64,64,64,63,63,62,62,62,61,61,61,60,60,59,59,59,58,58,57,57,57,56,56,56,55,55,54,54,53,53,53,52,52,51,51,51,50,50,50,49,49,49,48,48,48,47,47,46,46,45,45,45,44,44,44,43,43,42,42,41,41,41,40,40,40,39,39,38,38,37,37,37,36,36,35,35,35,34,34,33,33,33,32,32,31,31,30,30,29,28,28,26,22},
};

static uint16_t i = 0;

uint16_t stepforwardM3(){
   if (i == 1023) {
      i = 0;
	  //PORTC ^= (1 << PC1);
   }
   else{
      i++;
   }
   OCR1A = pgm_read_byte(&(M3[0][i]));; 
   OCR1B = pgm_read_byte(&(M3[1][i]));; 
   OCR2A = pgm_read_byte(&(M3[2][i]));; 
   return i;
}

uint16_t stepbackwardM3(){
   if (i == 0) {
      i = 1023;
	  //PORTC ^= (1 << PC1);
   }
   else{
      i--;
   }
   OCR1A = pgm_read_byte(&(M3[0][i]));; 
   OCR1B = pgm_read_byte(&(M3[1][i]));; 
   OCR2A = pgm_read_byte(&(M3[2][i]));; 
   return i;
}

void stepM3(int16_t *steps){
	if (*steps > 0){
		stepforwardM3();
		(*steps)--;
	}
	if (*steps < 0){
		stepbackwardM3();
		(*steps)++;
	}
}

int16_t steps = 0;
double feedGyroX[2] = {0.0};
uint16_t j = 0;
double setpoint = 0.0;
uint16_t speed = 1;
int16_t GyroX = 0;
uint8_t DataR[2] = {0};
uint8_t DataDigit=0;
double feedjoyX[2] = {0.0};

int main(void) {
   // set clock divider to /1
   CLKPR = (1 << CLKPCE);
   CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
   //
   DDRB = (1 << PB1) | (1 << PB2) | (1 << PB3);
   DDRC = (1 << PC1);
   
   GTCCR = (1<<TSM) | (1<<PSRSYNC) | ((0<<PSRASY)); // halt timers; (1<<PSRASY) halt timer2, (1<<PSRSYNC) halt timer 0&1
      
   TCCR1A |= (1 << COM1A1) | (1<< COM1A0) | (1 << COM1B1) | (1<< COM1B0);
   TCCR1A |= (1 << WGM10); // phase correct 8bits mode
   TCCR1B |= (1 << CS10); //1 prescale
   OCR1A = M3[0][0];//Cmm[0]; // compare value, 16bit
   OCR1B = M3[1][0];

   TCCR2A |= (1 << COM2A1) | (1<< COM2A0) | (1 << COM2B1) | (1<< COM2B0);//set on upcounting, clear on down couting
   TCCR2A |= (1 << WGM20); // phase correct 8bits mode
   TCCR2B |= (1 << CS20);
   OCR2A = M3[2][0]; // compare value, 8bit
   
   //TCCR0B |= (1 << CS00); //1 prescale
   //TCNT0= 0x00;
   TCNT1L= 0x00;
   TCNT1H= 0x00;
   TCNT2= 0x00;
   
   GTCCR = 0;//release all timer
   
   //ADC setup
   ADMUX = (1 << ADLAR) | (1 << MUX1) | (1 << MUX2);// left align reading, ADC6
   ADMUX |= (1 << REFS0);
   ADCSRA = (1 << ADPS0) | (1 << ADPS1) | (1 << ADPS2) | (1 << ADATE);
   ADCSRA |= (1 << ADEN);
   ADCSRA |= (1 << ADSC);
   //ACD enable, start conversation, 128 cloack factor, auto triger enable
   
   stdout = &mystdout;// setup stdio stream
   uart_init();   
   sei(); // interrupt enable
   
   while (1) {
		j++;
		if (j % speed == 0) stepM3(&steps);
		_delay_us(interval);
   }
}

ISR(USART_RX_vect){
	 uint8_t status = UCSR0A;
	 DataDigit = UCSR0B;
	 DataDigit = (DataDigit >> 1) & 0x01;
	 if (DataDigit == 1) {
		DataR[1] = UDR0;
		//DataDigit = 0;
		GyroX = DataR[1] | (DataR[0] << 8);
		//GyroX = -550;
		//PORTC ^= (1 << PC1);
		feedGyroX[1] = feedGyroX[0];
		feedGyroX[0] = (GyroX+610)*500.0/65536.0*interval*sensormotor/1000000.0*1048.0*7.0/360.0;
		
		/* joyX = 127;
		if ((joyX > 100) & (joyX < 155)) feedjoyX = 0.0;		
		else if (joyX <= 100) feedjoyX = (joyX -100.0)/30.0;
		else if (joyX >=155) feedjoyX = (joyX -155.0)/30.0; */
		feedjoyX[1] = feedjoyX[0];
		if ((ADCH<100) | (ADCH>155)) feedjoyX[0] = (ADCH-127.0)/2.0;
		else feedjoyX[0] = 0.0;
		
		setpoint = -(feedGyroX[0]*50+(feedGyroX[0]-feedGyroX[1])*100)+feedjoyX[0]-(feedjoyX[0]-feedjoyX[1])*0.95;
		steps = (int) setpoint;
		//printf("%" PRId16 "\n",steps); 
		if (abs(steps) >= sensormotor) speed=1;
		else speed = sensormotor/abs(steps);
		j=0;
	}
	else {
		DataR[0] = UDR0;
		//DataDigit = 1;
	} 
}