motor_driv.c

电机闭环控制

#include "motor_driv.h"
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <inttypes.h>
#include <stdbool.h>

#include "bit_operation.h"


#define LEFT_DIR  6
#define RIGHT_DIR 7
//Keep x in range
#define kir(x,y)  do {\
				(x = x > ((1<<y)-1)? ((1<<y)-1) : x);\
				}while(0)

void
motor_driv_init (void)
{
  sbi (DDRB, 1);
  sbi (DDRB, 2);

  sbi (DDRD, 7);
  sbi (DDRD, 6);
  TCCR1A = 0xa3;
  TCCR1B = 0x09;
  OCR1A = 0x0000;
  OCR1B = 0x0000;
  TCNT1 = 0x0000;
}

void
set_current (int left, int right)
{
 	if (left < 0) {
		left = -left;
		sbi (PORTD, LEFT_DIR);
		//sbi( TCCR1A, COM1A0 );
	}else{
		clb (PORTD, LEFT_DIR);
		//clb( TCCR1A, COM1A0 );
	}

	if (right < 0) {
		right = -right;
		sbi (PORTD, RIGHT_DIR);
		//sbi( TCCR1A, COM1B0 );
	}else{
		clb (PORTD, RIGHT_DIR);
		//clb( TCCR1A, COM1B0 );
	}

	kir (left, 10);
	kir (right, 10);
	OCR1A = left;
	OCR1B = right;
}