两轮自平衡小车AVR程序

#include "DSP281x_Device.h" // Headerfile Include File

#include "DSP281x_Examples.h" //

#define HIST 16

unsigned int k=0;

static float adc_current_samples[6][HIST];//定义行为 6，列为 16 的二维数组

static float adclo=0.0;

static Uint16 totsamples_0;

unsigned int count;

static Uint16 still=0x00; //still=1.0, no move; still=0.0,move.

static Uint16 stand=0x00;

static Uint16 last_balance_s0;

static Uint16 samples[6],ticks;

static float left_steer_cof=0.05;

static float right_steer_cof=0.05;

static float steer_lim=0.13;

static float initial_angle=0.0000874;

static float hard_speed_lim=0.90;

static float bus_current;

static float batt_voltage1, ay, ax, in_steer_knob;

static float pitch_rate;

static float interval;

static float timer0_seconds_conv;

static float p_gain;

static float d_gain;

static float p1_gain;

static float d1_gain;

static float p0_gain;

static float d0_gain;

static float tp0,td0,tp1,td1,tp,td;

static float d_step=0.0001;

static float p_step;

static float cor_bat;

static float sample_conv1 = 1.0/1024.0/(float)HIST*2.07;

void InitAdc(void);

float angle;

float ay_bias;

//float ax_bias;

float steer_knob_bias;

float rate;

float curr_bias;

{if((!pitch_rate) && (!in_steer_knob)) mode = 0x00;

else mode = 0x01; }

void delay (void)

{ Uint16 xt;

static Uint16 k_temp;

for (xt=0;xt<15;xt++){

k_temp++; }

}

void delay1(void)

static Uint16 k_temp;

for (xt=0;xt<15000;xt++){

if (*lasts0 == totsamples_0) {

return 0;//if no new adc samples, return with 0

}

for (i=0; i<6; i++) {

for (j=0; j<HIST; j++)

{

tot+=adc_current_samples[i][j];

samples[i] = tot;//samples[i]=Σadc_current_samples[i][j](j=1,2,...,ADC_HIST)

}

}

else if(x>4.07) return 0x3;//24.4

else if(x>3.95) return 0x2;//23.7

else if(x>3.83) return 0x1;//23.0

else return 0x0;

}

void lpf_update(float *state, float tc, float interval, float input)

{

float frac=interval/tc;

if (frac>1.0) frac=1.0;

*state = input*frac + *state * (1.0-frac);

}

float fmax(float a, float b)

if (a>=b) {

} else {

if (a<=b) {

return a;

} else {

}

float flim(float x, float lo, float hi)

{

if (x<lo) return lo;

return x;

it->rate_bias=0.0;

it->steer_knob_bias=0.0;