//
// Motors module common operations library
//
// Department of Mechatronics
// Tallinn University of Technology
// © Copyrights 2012
//

//
// Take use of pin and timer operations
//
#include "pin.h"

#if defined (__AVR_ATmega128__)
#include "timer128.h"
#elif defined (__AVR_ATmega2561__)
#include "timer2561.h"
#endif

//
// Take use of delay functions
//
#include <util/delay.h>

//
// Use interrupts
//
#include <avr/interrupt.h>

//
// PWM full period (20ms)
//
#define PWM_PERIOD      (F_CPU / 8 / 50)
 
//
// PWM servo middle position (1.5ms)
//
#define PWM_MIDDLE_POS  (PWM_PERIOD * 15 / 200)
 
//
// PWM ratio to get position from -100 to 100
// We add +1 to get a full range and a little bit more
//
#define PWM_RATIO       (PWM_PERIOD / 20 / 2 / 100 + 1)

//
// maximum number of PWM channels for DC Motor PWM
//
#define CHMAX       8 

//
// default PWM value at start up for all DC Motor PWM channel
//   
#define PWMDEFAULT  0x00 
 
// global buffers
unsigned char compare[CHMAX];
volatile unsigned char compbuff[CHMAX];

// global variables
unsigned char motor[4];

//
// Pin configuration
//
static pin dcmotor_pins[4][2] =
{
	{ PIN(B, 7), PIN(B, 4) },
	{ PIN(D, 1), PIN(D, 0) },
	{ PIN(D, 7), PIN(D, 6) },
	{ PIN(D, 5), PIN(D, 4) }
};

static pin unipolar_pins[2][4] =
{
	{
		PIN(E, 2), PIN(E, 3),
		PIN(E, 4), PIN(E, 5)
	},
	{
		PIN(E, 0), PIN(E, 1),
		PIN(E, 6), PIN(E, 7)
	}
};

static pin bipolar_pins[4] =
{
	PIN(B, 0), PIN(B, 1),
	PIN(B, 2), PIN(B, 3),	
};

static pin servo_pins[2] =
{
	PIN(B, 5), PIN(B, 6)
};

static int motorindex[4][2] =
{
	{ 0, 1 },
	{ 2, 3 },
	{ 4, 5 },
	{ 6, 7 }
};

//
// Initialize specified DC-motor
//
void dcmotor_init(unsigned char index)
{	
	pin_setup_output(dcmotor_pins[index][0]);
	pin_setup_output(dcmotor_pins[index][1]);
}
 
//
// Drive command for specified DC-motor
//
void dcmotor_drive(unsigned char index, signed char direction)
{	
	pin_set_to(dcmotor_pins[index][0], direction < 0);			
	pin_set_to(dcmotor_pins[index][1], direction > 0);
}


//
// Initialize PWM for specified DC-motor. You can also choose prescaler. 
//
void dcmotor_drive_pwm_init(unsigned char index, timer2_prescale prescaler)
{
  	unsigned char i, pwm;
	
	pin_setup_output(dcmotor_pins[index][0]);
	pin_setup_output(dcmotor_pins[index][1]);
	motor[index] = 1;
	  
    
  	pwm = PWMDEFAULT;


  	for(i=0 ; i<CHMAX ; i++)      // initialise all channels
  	{
    	compare[i] = pwm;           // set default PWM values
    	compbuff[i] = pwm;          // set default PWM values
  	}

	// Taimer 2 normaalre˛iimi, taktijagurit ei kasutata
	timer2_init_normal(prescaler);
  	// Taimer 2 ületäitumise katkestuse lubamine
	timer2_overflow_interrupt_enable(true);

  	// Enable interrupts
	sei();
}

//
// Service Routine for DC motor PWM interrupt 
//
ISR(TIMER2_OVF_vect)
{
  	//static unsigned char pinlevelB=PORTB_MASK, pinlevelD=PORTD_MASK;
 	 static unsigned char softcount=0xFF;

	// increment modulo 256 counter and update
	// the compare values only when counter = 0.
   	if(++softcount == 0)
  	{                                         
    	compare[0] = compbuff[0];   
    	compare[1] = compbuff[1];
    	compare[2] = compbuff[2];
    	compare[3] = compbuff[3];
    	compare[4] = compbuff[4];
    	compare[5] = compbuff[5];
    	compare[6] = compbuff[6];
    	compare[7] = compbuff[7];	
    	// last element should equal CHMAX - 1

    	if(motor[0]==1) pin_set(dcmotor_pins[0][1]);
		if(motor[0]==1) pin_set(dcmotor_pins[0][0]);
    	if(motor[1]==1) pin_set(dcmotor_pins[1][1]);
		if(motor[1]==1) pin_set(dcmotor_pins[1][0]);
		if(motor[2]==1) pin_set(dcmotor_pins[2][1]);
		if(motor[2]==1) pin_set(dcmotor_pins[2][0]);
		if(motor[3]==1) pin_set(dcmotor_pins[3][1]);
		if(motor[3]==1) pin_set(dcmotor_pins[3][0]);	

  	}
  	
	// clear port pin on compare match (executed on next interrupt)
	if((compare[0] == softcount)&&(motor[0]==1)) pin_clear(dcmotor_pins[0][1]);
  	if((compare[1] == softcount)&&(motor[0]==1)) pin_clear(dcmotor_pins[0][0]);	
  	if((compare[2] == softcount)&&(motor[1]==1)) pin_clear(dcmotor_pins[1][1]);
  	if((compare[3] == softcount)&&(motor[1]==1)) pin_clear(dcmotor_pins[1][0]);
	if((compare[4] == softcount)&&(motor[2]==1)) pin_clear(dcmotor_pins[2][1]);
  	if((compare[5] == softcount)&&(motor[2]==1)) pin_clear(dcmotor_pins[2][0]);
  	if((compare[6] == softcount)&&(motor[3]==1)) pin_clear(dcmotor_pins[3][1]);
  	if((compare[7] == softcount)&&(motor[3]==1)) pin_clear(dcmotor_pins[3][0]);
	
	 	
}

//
// Change PWM for specified DC-motor
//
void dcmotor_drive_pwm(unsigned char index, signed char direction, unsigned char speed) 
{
	if(direction == -1)
	{
		compbuff[motorindex[index][0]] = 0x00;
		compbuff[motorindex[index][1]] = speed;
	}
	if(direction == 1)
	{
		compbuff[motorindex[index][0]] = speed;
		compbuff[motorindex[index][1]] = 0x00;
	} 
}


//
// Initialize specified unipolar stepper motor
//
void unipolar_init(unsigned char index)
{	
	for (unsigned char i = 0; i < 4; i++)
	{
		// Set control pins as output
		pin_setup_output(unipolar_pins[index][i]);
		
		// Initially stop
		pin_clear(unipolar_pins[index][i]);
	}
}

//
// Unipolar stepper motor half-step
//
void unipolar_halfstep(unsigned char index, signed char direction, unsigned short num_steps, unsigned char speed)
{
	unsigned short i;
	unsigned char state1 = 0, state2 = 1;
	unsigned char pattern;
	unsigned char timeout;

	// Ensure +-1
	direction = ((direction < 0) ? -1 : +1);

	// Ensure speed <= 100
	speed = (speed > 100 ? 100 : speed);

	// Complete steps
	for (i = 0; i < num_steps; i++)
	{		
		state1 += direction;
		state2 += direction;

		// Make pattern
		pattern =
			(1 << ((state1 % 8) >> 1)) |
			(1 << ((state2 % 8) >> 1));
		
		// Set output
		PIN_ARRAY_OF_4_MASK(unipolar_pins[index], pattern & 0x0F);

		// Wait for the motor to complete the step		
		timeout = speed;
		
		while (timeout-- > 0)
		{			
			_delay_ms(1);
		}
	}

	// Turn off motor
	PIN_ARRAY_OF_4_MASK(unipolar_pins[index], 0x00);
}

//
// Bipolar stepper initialization
//
void bipolar_init(void)
{
	for (unsigned char i = 0; i < 4; i++)
	{
		// Set control pins as output
		pin_setup_output(bipolar_pins[i]);
		
		// Initially stop
		pin_clear(bipolar_pins[i]);
	}
}

//
// Bipolar stepper motor half-step
//
void bipolar_halfstep(signed char direction, unsigned short num_steps, unsigned char speed)
{
	unsigned short i;
	unsigned char state1 = 0, state2 = 1;
	unsigned char pattern;
	unsigned char timeout;

	// Ensure +-1
	direction = ((direction < 0) ? -1 : +1);

	// Ensure speed <= 100
	speed = (speed > 100 ? 100 : speed);

	// Complete steps
	for (i = 0; i < num_steps; i++)
	{		
		state1 += direction;
		state2 += direction;

		// Make pattern
		pattern =
			(1 << ((state1 % 8) >> 1)) |
			(1 << ((state2 % 8) >> 1));
		
		// Set output
		PIN_ARRAY_OF_4_MASK(bipolar_pins, pattern & 0x0F);

		// Wait for the motor to complete the step		
		timeout = speed;
		
		while (timeout-- > 0)
		{			
			_delay_ms(1);
		}
	}

	// Turn off motor
	PIN_ARRAY_OF_4_MASK(bipolar_pins, 0x00);
}

//
// Initialize specified servo motor
//
void servomotor_init(unsigned char index)
{
	// Configure PWM pin
	pin_setup_output(servo_pins[index]); 
 
	// Set timer control registers
	// Clear OUTA and OUTB on compare match
	// Fast PWM mode with ICR = TOP
	// Prescaler 8
	timer1_init_fast_pwm(
		TIMER1_PRESCALE_8,
		TIMER1_FAST_PWM_TOP_ICR,
		TIMER1_FAST_PWM_OUTPUT_CLEAR_ON_MATCH,
		TIMER1_FAST_PWM_OUTPUT_CLEAR_ON_MATCH,
		TIMER1_FAST_PWM_OUTPUT_DISABLE);
 
	// TOP period
	timer1_set_input_capture_value(PWM_PERIOD);	
}

//
// Position servo motors
// pos is from -100 to 100
//
void servomotor_position(unsigned char index, signed short position)
{	
	switch (index)
	{
		case 0:			
			timer1_set_compare_match_unitA_value(PWM_MIDDLE_POS + position * PWM_RATIO);
			break;

		case 1:
			timer1_set_compare_match_unitB_value(PWM_MIDDLE_POS + position * PWM_RATIO);
			break;
	}
}
