#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
	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;
} 

void uart_setchar_9bit0(char c) {
    loop_until_bit_is_set(UCSR0A, UDRE0); /* Wait until data register empty. */
    UCSR0B &= ~(1 << TXB80);
    UDR0 = c;
} 

void uart_setchar_9bit1(char c) {
    loop_until_bit_is_set(UCSR0A, UDRE0); /* Wait until data register empty. */
    UCSR0B |= (1 << TXB80);
    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

///////////////////////////////////////////////////////////////////////////////////////////////

void I2C_init(void){
	//TWSR = 0x00;
	TWBR = 0x5C;// TWI clock 100kHz, SCL frequency = F_CPU/(16 + 2(TWBR)*(TWSR prescale))
	TWCR = (1 << TWEN);
	//address 1101000
}

void I2C_start(void){
	TWCR = (1 << TWEN) | (1 << TWINT) |(1 << TWSTA);//twi enable/clear int flag/start (master)
	while ((TWCR & (1<<TWINT)) == 0);//? wait for TWINT to be 1?
	//if ((TWSR & 0xF8) != TW_START) return -1;//comfire start condition has been transmitted
}

void I2C_stop(void){
	TWCR = (1 << TWEN) | (1 << TWINT) |(1 << TWSTO);
}

void I2C_write(uint8_t data){
	TWDR = data;
	TWCR = (1 << TWINT) | (1 << TWEN);
	while ((TWCR & (1<<TWINT)) == 0);//? wait for TWINT to be 1
	//if ((TWSR & 0xF8) != TW_MT_SLA_ACK) return -1;//comfire start condition has been transmitted
}

uint8_t I2C_readACK(){
	TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWEA);//with acknowledge
    while ((TWCR & (1<<TWINT)) == 0);
    return TWDR;
}

uint8_t I2C_readNACK(){
	TWCR = (1<<TWINT)|(1<<TWEN);//without acknowledge
    while ((TWCR & (1<<TWINT)) == 0);
    return TWDR;
}

uint8_t I2C_status(void)
{
    uint8_t status;//mask status
    status = TWSR & 0xF8;
    return status;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////

uint8_t MPU_init(){
	I2C_init();
	I2C_start();
	if (I2C_status() != TW_START) return -1;
	I2C_write(((MPU6050 << 1) | TW_WRITE)); //TW_WRITE = 0 defined in twi.h
	if (I2C_status() != TW_MT_SLA_ACK) return -1; 
	I2C_write(0x6B);//PWR_MGMT_1 address
	if (I2C_status() != TW_MT_DATA_ACK) return -1; 
	I2C_write(0);//set PWR_MGMT_1 to 0
	if (I2C_status() != TW_MT_DATA_ACK) return -1; 
	I2C_stop();
	_delay_us(10);//allow time for stop to be sent
	return 0;
}

uint8_t MPU_write(uint8_t u8addr, uint8_t u8data){
	I2C_init();
	I2C_start();
	if (I2C_status() != TW_START) return -1;
	I2C_write(((MPU6050 << 1) | TW_WRITE)); //TW_WRITE = 0 defined in twi.h
	if (I2C_status() != TW_MT_SLA_ACK) return -1; 
	I2C_write(u8addr);//set address
	if (I2C_status() != TW_MT_DATA_ACK) return -1; 
	I2C_write(u8data);//set value
	if (I2C_status() != TW_MT_DATA_ACK) return -1; 
	I2C_stop();
	_delay_us(10);//allow time for stop to be sent
	return 0;
}

uint8_t MPU_read(uint8_t u8addr, uint16_t *u16data){
	I2C_start();
	if (I2C_status() != TW_START) return -1;
	I2C_write(((MPU6050 << 1) | TW_WRITE)); //TW_WRITE = 0 defined in twi.h
	if (I2C_status() != TW_MT_SLA_ACK) return -1; 
	I2C_write(u8addr); //send reading address of MPU-6050
	if (I2C_status() != TW_MT_DATA_ACK) return -1; 
	I2C_stop();
	_delay_us(10);//wait for data
	
	I2C_start();
	if (I2C_status() != TW_START) return -1;
	I2C_write(((MPU6050 << 1) | TW_READ));//ask to read
	if (I2C_status() != TW_MR_SLA_ACK) return -1;
	
	*u16data = I2C_readACK() << 8; //significant 8 bits with acknowledge
	*u16data |= I2C_readNACK(); //no ACK after the last bytes.
	I2C_stop();
	_delay_us(10);//wait for data
	return 0;
}

///////////////////////////////////////////////////////////////////////////////////////////
//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 M1[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},
};// calibrated

static uint16_t i1 = 0;

uint16_t stepforwardM1(){
   if (i1 == 1023) i1 = 0;
   else i1++;
   OCR1A = pgm_read_byte(&(M1[0][i1]));; 
   OCR1B = pgm_read_byte(&(M1[1][i1]));; 
   OCR2A = pgm_read_byte(&(M1[2][i1]));; 
   return i1;
}

uint16_t stepbackwardM1(){
   if (i1 == 0) i1 = 1023;
   else i1--;
   OCR1A = pgm_read_byte(&(M1[0][i1]));; 
   OCR1B = pgm_read_byte(&(M1[1][i1]));; 
   OCR2A = pgm_read_byte(&(M1[2][i1]));; 
   return i1;
}

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

static uint16_t i2 = 0;

uint16_t stepforwardM2(){
   if (i2 == 1023) i2 = 0;
   else i2++;
   OCR2B = pgm_read_byte(&(M2[0][i2]));; 
   OCR0A = pgm_read_byte(&(M2[1][i2]));; 
   OCR0B = pgm_read_byte(&(M2[2][i2]));; 
   return i2;
}

uint16_t stepbackwardM2(){
   if (i2 == 0) i2 = 1023;
   else i2--;
   OCR2B = pgm_read_byte(&(M2[0][i2]));; 
   OCR0A = pgm_read_byte(&(M2[1][i2]));; 
   OCR0B = pgm_read_byte(&(M2[2][i2]));; 
   return i2;
}

void stepM2(int16_t *steps){
	if (*steps > 0){
		stepforwardM2();
		(*steps)--;
	}
	if (*steps < 0){
		stepbackwardM2();
		(*steps)++;
	}
}

int main(void) {
   CLKPR = (1 << CLKPCE);
   CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0); // set clock divider to 1
   
   DDRB = (1 << PB1) | (1 << PB2) | (1 << PB3); //Motor1
   DDRD = (1 << PD3) | (1 << PD5) | (1 << PD6); //Motor2
   DDRC = (1 << PC1); // LED
   /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   GTCCR = (1<<TSM) | (1<<PSRSYNC) | ((0<<PSRASY)); // halt timers; (1<<PSRASY) halt timer2, (1<<PSRSYNC) halt timer 0&1
   
   TCCR0A |= (1 << COM0A1) | (1<< COM0A0) | (1 << COM0B1) | (1<< COM0B0);//set on upcounting, clear on down couting
   TCCR0A |= (1 << WGM00); // phase correct 8bits mode
   TCCR0B |= (1 << CS00);
   OCR0A = M2[1][0]; // compare value, 8bit
   OCR0B = M2[2][0];
   
   TCCR1A |= (1 << COM1A1) | (1<< COM1A0) | (1 << COM1B1) | (1<< COM1B0);
   TCCR1A |= (1 << WGM10); // phase correct 8bits mode
   TCCR1B |= (1 << CS10); //1 prescale
   OCR1A = M1[0][0];//Cmm[0]; // compare value, 16bit
   OCR1B = M1[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 = M1[2][0]; // compare value, 8bit
   OCR2B = M2[0][0];
   
   TCNT0= 0x00;
   TCNT1L= 0x00;
   TCNT1H= 0x00;
   TCNT2= 0x00;
   
   GTCCR = 0;//release all timer
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   stdout = &mystdout;// setup stdio stream
   uart_init();
   
   int16_t AcY;
   int16_t AcZ;
   int16_t GyroX;
   int16_t GyroY;
   int16_t GyroZ;
   
   uint8_t GyroXH; // break GyroX into two for serial transmition
   uint8_t GyroXL;
   
   uint16_t j = 0;
   
   int16_t stepsM1 = 0;
   double feedGyroY[3];
   double setpointM1 = 0.0;
   uint16_t speedM1 = 1;
/*    double AcZarray[100] = {0};
   uint16_t AcZpointer = 0;
   double AcZsum = 0.0; */
   double feedAcZ = 0.0;
   
   int16_t stepsM2 = 0;
   double feedGyroZ[3];
   double setpointM2 = 0.0;
   uint16_t speedM2 = 1;
/*    double AcYarray[100] = {0};
   uint16_t AcYpointer = 0;
   double AcYsum = 0.0; */
   double feedAcY = 0.0;
   
   MPU_init();
   MPU_write(0x1A, 0);
   //MPU_write(0x1B, (2 << 3));
   
   while (1) {
		if (j >= sensormotor) {
			MPU_read(0x3F,&AcZ); // for M1
			MPU_read(0x45,&GyroY);
			
			MPU_read(0x3D,&AcY); // for M2
			MPU_read(0x47,&GyroZ);
			
			MPU_read(0x43,&GyroX); // for M3
			GyroXH = ((GyroX & 0xFF00)>>8);
			uart_setchar_9bit0(GyroXH);
			GyroXL = GyroX & 0x00FF;
			uart_setchar_9bit1(GyroXL);
			//printf("%" PRId16 "\n",GyroZ); 
			////////////////////////////////////////////////////////////////////////
			/* AcZarray[AcZpointer] = AcZ;
			AcZsum = AcZsum + AcZarray[AcZpointer]; 
			AcZpointer++;
			if (AcZpointer == 100) AcZpointer = 0;
			AcZsum = AcZsum - AcZarray[AcZpointer]; */
			
			feedGyroY[2] = feedGyroY[1];
			feedGyroY[1] = feedGyroY[0];
			/*if (AcZ > 2000) AcZ = 2000;
			if (AcZ < -2000) AcZ = -2000;*/
			feedAcZ = (AcZ-2500)*4.0/65536.0*1048.0*7.0/3.14/2;
			feedGyroY[0] = (GyroY+453.0)*500.0/65536.0*interval*sensormotor/1000000.0*1048.0*7.0/360.0;
			
			setpointM1 = -(feedAcZ*0.02+feedGyroY[0]*30+(feedGyroY[0]-feedGyroY[1])*70+(feedGyroY[2]+feedGyroY[0]-2*feedGyroY[1])*0);
			stepsM1 = (int) setpointM1;
			if (abs(stepsM1) >= sensormotor) speedM1=1;
			else speedM1 = sensormotor/abs(stepsM1);
			////////////////////////////////////////////////////////////////////////
			feedGyroZ[2] = feedGyroZ[1];
			feedGyroZ[1] = feedGyroZ[0];
			if (AcY > 2000) AcY = 2000;
			if (AcY < -2000) AcY = -2000;
			feedAcY = AcY*4.0/65536.0*1048.0*7.0/3.14/2;
			feedGyroZ[0] = (GyroZ+214.6)*500.0/65536.0*interval*sensormotor/1000000.0*1048.0*7.0/360.0;
			
			setpointM2 = (-feedAcY*0.02+feedGyroZ[0]*50+(feedGyroZ[0]-feedGyroZ[1])*100);
			stepsM2 = (int) setpointM2;
			if (abs(stepsM2) >= sensormotor) speedM2=1;
			else speedM2 = sensormotor/abs(stepsM2);
			//////////////////////////////////////////////////////////////////////
			j=0;
		}
		else j++;
		if (j % speedM1 == 0) stepM1(&stepsM1);
		if (j % speedM2 == 0) stepM2(&stepsM2);
		_delay_us(interval);
   }
}