motor_pi.c

电机闭环控制

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

#include <math.h>
#include <ctype.h>
  
#include "coder.h"
#include "motor_driv.h"
#include "motor_iic.h"
  
#include "bit_operation.h"
  
#define dkir(x,len)  do{\
  if (x > 0)\
{\
x = x > ((1 << len) - 1) ? ((1 << len) - 1) : x;\
}\
else \
{\
x = x < -((1 << len) - 1) ? -((1 << len) - 1) : x;\
}\
}while (0)
  
//This should init the timer 0 to
//
  void           
pi_init (void) 
{
  
    //Timer 0 init.
    TCNT0 = 0x00;
  
TCCR0 = 0x05;
  
    // Timer/Counter2 Clock source: System Clock
    // Timer/Counter2 Clock value: Stopped
    // Timer/Counter2 Mode: Normal
    // Timer/Counter2 Output: Disconnected
    ASSR = 0x00;
  
OCR2 = 0x00;
  
TCNT2 = 0x00;
  
TCCR2 = 0x00;
  

TIMSK = 0x01;
  
    //}}WIZARD_MAP(Timers)
} 

int        
sign (int t) 
{
  
return t > 0 ? 1 : -1;

}

ISR (SIG_OVERFLOW0) 
{
static int    left_current = 0; 
static int     right_current = 0;
static int     left_integrate = 0;
static int     right_integrate = 0;
static int     left_e;
static int     right_e;
        left_e = left_expected_v;
        right_e = right_expected_v;
        left_integrate += (left_e - left_v); 
        right_integrate += (right_e - right_v);
        dkir (left_integrate, 12);
        dkir (right_integrate, 12);
		
        left_current = left_integrate/4 + ((left_e - left_v) / 2);
        right_current = right_integrate/4 + ((right_e - right_v) / 2);
        set_current (left_current, right_current);
  right_v = 0;
  left_v = 0;
}