#include "msp430f5438.h"
#include "f5438a_uart.h"
#include "speed.h"
#include "all_define.h"
#include "clock_f5438a.h"
#include "get_a.h"
#include "TimerB_PWM.h"
#include "math.h"
#include "lcd1602.h"
#include "Balance_Kalman.h"
int w_temp,temp1;
float w,angle1;
float gyro,acc;
double ut_temp;
int data0_old=0,data0_new, period0=32767,period0_n=0, count_ovf0=0;
int data1_old=0,data1_new, period1=32767,period1_n=0, count_ovf1=0;
int n0,n1;
unsigned int accx;
unsigned char dir0,dir1,f_refresh0=0,f_refresh1=0;
double ut,angle_ek,speed,speed_filter=0,position=0;
int Kp_angle=400;
int Kd_angle=0;
int Kp_speed=0;
double Ki_speed=0;
double Kd_speed=0;
char ctr_1602=0,ctr_pos=0;
unsigned int temp_pos,ut_temp2;
unsigned char f_forward=FALSE,f_backward=FALSE,f_left=FALSE,f_right=FALSE;
unsigned char f_menu=FALSE,f_exit=FALSE,f_ok=FALSE;
int left_sp,right_sp;
void TimerInit_PID( void );
void main(void)
{
WDTCTL = WDTPW + WDTHOLD;// Stop watchdog timer
TX2_init();
LCD_init();
InitPwm();
TimerInit_PID();
P10DIR = 0xff;
P10OUT = 0xff;
uart_init();
INIT_AD();
InitPo();
InitTimer_speed();
_EINT();
while(1)
{
LCD_write_string(0,0,"Wellcome to DLUT");
}
}
void TimerInit_PID()
{
TA0CCTL0 = CCIE; // CCR0 interrupt enabled
TA0CCR0 = 255;
TA0CTL = TASSEL_2 + MC_1 + TACLR; // ACLK, up to CCR0, clear TAR, 4分频
}
#pragma vector = TIMER0_A0_VECTOR
__interrupt void PID()
{
P10OUT = ~P10OUT;
//acc = get_ax();
//gyro = get_tyro_mv();
acc=acc-1650;
acc=acc/800;//*100 求出多少g
if(acc>1)
acc=1;
else if(acc= 0)//根据输出量正负来切换L298N的转向信号
{
TBCCR1 = 0;
TBCCR2 = left_sp;
}
else
{
left_sp = -left_sp;
TBCCR2 = 0;
TBCCR1 = left_sp;
}
if(right_sp >= 0)
{
TBCCR3 = 0;
TBCCR4 = right_sp;
}
else
{
right_sp = -right_sp;
TBCCR4 = 0;
TBCCR3 = right_sp;
}
}
#pragma vector=PORT1_VECTOR
__interrupt void Port_1(void)
{
_DINT();
data0_new = TA1R;
if((P1IN & BIT5)!=0x00) dir0 = NEG;
else dir0 = POS;
if(data0_new1)
period0_n+=(count_ovf0-1)*256;
}
else
{
period0_n=data0_new-data0_old;
if(count_ovf0>0)
period0_n+=count_ovf0*256;
}
//if(period1_n-period1-200)
period0=period0_n;
f_refresh0=1;
data0_old=data0_new;
count_ovf0=0;
P1IFG = 0x00; // P1.4 IFG cleared
_EINT();
}
#pragma vector=PORT2_VECTOR
__interrupt void Port_2(void)
{
_DINT();
data1_new = TA1R;
if((P2IN & BIT5)!=0x00) dir1 = NEG;
else dir1 = POS;
if(data1_new1)
period1_n+=(count_ovf1-1)*256;
}
else
{
period1_n=data1_new-data1_old;
if(count_ovf1>0)
period1_n+=count_ovf1*256;
}
//if(period1_n-period1-200)
period1=period1_n;
f_refresh1=1;
data1_old=data1_new;
count_ovf1=0;
P2IFG = 0x00; // P1.4 IFG cleared
_EINT();
}
#pragma vector=ADC12_VECTOR
__interrupt void ADC12ISR (void)
{
static unsigned int index = 0;
switch(__even_in_range(ADC12IV,34))
{
case 0: break; // Vector 0: No interrupt
case 2: break; // Vector 2: ADC overflow
case 4: break; // Vector 4: ADC timing overflow
case 6: break; // Vector 6: ADC12IFG0
case 8: break; // Vector 8: ADC12IFG1
case 10: break; // Vector 10: ADC12IFG2
case 12: // Vector 12: ADC12IFG3
A0results[0] = ADC12MEM0; // Move A0 results, IFG is cleared
//A1results[0] = ADC12MEM1; // Move A1 results, IFG is cleared
//A2results[0] = ADC12MEM2; // Move A2 results, IFG is cleared
A3results[0] = ADC12MEM3; //
//index++; // Increment results index, modulo; Set Breakpoint1 here
//if (index == 8)
//index = 0; // Reset index, Set breakpoint 2 here
acc = A0results[0]*3.3/4096*1000;
gyro = A3results[0]*3.3/4096*1000;
ADC12IFG = 0x00;
break;
case 14: break; // Vector 14: ADC12IFG4
case 16: break; // Vector 16: ADC12IFG5
case 18: break;
// Vector 18: ADC12IFG6
case 20: break; // Vector 20: ADC12IFG7
case 22: break; // Vector 22: ADC12IFG8
case 24: break; // Vector 24: ADC12IFG9
case 26: break; // Vector 26: ADC12IFG10
case 28: break; // Vector 28: ADC12IFG11
case 30: break; // Vector 30: ADC12IFG12
case 32: break; // Vector 32: ADC12IFG13
case 34: break; // Vector 34: ADC12IFG14
default: break;
}
ADC12IFG = 0x00;
}
#pragma vector = TIMER1_A0_VECTOR
__interrupt void TIMER1_A0_ISR(void)
{ // counter = 0;
_DINT();
count_ovf0++;
count_ovf1++;
_EINT();
}
ARM技术