自平衡小车程序

自平衡小车程序
自平衡小车程序

#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;

#define OCRMAX 1023

unsigned int pwm;

static float left_cof=1.00;

static float right_cof=1.00;

struct gyro_filter pitch_filter;

static Uint16 mode=0x00;//0x00----balance mode; 0x01-------assist mode 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 cmd;

static float lpf_angle;

static float lpf_angrate;

static float lpf_steer_knob;

static float left_motor_pwm, right_motor_pwm, steer_cmd;

static float left_motor_pwm, right_motor_pwm, steer_cmd;

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 p_cmd,d_cmd;//next_cmd;

static float sample_conv = 1.0/1024.0/(float)HIST*5.0;//=3.052exp(-4) static float sample_conv1 = 1.0/1024.0/(float)HIST*2.07;

static float sample_conv2 = 1.0/1024.0/(float)HIST*1.9;

static float sample_conv3 = 1.0/1024.0/(float)HIST*333.3*2.5;

interrupt void T2_AD_isr(void);

void InitEv(void);

void InitAdc(void);

void InitGpio(void);

struct gyro_filter {

float angle;

float ay_bias;

//float ax_bias;

float rate_bias;

float steer_knob_bias;

float rate;

float steer_knob;

float curr_bias;

float curr;

Uint16 inited;

};

void check_mode ()

{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)

{ Uint16 xt;

static Uint16 k_temp;

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

k_temp++; }

}

int adc_collect_samples(Uint16 *samples, Uint16 *lasts0)

{

unsigned int i,j;

if (*lasts0 == totsamples_0) {

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

}

*lasts0 = totsamples_0;

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

Uint16 tot=0;

for (j=0; j

{

tot+=adc_current_samples[i][j];

}

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

return 1;

}

Uint16 bat_judge( float x)

{

if(x>4.30) return 0x5;//25.8

else if(x>4.18) return 0x4;//25.1

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) {

return a;

} else {

return b;

}

}

float fmin(float a, float b)

{

if (a<=b) {

return a;

} else {

return b;

}

}

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

{

if (x>hi) return hi;

if (x

return x;

}

void gyro_init(struct gyro_filter *it)

{ it->inited =0x00;

it->angle=0.0;

it->ay_bias=0.0;

// it->ax_bias=0.0;

it->rate_bias=0.0;

it->steer_knob_bias=0.0;

it->rate=0.0;

it->steer_knob=0.0;

it->curr_bias=0.0;

it->curr=0.0;

lpf_angle=0.0;

lpf_angrate=0.0;

lpf_steer_knob=0.0;

}

void gyro_sample(struct gyro_filter *it, float in_rate,

float in_y, float in_x, float in_steer,float curri,float interval)

{

flim( in_y, -1.0,1.0);

flim( in_x, -1.0,1.0);

if (!it->inited) {

it->angle=0.0;

it->rate_bias = in_rate;

it->ay_bias = in_y;

it->curr_bias = curri;

it->steer_knob_bias = in_steer;

it->inited=1;

return; }

it->rate = in_rate - it->rate_bias;

it->angle =in_y-it->ay_bias;//it->angle+(in_y-it->ay_bias)*last_ax-in_x*(last_ay-it->ay_bias); it->curr= curri-it->curr_bias;

it->steer_knob= flim( in_steer-it->steer_knob_bias,-5.0,5.0);

}

void set_motor_idle()

{

GpioDataRegs.GPFDA T.bit.GPIOF12=1; //SD high

GpioDataRegs.GPFDA T.bit.GPIOF13=1; //OE high

stand =0x00;

EvaRegs.T2CON.all =0x1440;

gyro_init(&pitch_filter);

return;

}

void set_motor(float level_left, float level_right)

{

int leveli_left,leveli_right;

if (fabs(level_left)<0.002 || fabs(level_right)<0.002) return;

leveli_left = (int16)(level_left*OCRMAX);

leveli_right = (int16)(level_right*OCRMAX);

if (leveli_left<-(OCRMAX-2)) leveli_left=-(OCRMAX-2);

if (leveli_left>OCRMAX-2) leveli_left=OCRMAX-2;

if (leveli_right<-(OCRMAX-2)) leveli_right=-(OCRMAX-2);

if (leveli_right>OCRMAX-2) leveli_right=OCRMAX-2;

if(still) {

GpioDataRegs.GPFDA T.bit.GPIOF12=0; // SD=0

// asm("nop");

while(GpioDataRegs.GPFDA T.bit.GPIOF12==1) continue;

GpioDataRegs.GPADA T.bit.GPIOA13=1; //PORTC |= 0x50; //enable CCW0

GpioDataRegs.GPBDA T.bit.GPIOB13=1;

// delay();

GpioDataRegs.GPADA T.bit.GPIOA14=1; //PORTC |= 0xa0; //disable CW0 GpioDataRegs.GPBDA T.bit.GPIOB14=1;

//asm("nop");

continue;

still=0x00;

}

if (leveli_left<0) {

EvaRegs.ACTRA.all=0x00C2;

pwm=-leveli_left;

}

else {

EvaRegs.ACTRA.all=0x002C;

pwm=leveli_left;

}

if (leveli_right<0) {

EvbRegs.ACTRB.all=0x002C;

pwm=-leveli_right;

}

else {

EvbRegs.ACTRB.all=0x00C2;

pwm=leveli_right;

}

}

void motor_ready(void)

{

gyro_init(&pitch_filter);

GpioDataRegs.GPADA T.bit.GPIOA13=1; //PORTC |= 0x50; //enable CCW0 GpioDataRegs.GPBDA T.bit.GPIOB13=1;

GpioDataRegs.GPADA T.bit.GPIOA14=1; //PORTC |= 0xa0; //disable CW0 GpioDataRegs.GPBDA T.bit.GPIOB14=1;

GpioDataRegs.GPFDA T.bit.GPIOF12=0; //SD low

GpioDataRegs.GPFDA T.bit.GPIOF13=0; //OE low

EvaRegs.T1CON.bit.TENABLE=1; //启动定时器T1

EvbRegs.T3CON.bit.TENABLE=1; //启动定时器T3

EvaRegs.T2CON.all =0x1440; // 16分频,使能定时器操作,连续增模式

}

void balance(void)

{ float steer_cmd_abs,temp_steer_cmd;

unsigned int T2cnt;

if (!adc_collect_samples(samples, &last_balance_s0))

return;

{

T2cnt=EvaRegs.T2CNT; //读取定时器2的值

ticks = OCRMAX * count + T2cnt;

interval = timer0_seconds_conv * (float)ticks;

}

count=0;

EvaRegs.T2CON.all = 0x1400; //关闭定时器2

EvaRegs.T2CNT = 0x0000;

EvaRegs.T2CON.all = 0x1440; //启动定时器2

EvaRegs.GPTCONA.bit.T1TOADC = 2;

ay = sample_conv1*samples[4]-sample_conv2*samples[0];//1.034

pitch_rate = samples[1]*sample_conv3;

in_steer_knob = (samples[2]+samples[3])*sample_conv;

bus_current = samples[4]*sample_conv;

batt_voltage1 = samples[5]*sample_conv;

check_mode();

if(!pitch_filter.inited)

{

if(batt_voltage1<3.0) {

set_motor_idle();

return;

}

else

cor_bat=-0.857*batt_voltage1+4.58;//-0.857=(1-1.3)/(4.18-3.83),4.58=1+0.857*4.18 cor_bat=flim(cor_bat,1.0,1.3);

}

gyro_sample(&pitch_filter, pitch_rate,ay,ax,in_steer_knob,bus_current, interval); EvaRegs.GPTCONA.bit.T1TOADC = 0;

if(!mode) {

if(fabs(pitch_filter.angle)

GpioDataRegs.GPADA T.bit.GPIOA14=0;

GpioDataRegs.GPBDA T.bit.GPIOB14=0;

GpioDataRegs.GPADA T.bit.GPIOA13=0;

GpioDataRegs.GPBDA T.bit.GPIOB13=0;

GpioDataRegs.GPFDA T.bit.GPIOF12=1;

still=0x01;

return; }

EvaRegs.GPTCONA.bit.T1TOADC = 2;

lpf_steer_knob = -pitch_filter.steer_knob;

if(stand) {

p_gain = fmax(p0_gain,p_gain-p_step);//0.1

d_gain = fmin(d0_gain,d_gain+d_step);

tp=tp0;

td=td0; }

else { p_gain = fmin(p1_gain,p_gain+p_step);

d_gain = fmax(d1_gain,d_gain-d_step);//0.1

tp=tp1;

td=td1;

}

lpf_update(&lpf_angle,tp, interval, -pitch_filter.angle*cor_bat);//0.6

lpf_update(&lpf_angrate,td, interval, -pitch_filter.rate*cor_bat);//0.5

p_cmd=p_gain*lpf_angle*fabs(lpf_angle);

d_cmd=d_gain*lpf_angrate;

cmd=p_cmd + d_cmd;

cmd=flim(cmd, -hard_speed_lim, hard_speed_lim);

if(lpf_steer_knob>0.0) steer_cmd = flim(left_steer_cof * lpf_steer_knob*(1-fabs(cmd)), -steer_lim,steer_lim);

else steer_cmd = flim(right_steer_cof * lpf_steer_knob*(1-fabs(cmd)),

-steer_lim,steer_lim);

steer_cmd_abs = fabs(steer_cmd);

if ((lpf_steer_knob>0.0 && cmd>0.01)||(lpf_steer_knob<0.0 && cmd<-0.01))

temp_steer_cmd = flim(steer_cmd+0.1*interval,-steer_cmd_abs,steer_cmd_abs); //?????????? else if ((lpf_steer_knob>0.0 && cmd<-0.01)||(lpf_steer_knob<0.0 && cmd>0.01))

temp_steer_cmd = flim(steer_cmd-0.1*interval,-steer_cmd_abs,steer_cmd_abs); //?????????? else temp_steer_cmd=0.0;

EvaRegs.GPTCONA.bit.T1TOADC = 0;

if (cmd>=0.0) {

left_motor_pwm = 1.0-cmd*left_cof - temp_steer_cmd;

right_motor_pwm =1.0-cmd*right_cof + temp_steer_cmd;

} else {

left_motor_pwm = -(1.0+cmd*left_cof) + temp_steer_cmd;

right_motor_pwm =-(1.0+cmd*right_cof)- temp_steer_cmd;

}

set_motor(left_motor_pwm, right_motor_pwm);

}

}

void stabilize_analog(void)

{

Uint16 samples[6];

Uint16 i;

Uint16 last_s0=0;

for (i=0; i

if (adc_collect_samples(samples, &last_s0)) i++;

}

}

void stand_test()

{

if(GpioDataRegs.GPBDA T.bit.GPIOB6&&GpioDataRegs.GPBDA T.bit.GPIOB7) {

stand=0x02;

return;

}

else{

if(GpioDataRegs.GPBDA T.bit.GPIOB6||GpioDataRegs.GPBDA T.bit.GPIOB7)

{

stand=0x01;

return;

}

else{

stand=0x00;

return;

}

}

}

void main(void)

{

InitSysCtrl();

EALLOW;

DINT;

IER = 0x0000; //禁止VPU中断并清除所有CPU中断标志位

IFR = 0x0000;

InitPieCtrl();

InitPieVectTable();

InitGpio();

InitAdc();

InitEv();

EALLOW; // This is needed to write to EALLOW protected registers

PieVectTable.T2PINT=&T2_AD_isr;

EDIS; // This is needed to disable write to EALLOW protected registers

PieCtrlRegs.PIEIER3.bit.INTx1=1;//T2pint中断

IER |= M_INT1;//开启中断1

PieCtrlRegs.PIEIER1.bit.INTx7=1;

EINT; // Enable Global interrupt INTM

ERTM; // Enable Global realtime interrupt DBGM

for(;;)

{

DisableDog();

GpioDataRegs.GPADA T.bit.GPIOA12=1;

GpioDataRegs.GPADA T.bit.GPIOA15=1;

GpioDataRegs.GPBDA T.bit.GPIOB12=1;

GpioDataRegs.GPBDA T.bit.GPIOB15=1;

GpioDataRegs.GPFDA T.bit.GPIOF12=0; //SD low GpioDataRegs.GPFDA T.bit.GPIOF13=0; //OE low

GpioDataRegs.GPADA T.bit.GPIOA13=0;

GpioDataRegs.GPADA T.bit.GPIOA14=0;

GpioDataRegs.GPBDA T.bit.GPIOB13=0;

GpioDataRegs.GPBDA T.bit.GPIOB14=0;

InitAdc();

stabilize_analog();

InitEv();

gyro_init(&pitch_filter);

motor_ready();

for(;;)

{

balance();

if(!stand) break;

}

set_motor_idle();

}

}

interrupt void T2_AD_isr(void)

{

if(k<16)

{

adc_current_samples[0][k]=((float)AdcRegs.ADCRESULT0)*3.0/65520.0+adclo;

adc_current_samples[1][k]=((float)AdcRegs.ADCRESULT1)*3.0/65520.0+adclo;

adc_current_samples[2][k]=((float)AdcRegs.ADCRESULT2)*3.0/65520.0+adclo;

adc_current_samples[3][k]=((float)AdcRegs.ADCRESULT3)*3.0/65520.0+adclo;

adc_current_samples[4][k]=((float)AdcRegs.ADCRESULT4)*3.0/65520.0+adclo;

adc_current_samples[5][k]=((float)AdcRegs.ADCRESULT5)*3.0/65520.0+adclo;

k++;

AdcRegs.ADCST.bit.INT_SEQ1_CLR=1; //清除状态字

AdcRegs.ADCTRL2.bit.RST_SEQ1=1;//复位seq1

EvaRegs.EV AIFRB.bit.T2PINT=1;//清除中断标志

EvaRegs.EV AIMRB.bit.T2PINT=1;//中断允许

PieCtrlRegs.PIEACK.bit.ACK3=1;//向cpu申请中断

}

else{

totsamples_0++;

k=0;

count=totsamples_0;

AdcRegs.ADCST.bit.INT_SEQ1_CLR=1; //清除状态字

AdcRegs.ADCTRL2.bit.RST_SEQ1=1;//复位seq1

EvaRegs.EV AIFRB.bit.T2PINT=1;//清除中断标志

// EvaRegs.EV AIMRB.bit.T2PINT=1;//中断允许

EvaRegs.GPTCONA.bit.T1TOADC = 0;

}

}

//--------------------------------------------------------------------------- // InitEv:

//--------------------------------------------------------------------------- // This function initializes to a known state.

//

void InitEv(void)

{

EALLOW;

/*****************************/

/***********配置ACTR**********/

/*****************************/

/* EvaRegs.ACTRA.bit.CMP1ACT=2;

//PW1引脚高电平有效

EvaRegs.ACTRA.bit.CMP2ACT=1;

//PWM2引脚低电平有效*/

/*****************************/

/*******配置死区寄存器********/

/*****************************/

EvaRegs.DBTCONA.bit.DBT=5;

//死区定时器周期为5

EvaRegs.DBTCONA.bit.EDBT1=1;

//死区定时器1使能

EvaRegs.DBTCONA.bit.DBTPS=1;

//死区定时器预定标因子,死区时钟为HSPCLK/2

/*****************************/

/*********配置COMCONA*********/

/*****************************/

https://www.360docs.net/doc/a417043013.html,CONA.bit.CENABLE=1;

//使能比较单元的比较操作

https://www.360docs.net/doc/a417043013.html,CONA.bit.CLD=2;

//当给CMPR1赋新值时,立即装载

https://www.360docs.net/doc/a417043013.html,CONA.bit.FCOMPOE=1;

//全比较操作,PWM输出由相应的比较逻辑驱动

/*****************************/

/**********设置T1CON**********/

/*****************************/

EvaRegs.T1CON.bit.TMODE=2;

//T1工作于连续增模式

EvaRegs.T1CON.bit.TPS=3;

//输入时钟预定标因子为X/8

EvaRegs.T1CON.bit.TENABLE=1;

//禁止定时器操作

EvaRegs.T1CON.bit.TCLKS10=0;

//使用内部时钟

EvaRegs.T1CON.bit.TECMPR=1;

//使能定时器比较操作

/*****************************/

/**********设置T1PR等*********/

/*****************************/

EvaRegs.T1PR =OCRMAX;

EvaRegs.T1CNT = 0x0; ///定时器1初值设为0

EvaRegs.CMPR1=pwm;

EvaRegs.CMPR2=pwm;

// EvaRegs.T1CON.bit.TENABLE=1; //启动定时器T1

/* EvbRegs.ACTRB.bit.CMP7ACT=2;

//PWM7引脚高电平有效

EvbRegs.ACTRB.bit.CMP8ACT=1;

//PWM8引脚低电平有效*/

/*****************************/

/*******配置死区寄存器********/

/*****************************/

EvbRegs.DBTCONB.bit.DBT=5;

//死区定时器周期为5

EvbRegs.DBTCONB.bit.EDBT1=1;

//死区定时器1使能

EvbRegs.DBTCONB.bit.DBTPS=3;

//死区定时器预定标因子,死区时钟为HSPCLK/8

/*****************************/

/*********配置COMCONA*********/

/*****************************/

https://www.360docs.net/doc/a417043013.html,CONB.bit.CENABLE=1;

//使能比较单元的比较操作

https://www.360docs.net/doc/a417043013.html,CONB.bit.CLD=2;

//当给CMPR1赋新值时,立即装载

https://www.360docs.net/doc/a417043013.html,CONB.bit.FCOMPOE=1;

//全比较操作,PWM输出由相应的比较逻辑驱动

/*****************************/

/**********设置T1CON**********/

/*****************************/

EvbRegs.T3CON.bit.TMODE=2;

//T1工作于连续增模式

EvbRegs.T3CON.bit.TPS=3;

//输入时钟预定标因子为X/8

EvbRegs.T3CON.bit.TENABLE=1;

//禁止定时器操作

EvbRegs.T3CON.bit.TCLKS10=0;

//使用内部时钟

EvbRegs.T3CON.bit.TECMPR=1;

//使能定时器比较操作

/*****************************/

/**********设置T1PR等*********/

/*****************************/

EvbRegs.T3PR =OCRMAX;

EvbRegs.T3CNT = 0; ///定时器3初值设为0

EvbRegs.CMPR4=pwm;

EvbRegs.CMPR5=pwm;

// EvbRegs.T3CON.bit.TENABLE=1;

//启动定时器T3

EvaRegs.GPTCONA.all=0;

EvaRegs.T2PR =OCRMAX; // 定时器2的周期为20k

EvaRegs.T2CMPR=0x0000;

EvaRegs.T2CNT =0x0000; // Timer2 counter

//EvaRegs.T2CON.all =0x1440;//16分频,使能定时器操作,连续增模式

EvaRegs.EV AIMRB.bit.T2PINT = 1;//定时器2周期中断允许

EvaRegs.EV AIFRB.bit.T2PINT = 1;//清除标志

// EvaRegs.GPTCONA.bit.T1TOADC = 2;

EDIS;

}

void InitAdc(void)

{

//unsigned int p;

// To powerup the ADC the ADCENCLK bit should be set first to enable

// clocks, followed by powering up the bandgap and reference circuitry.

// After a 5ms delay the rest of the ADC can be powered up. After ADC

// powerup, another 20us delay is required before performing the first

// ADC conversion. Please note that for the delay function below to

// operate correctly the CPU_CLOCK_SPEED define statement in the

// DSP28_Examples.h file must contain the correct CPU clock period in

// nanoseconds. For example:

AdcRegs.ADCTRL1.bit.RESET=1; //ADC模块软件复位

// asm(" NOP");

AdcRegs.ADCTRL1.bit.RESET=0; //这一句可要可不要,因为硬件会自动返回0

AdcRegs.ADCTRL1.bit.SUSMOD=3; //仿真挂起的模式3 ??

AdcRegs.ADCTRL1.bit.ACQ_PS=0x0; //决定启动转换新号SOC的脉冲宽度,SOC用于控制持续多长时间的采样开关闭合,SOC脉冲宽度大小为ADCLK周期的(ADCTRL(11:8)+1)倍

AdcRegs.ADCTRL1.bit.CPS=0; //对外设时钟HSPCLK进行分频,1分频

AdcRegs.ADCTRL1.bit.CONT_RUN=1;//连续运转模式

AdcRegs.ADCTRL1.bit.SEQ_CASC=1;//级联模式,SEQ1和SEQ2工作时作为一个16位状态排序器(SEQ)

//ADC模块控制寄存器3(ADCTRL3)AdcRegs.ADCTRL3.bit.ADCBGRFDN=3;//ADC带间隙和参考电路上电

// for(p=0;p<10000;p++) asm(" NOP"); //延时等待ADC上电

AdcRegs.ADCTRL3.bit.ADCPWDN=1; //ADC除间隙和参考电路之外的模拟电路上电// for(p=0;p<5000;p++) asm(" NOP"); //延时等待ADC上电

AdcRegs.ADCTRL3.bit.ADCCLKPS=5; //内核时钟分频,产生ADC内核时钟ADCLK;ADCLK=HSPCLK/[30*(ADCTRL1.7+1)];ADCTRL1.7为AdcRegs.ADCTRL1.bit.CPS=0。

AdcRegs.ADCTRL3.bit.SMODE_SEL=1;//同步采样模式

//最大转换通道寄存器(MAXCONV)AdcRegs.ADCMAXCONV.bit.MAX_CONV1=5;//即在SEQ采样模式下,使用MAXCONV1(3:0)

//ADC输入通道选择排序控制寄存器(CHSELSEQn)

AdcRegs.ADCCHSELSEQ1.bit.CONV00=0; //AD转换排序

AdcRegs.ADCCHSELSEQ1.bit.CONV01=1;

AdcRegs.ADCCHSELSEQ1.bit.CONV02=2;

AdcRegs.ADCCHSELSEQ1.bit.CONV03=3;

AdcRegs.ADCCHSELSEQ2.bit.CONV04=4;

AdcRegs.ADCCHSELSEQ2.bit.CONV05=5;

//AD状态和标志寄存器(ADCST)AdcRegs.ADCST.bit.INT_SEQ1_CLR=1; //ADCST中断清除位,清除SEQ1的中断标志

AdcRegs.ADCST.bit.INT_SEQ2_CLR=1; //ADCST中断清除位,清除SEQ2的中断标志

//ADC模块控制寄存器2(ADCTRL2)AdcRegs.ADCTRL2.bit.EVB_SOC_SEQ=0; //在级联排序模式下,EVB启动转换使能不起作用

AdcRegs.ADCTRL2.bit.RST_SEQ1=1; //复位排序器1

AdcRegs.ADCTRL2.bit.INT_ENA_SEQ1=1; //SEQ1中断使能,使能通过INT SEQ1向CPU发送中断请求

AdcRegs.ADCTRL2.bit.INT_MOD_SEQ1=0; //选择SEQ1的中断模式,它会影响SEQ1排序转换结束时INT SEQ1的设置,为0表示在每个SEQ1序列结束时,INT SEQ1置位AdcRegs.ADCTRL2.bit.EV A_SOC_SEQ1=1; //SEQ1 EV A启动转换,即EV A的触发信号启动SEQ1

AdcRegs.ADCTRL2.bit.EXT_SOC_SEQ1=0; //SEQ1外部信号启动转换位,0表示没有影响

AdcRegs.ADCTRL2.bit.RST_SEQ2=0; //表示没有可执行操作

AdcRegs.ADCTRL2.bit.SOC_SEQ2=0; //清除没有处理的SOC触发信号

AdcRegs.ADCTRL2.bit.INT_ENA_SEQ2=0; //INT SEQ2向CPU发送中断请求被禁止AdcRegs.ADCTRL2.bit.INT_MOD_SEQ2=0; //选择SEQ2的中断模式,它会影响SEQ2排序转换结束时INT SEQ2的设置,为0表示在每个SEQ2序列结束时,INT SEQ2置位AdcRegs.ADCTRL2.bit.EVB_SOC_SEQ2=0; //EVB的触发信号不能启动SEQ2

}

void InitGpio(void)

{

// Set GPIO A port pins,AL(Bits 7:0)(input)-AH(Bits 15:8) (output) 8bits

// Input Qualifier =0, none

EALLOW;

GpioMuxRegs.GPAMUX.all=0x000f;

GpioMuxRegs.GPADIR.all=0xf000;

GpioMuxRegs.GPAQUAL.all=0x0000; // Input qualifier disabled

// Set GPIO B port pins, configured as EVB signals

// Input Qualifier =0, none

// Set bits to 1 to configure peripherals signals on the pins

GpioMuxRegs.GPBMUX.all=0x003f;

GpioMuxRegs.GPBDIR.all=0xf000;

GpioMuxRegs.GPBQUAL.all=0x0000; // Input qualifier disabled

GpioMuxRegs.GPFMUX.bit.MDXA_GPIOF12=0; //确定GPIOF12为通用I/O引脚

GpioMuxRegs.GPFDIR.bit.GPIOF12=1; //方向为输出

GpioMuxRegs.GPFMUX.bit.MDRA_GPIOF13=0;

GpioMuxRegs.GPFDIR.bit.GPIOF13=1;

EDIS;

}

两轮自平衡小车的设计

常熟理工学院学报(自然科学)Journal of Changshu Institute Technology (Natural Sciences )第26卷第10Vol.26No.102012年10月Oct.,2012 收稿日期:2012-09-07 基金项目:江苏省大学生实践创新训练计划项目“两轮自平衡机器小车的设计”(jx110152011) 作者简介:李荣伟(1989—),男,江苏东海人,常熟理工学院电气与自动化工程学院测控技术与仪器专业2009级学生. 通讯作者:李鑫(1983—),男,安徽亳州人,实验师,硕士,研究方向:智能控制技术与现代检测技术,E-mail:lixin_yy@https://www.360docs.net/doc/a417043013.html,.两轮自平衡小车的设计 李荣伟,李鑫,孙传开,张冬林,江振峰 (常熟理工学院电气与自动化工程学院,江苏常熟215500) 摘要:设计了以陀螺仪ENC-03以及MEMS 加速度计MMA7260为传感器的姿态感知系统, 选用16位单片机MC9S12XS128为控制核心处理器,完成对传感器信号的采集处理、车身控制以及人机交互的设计,实现小车自主控制平衡状态、运行速度以及转向角度大小等功能.实验结果表明该系统的性能满足设计要求. 关键词:两轮自平衡;姿态检测;卡尔曼滤波;数据融合;PID 控制器 中图分类号:TP242.6文献标识码:A 文章编号:1008-2794(2012)10-0070-06 近年来,随着电子技术的发展与进步,移动机器人的研究已成为目前科学研究最活跃的领域之一,移动机器人经常会遇到在较为狭窄复杂的环境中如何灵活快捷地执行任务的问题.两轮自平衡机器人的概念就是在此背景下提出来的,这种机器人区别于其他移动机器人最显著的特点是:采用了两轮共轴、各自独立驱动的工作方式(又称差分式驱动),车身的重心位于车轮轴的上方,通过轮子的前后移动保持车身的平衡、行驶[1].因为具有体积小、运动灵活、零转弯半径等特点,所以在军用和民用领域有着广泛的应用前景,更重要的是系统具有多变量、非线性、强耦合、不稳定性的特性,使其成为很好地验证控制理论及方法的平台,具有很高的研究价值. 1系统整体设计 本文设计的自平衡车采用姿态传感器(加速度计和陀螺 仪)监测车身所处的俯仰状态和状态变化率,通过高速微控制 器(MC9S12XS128)完成数据融合处理,得到平滑而稳定车体 姿态信息,然后驱动电动机产生前进或后退的加速度来控制 车体保持平衡,同时系统还要根据速度的反馈量来完成对车 体速度和方向的控制,微控制器还需构建相关输入输出模块 和人机交互设备.系统设计总体结构框图如图1所示. 已知自平衡车高度为l ,质量为m ,将其抽象为一级倒立 摆,并将倒立摆置于可水平移动的小车上.假设其受外力干图1系统设计总体结构框图

两轮自平衡小车毕业设计毕业论文

两轮自平衡小车毕业设计毕业论文 目录 1.绪论 (1) 1.1研究背景与意义 (1) 1.2两轮自平衡车的关键技术 (2) 1.2.1系统设计 (2) 1.2.2数学建模 (2) 1.2.3姿态检测系统 (2) 1.2.4控制算法 (3) 1.3本文主要研究目标与内容 (3) 1.4论文章节安排 (3) 2.系统原理分析 (5) 2.1控制系统要求分析 (5) 2.2平衡控制原理分析 (5) 2.3自平衡小车数学模型 (6) 2.3.1两轮自平衡小车受力分析 (6) 2.3.2自平衡小车运动微分方程 (9) 2.4 PID控制器设计 (10) 2.4.1 PID控制器原理 (10) 2.4.2 PID控制器设计 (11) 2.5姿态检测系统 (12) 2.5.1陀螺仪 (12) 2.5.2加速度计 (13) 2.5.3基于卡尔曼滤波的数据融合 (14) 2.6本章小结 (16) 3.系统硬件电路设计 (17) 3.1 MC9SXS128单片机介绍 (17) 3.2单片机最小系统设计 (19) 3.3 电源管理模块设计 (21) I

3.4倾角传感器信号调理电路 (22) 3.4.1加速度计电路设计 (22) 3.4.2陀螺仪放大电路设计 (22) 3.5电机驱动电路设计 (23) 3.5.1驱动芯片介绍 (24) 3.5.2 驱动电路设计 (24) 3.6速度检测模块设计 (25) 3.6.1编码器介绍 (25) 3.6.2 编码器电路设计 (26) 3.7辅助调试电路 (27) 3.8本章小结 (27) 4.系统软件设计 (28) 4.1软件系统总体结构 (28) 4.2单片机初始化软件设计 (28) 4.2.1锁相环初始化 (28) 4.2.2模数转换模块(ATD)初始化 (29) 4.2.3串行通信模块(SCI)初始化设置 (30) 4.2.4测速模块初始化 (31) 4.2.5 PWM模块初始化 (32) 4.3姿态检测系统软件设计 (32) 4.3.1陀螺仪与加速度计输出值转换 (32) 4.3.2卡尔曼滤波器的软件实现 (34) 4.4平衡PID控制软件实现 (36) 4.5两轮自平衡车的运动控制 (37) 4.6本章小结 (39) 5. 系统调试 (40) 5.1系统调试工具 (40) 5.2系统硬件电路调试 (40) 5.3姿态检测系统调试 (41) 5.4控制系统PID参数整定 (43) II

两轮自平衡小车控制系统的设计

两轮自平衡小车控制系统的设计 摘要:介绍了两轮自平衡小车控制系统的设计与实现,系统以飞思卡尔公司的16位微控制器MC9S12XS128MAL作为核心控制单元,利用加速度传感器MMA7361测量重力加速度的分量,即小车的实时倾角,以及利用陀螺仪ENC-03MB测量小车的实时角速度,并利用光电编码器采集小车的前进速度,实现了小车的平衡和速度控制。在小车可以保持两轮自平衡前提下,采用摄像头CCD-TSL1401作为路径识别传感器,实时采集赛道信息,并通过左右轮差速控制转弯,使小车始终沿着赛道中线运行。实验表明,该控制系统能较好地控制小车平衡快速地跟随跑道运行,具有一定的实用性。 关键词:控制;自平衡;实时性 近年来,随着经济的不断发展和城市人口的日益增长,城市交通阻塞以及耗能、污染问题成为了一个困扰人们的心病。新型交通工具的诞生显得尤为重要,两轮自平衡小车应运而生,其以行走灵活、便利、节能等特点得到了很大的发展。但是,昂贵的成本还是令人望而止步,成为它暂时无法广泛推广的一个重要原因。因此,开展对两轮自平衡车的深入研究,不仅对改善平衡车的性价比有着重要意义,同时也对提高我国在该领域的科研水平、扩展机器人的应用背景等具有重要的理论及现实意义。全国大学生飞思卡尔智能车竞赛与时俱进,第七届电磁组小车首次采用了两轮小车,模拟两轮自平衡电动智能车的运行机理。在此基础上,第八届光电组小车再次采用两轮小车作为控制系统的载体。小车设计内容涵盖了控制、模式识别、传感技术、汽车电子、电气、计算机、机械及能源等多个学科的知识。 1 小车控制系统总体方案 小车以16位单片机MC9S12XS128MAL作为中央控制单元,用陀螺仪和加速度传感器分别检测小车的加速度和倾斜角度[1],以线性CCD采集小车行走时的赛道信息,最终通过三者的数据融合,作为直流电机的输入量,从而驱动直流电机的差速运转,实现小车的自动循轨功能。同时,为了更方便、及时地观察小车行走时数据的变化,并且对数据作出正确的处理,本系统调试时需要无线模块和上位机的配合。小车控制系统总体架构。 2 小车控制系统自平衡原理 两轮小车能够实现自平衡功能,并且在受到一定外力的干扰下,仍能保持直立状态,是小车可以沿着赛道自动循线行走的先决条件。为了更好地控制小车的行走方式,得到最优的行走路径,需要对小车分模块分析与控制。 本控制系统维持小车直立和运行的动力都来自小车的两个轮子,轮子转动由两个直流电机驱动。小车作为一个控制对象,它的控制输入量是两个电机的转动速度。小车运动控制可以分解成以下3个基本控制任务。 (1)小车平衡控制:通过控制两个电机正反方向运动保持小车直立平衡状态; (2)小车速度控制:通过调节小车的倾斜角度来实现小车速度控制,本质上是通过控制电机的转速来实现小车速度的控制。 (3)小车方向控制:通过控制两个电机之间的转动差速实现小车转向控制。 2.1 小车平衡控制 要想实现小车的平衡控制,需要采取负反馈控制方式[2]。当小车偏离平衡点时,通过控制电机驱动电机实现加、减速,从而抵消小车倾斜的趋势,便可以保持车体平衡。即当小车有向前倾的趋势时,可以使电机正向加速,给小车一个向前的加速度,在回复力和阻尼力的作用下,小车不至于向前倾倒;当小车有向后倾的趋势时,可以使小车反向加速,给小车一个向后的加速度,从而不会让小车向后倾倒,。

汽车理论图形MATLAB程序

功率平衡图 m=1230;g=9.8; ig=[3.615 2.053 1.393 1.031 0.837]; i0=3.75; r=0.31;yt=0.9;f=0.017;CD=0.31;A=2.2; np=6000;Pemax=83; %绘制汽车驱动力与行驶阻力平衡图 for i=1:56; n=500:100:6000; Pe(i)=Pemax*(n(i)/np+(n(i)/np)^2-(n(i)/np)^3); Tq(i)=9549*Pe(i)/n(i); end for j=1:5 for i=1:56 Ft(i,j)=Tq(i)*ig(j)*i0*yt/r; ua(i,j)=0.377*r*n(i)/(ig(j)*i0); Fz(i,j)=m*g*f+CD*A*(ua(i,j)^2)/21.15; end end figure plot(ua,Ft,ua,Fz); title('汽车驱动力与行驶阻力平衡图'); xlabel('ua(km/h)'); ylabel('Ft(N)'); text(20,6700,'Ft1'); text(40,4000,'Ft2'); text(50,2800,'Ft3'); text(80,2000,'Ft4'); text(100,1600, 'Ft5'); text(100,800,'Ff+Fw'); for k=1:56; n=500:100:6000; Pe(k)=Pemax*(n(k)/np+(n(k)/np)^2-(n(k)/np)^3); Tq(k)=9549*Pe(k)/n(k); Ft(k)=Tq(k)*ig(4)*i0*yt/r; ua(k)=0.377*r*n(k)/(ig(4)*i0); Fz(k)=m*g*f+CD*A*(ua(k)^2)/21.15; E(k)=abs((Ft(k)-Fz(k))); end [Emin,kmin]=min(E); Umax=ua(kmin)

单片机控制单轴双轮自动平衡小车设计开题报告

毕业设计(论文) 开题报告 题目:单片机控制单轴双轮自动平衡小车设计系别:电气工程系 专业:电气工程及其自动化 班级: 学号 学生姓名: 指导教师: 2016年 3月

中原工学院信息商务学院 毕业论文(设计)开题报告 论文(设计)题目单片机控制单轴双轮自动平衡小车 姓名系别电气工程系专业 班级 电气121学号6 1选题目的和意义: 平衡车是一个不稳定、强耦合、非线性系统,对平衡车的研究有利于我们更熟练得运用自动控制理论,并且发展更可靠稳定的控制方法。在实际应用中,平衡车由于体积小,灵活方便,不管是在军用或者民用领域都有广阔的应用空间,两轮自平衡小车可以作为一种小范围的移动式服务平台。通过本课题的研究学习,会使自己更加了解单片机,熟悉电子电路,提升自己的对整个设计的把握,更透彻的掌握自动控制方法。 2本选题在国内外的研究状况及发展趋势: 国外方面:JOE 是瑞士研制的用DSP和FPGA 控制并基于倒立摆理论双轮车。通过倾斜传感器和倾角传感器来检测车体。通过电机上的编码盘检测电机的速度。采用了基于状态反馈的线性控制策略,车的运动被分解成直线和旋转运动,然后分析直线运动和旋转运动,得到电机需要的控制量,最终把控制量耦合叠加。他主要的设计思想依然是:使车子朝车体倾斜的方向运动来保持车身的平衡。主控芯片是HC11 微处理器,此处理器是David P.Anderson 专门的针对nBot 车设计的。传感器在得到车的车身信息后,再比例整合,当作模糊控制器的输入,按照之前设定的控制原则得到两个电机需要的PWM 电压。该控制只能能让小车平衡运动,而不能让小车自主直立。Segway 拥有更多的姿态传感器,它有5个陀螺仪传感器,然而事实是检测车身前倾斜只需要3个传感器就够了,其他的两个传感器只是增加安全性。传感器的信息会被传送到一个电路板,这个电路板是微处理器的集群,效率是个人电脑的三倍。这个集群是为了保证本载人平衡车在其中任何一个处理器出现问题时能报告错误,给驾驶者以处理问题的时间余量,保证了平衡车的安全性。 国内方面:哈工大尹亮制作的双轮移动车Sway,车身倾斜度采用AD 推出的双轴加速度传感器ADXL202 及反射式红外线距离传感器来获得。基于PWM 动态控制直流电机的速度。车与上位机间的数据通信使用PTR2000 超小型超低功耗高速无线收发数传MODEM。人机交互界面使用图形液晶点阵、方向摇杆、按键。依靠这些可靠并且完备

基于单片机的两轮自平衡车控制系统设计

基于单片机的两轮自平衡车控制系统设计 摘要 两轮自平衡车是一种高度不稳定的两轮机器人,就像传统的倒立摆一样,本质不稳定是两轮小车的特性,必须施加有效的控制手段才能使其稳定。本文提出了一种两轮自平衡小车的设计方案,采用重力加速度陀螺仪传感器MPU-6050检测小车姿态,使用互补滤波完成陀螺仪数据与加速度计数据的数据融合。系统选用STC 公司的8位单片机STC12C5A60S2为主控制器,根据从传感器中获取的数据,经过PID算法处理后,输出控制信号至电机驱动芯片TB6612FNG,以控制小车的两个电机,来使小车保持平衡状态。 整个系统制作完成后,小车可以在无人干预的条件下实现自主平衡,并且在引入适量干扰的情况下小车能够自主调整并迅速恢复至稳定状态。通过蓝牙,还可以控制小车前进,后退,左右转。 关键词:两轮自平衡小车加速度计陀螺仪数据融合滤波 PID算法 Design of Control System of Two-Wheel Self-Balance Vehicle based on Microcontroller Abstract Two-wheel self-balance vehicle is a kind of highly unstable two-wheel robot. The characteristic of two-wheel vehicle is the nature of the instability as traditional inverted pendulum, and effective control must be exerted if we need to make it stable. This paper presents a design scheme of two-wheel self-balance vehicle. We need using gravity accelerometer

双轮自平衡小车机器人系统设计与制作

燕山大学 课程设计说明书题目:双轮自平衡小车机器人系统设计与制作 学院(系):机械工程学院 年级专业:12级机械电子工程 组号:3 学生: 指导教师:史艳国建涛艳文史小华庆玲 唐艳华富娟晓飞正操胡浩波 日期: 2015.11

燕山大学课程设计(论文)任务书院(系):机械工程学院基层教学单位:机械电子工程系

摘要 两轮自平衡小车是一种非线性、多变量、强耦合、参数不确定的复杂系统,他体积小、结构简单、运动灵活,适合在狭小空间工作,是检验各种控制方法的一个理想装置,受到广大研究人员的重视,成为具有挑战性的课题之一。 两轮自平衡小车系统是一种两轮左右并行布置的系统。像传统的倒立一样,其工作原理是依靠倾角传感器所检测的位姿和状态变化率结合控制算法来维持自身平衡。本设计通过对倒立摆进行动力学建模,类比得到小车平衡的条件。从加速度计和陀螺仪传感器得出的角度。运用卡尔曼滤波优化,补偿陀螺仪的漂移误差和加速度计的动态误差,得到更优的倾角近似值。通过光电编码器分别得到车子的线速度和转向角速度,对速度进行PI控制。根据PID控制调节参数,实现两轮直立行走。通过调节左右两轮的差速实现小车的转向。 制作完成后,小车实现了在无线蓝牙通讯下前进、后退、和左右转向的基本动作。此外小车能在正常条件下达到自主平衡状态。并且在适量干扰下,小车能够自主调整并迅速恢复稳定状态。 关键词:自平衡陀螺仪控制调试

前言 移动机器人是机器人学的一个重要分支,对于移动机器人的研究,包括轮式、腿式、履带式以及水下式机器人等,可以追溯到20世纪60年代。移动机器人得到快速发展有两方面原因:一是其应用围越来越广泛;二是相关领域如计算、传感、控制及执行等技术的快速发展。移动机器人尚有不少技术问题有待解决,因此近几年对移动机器人的研究相当活跃。 近年来,随着移动机器人研究不断深入、应用领域更加广泛,所面临的环境和任务也越来越复杂。机器人经常会遇到一些比较狭窄,而且有很多大转角的工作场合,如何在这样比较复杂的环境中灵活快捷的执行任务,成为人们颇为关心的一个问题。双轮自平衡机器人概念就是在这样的背景下提出来的。两轮自平衡小车是一个高度不稳定两轮机器人,是一种多变量、非线性、强耦合的系统,是检验各种控制方法的典型装置。同时由于它具有体积小、运动灵活、零转弯半径等特点,将会在军用和民用领域有着广泛的应用前景。因为它既有理论研究意义又有实用价值,所以两轮自平衡小车的研究在最近十年引起了大量机器人技术实验室的广泛关注。 本论文主要叙述了基于stm32控制的两轮自平衡小车的设计与实现的整个过程。主要容为两轮自平衡小车的平衡原理,直立控制,速度控制,转向控制及系统定位算法的设计。通过此设计使小车具备一定的自平衡能力、负载承载能力、速度调节能力和无线通讯功能。小车能够自动检测自身机械系统的倾角并完成姿态的调整,并在加载一定重量的重物时能够快速做出调整并保证自身系统的自我平衡。能够以不同运动速度实现双轮车系统的前进、后退、左转与右转等动作,同时也能够实现双轮自平衡车系统的无线远程控制操作

线控两轮平衡车的建模与控制研究

线控两轮平衡车的建模 与控制研究 文稿归稿存档编号:[KKUY-KKIO69-OTM243-OLUI129-G00I-FDQS58-

线性系统理论 上机实验报告 题目:两轮平衡小车的建模与控制研究 完成时间:2016-11-29 1.研究背景及意义 现代社会人们活动范围已经大大延伸,交通对于每个人都十分重要。交通工具的选择则是重中之重,是全社会关注的焦点。 随着社会经济的发展,人民生活水平的提高,越来越多的小汽车走进了寻常百姓家。汽车快捷方便、省时省力,现代化程度高,种类繁多的个性化设计满足了不同人的需求。但它体积大、重量大、污染大、噪声大、耗油大、技术复杂、使用不便、价格贵、停放困难,效率不高,而且还会造成交通拥堵并带来安全隐患。相比之下,自行车是一种既经济又实用的交通工具。中国是自行车大国,短距离出行人们常选择骑自行车。自行车确实方便,但在使用之前需要先学会骑车,虽然看似简单,平衡能力差的人学起来却很困难,容易摔倒,造成人身伤害。另外,自行车毕竟不适宜长距离的行驶,遥远的路程会使人感到疲劳。 那么,究竟有没有这样一种交通工具,集两者的优点于一身呢?既能像汽车一样方便快捷又如自行车般经济简洁,而且操作易于掌握,易学又易用。两轮自平衡车概念就是在这样的背景下提出来的。 借鉴目前国内外两轮自平衡车的成功经验,本文提出的研究目标是设计一款新型的、结构简单、成本低的两轮自平衡车,使其能够很好地实现自平衡功能,同时设计结果通过MATLAB进行仿真验证。

2.研究内容 自平衡式两轮电动车是一个非线性、强耦合、欠驱动的自不稳定系统,对其控制策略的研究具有重大的理论意义。我们通过分析两轮平衡车的物理结构以及在平衡瞬间的力学关系,得到两轮车的力学平衡方程,并建立其数学模型。运用MATLAB 和SIMULINK 仿真系统的角度θ、角加速度? θ、位移x 和速度的? x 变化过程,对其利用外部控制器来控制其平衡。 3.系统建模 两轮平衡车的瞬时力平衡分析如图1所示。下面将分析归纳此时的力平衡方程[1-3],并逐步建立其数学模型。 对两轮平衡车的右轮进行力学分析,如图2所示。 依据图2对右轮进行受力分析,并建立其平衡方程: =R R R R M X f H ? - (1) R R R R J C f R ??? =- (2) 同理,对左轮进行受力分析,并建立其平衡方程: =R L L L M X f H ? - (3) L L L L J C f R ??? =- (4) 两轮平衡车摆杆的受力分析如图3所示,由图3可以得到水平和垂直方向的平衡方程以及转矩方程。 水平方向的平衡方程: H H x R L p m +=? ? (5) 其中θsin L x x m p +=,则有:

汽车理论课后题matlab程序

汽车理论课后题matlab程序

————————————————————————————————作者:————————————————————————————————日期: ?

1.3 n=600:1:4000; r=0.367; i0=5.83; eff=0.85; f=0.013; m=3880;g=9.8; G=m*g; CdA=2.77;a=1.947; hg=0.9;L=3.2; Iw1=1.798; Iw2=3.598; Iw=Iw1+Iw2; If=0.218; Ttq=-19.313+295.27*n/1000-165.44*(n/1000).^2+40.874*(n/1000).^3-3.8445*(n/1000).^4; %驱动力行驶阻力平衡图 for ig=[5.56,2.769,1.644,1.00,0.793] Ua=0.377*r*n/ig/i0; Ft=Ttq*ig*i0*eff/r; plot(Ua,Ft); hold on; end Ff=G*f; ua=0:0.1:max(Ua); Fw=CdA*ua.^2/21.15; plot(ua,(Ff+Fw)); title('驱动力-行驶阻力平衡图'); xlabel('Ua/(km/h)');ylab el('Ft/N'); gtext('Ft1'),gtext('Ft2'),gtext('Ft3'),gtext('Ft4'),gtext('Ft5'),gtext('Ff+Fw') [x,y]=ginput(1); disp('汽车的最高车速');disp(x);disp('km/h'); %最大爬坡度及最大爬坡度时的附着率 Ua=0.377*r*n/5.56/i0; Ft=Ttq*5.56*i0*eff/r; Fw=CdA*Ua.^2/21.15; i=tan(asin((Ft-(Ff+Fw))/G)); disp('汽车的最大爬坡度');disp(max(i)); C=max(i)/(a/L+hg/L*max(i)); disp('克服最大爬坡度时的附着率');disp(C); %加速度倒数曲线 figure; for ig=[5.56,2.769,1.644,1.00,0.793] Ua=0.377*r*n/ig/i0; q=1+Iw/(m*r^2)+If*ig^2*i0^2*eff/(m*r^2); Ft=Ttq*ig*i0*eff/r; Fw=CdA*Ua.^2/21.15; as=(Ft-(Ff+Fw))/q/m; plot(Ua,1./as); hold on; end axis([0 98 0 10]); title('行驶加速度倒数曲线');xlabel('Ua/(km/h)');ylabel('1/a'); gtext('1/a1'),gtext('1/a2'),gtext('1/a3'),gtext('1/a4'),gtext('1/

两轮自平衡小车的设计

2015年陇东学院第十六届“挑战杯” 课外学术科技作品竞赛 双轮自平衡小车的设计与制作 学院:电气工程学院 班级:12级自动化本科班 姓名:周永 2015年12月8日

双轮自平衡小车的设计与制作 摘要:双轮自平衡小车是一个集动态决策和规划、环境感知、行为控制和执行等多种功能于一体的综合复杂系统,其关键是在解决自平衡的同时,还能够适应在各种环境下的控制任务。通过运用外加速度传感器、角速度传感器等,可以实现小车的平衡自主前进。双轮自平衡小车,涉及到传感器的驱动,数据的处理,角度的计算,电机的控制等,内容比较丰富,可作为实践自动控制原理及单片机技术的一个不错选择,是自我锻炼的绝好选题,对于以后制作此方面的民用产品也有很大的启迪作用。 关键词:双轮;自平衡;控制;传感器 1.引言 目前市场上的各种电子产品及家电机器人等行业越来越多地用到了智能控制技术。可以说,当今社会是一个智能型社会。各方各面都在竭尽全力向着智能方向发展,不论是人工智能还是联网智能,都在突出一个智能。智能已经覆盖了我们生活的方方面面,我们正在被智能的概念所潜移默化。不论是智能手机、玩具还是机器人,都已经成了我们生活的一部分。正是在这种情况下,智能交通的发展也发生了翻天覆地的变化,从飞车到自动驾驶汽车,无不在向我们说明,现代人已经对智能型交通工具期待已久了。作为最新科技产品的一个代表,最近市场上新出现的独轮车越来越受到了消费者的青睐。可以想象,最近几年内此类产品将会在市场上争得一席之地。比起独轮车,两轮车具有同样的购买热度,但是设计难度却没那么高,所以我将选择了从双轮车开始玩起智能交通工具。 2设计方案 方案一:用51单片机作为主控制器,用MPU6050模块采集姿态数据,用光电编码器对5V直流电机进行编码,显示模块采用LCD12864液晶屏,电源采用三端稳压方案,用红外遥控控制小车行走。本设计简单廉价,然而由于主控的反应相对缓慢,很难满足设计要求。 方案二:采用STM32单片机作主控制器,仍然用MPU6050模块作姿态数据采集,而电机采用二手的型号为16G214E MR19的具有高精度霍尔编码器的原价2000+的瑞士进口12V直流电机,显示模块采用了更轻薄更清晰更小巧的

自平衡小车设计报告

2012年省电子竞赛设计报告 项目名称:自平衡小车 姓名:连文金、林冰财、陈立镔 指导老师:吴进营、苏伟达、李汪彪、何志杰日期:2012年9月7日

摘要: 本组的智能小车底座采用的是网上淘宝的三轮两个电机驱动的底座,主控芯片为STC89C52,由黑白循迹采集模块对车道信息进行采集,将采集的信息传送到主控芯片,再由主控芯片发送相应的指令到电机驱动模块L298N,从而控制电机的运转模式。 关键词: STC89C52 L298N 色标传感器 E18-F10NK 自动循迹 引言: 近现代,随着电子科技的迅猛发展,人们对技术也提出了更高的要求。汽车的智能化在提高汽车的行驶安全性,操作性等方面都有巨大的优势,在一些特殊的场合下也能满足一些特殊的需要。智能小车系统涉及到自动控制,车辆工程,计算机等多个领域,是未来汽车智能化是一个不可避免的大趋势。本文设计的小车以STC89C52为控制核心,用色标传感器 E18-F10NK作为检测元件实现小车的自动循迹前行。 一、系统设计 本组智能小车的硬件主要有以STC89C52 作为核心的主控器部分、自动循迹部分、电机驱动部分。 1.1方案论证及选择: 根据设计要求,可以有多种方法来实现小车的功能。我们采用模块化思想,从各个单元电路选择入手进行整体方案的论证、比较与选择。 本方案以STC89C52作为主控芯片,通过按键进行模式的选择切换,按键一选择三轮循迹,按键二进行两轮循迹。 1.1.1模式一(三轮循迹): 模式一(按键一控制):三轮循迹的时候,通过色标传感器和激光传感器进行实时的数据采集,反馈给主控芯片,主控芯片通过驱动L298来控制两路直流减速电机,从而保证路线的准确性。

自平衡小车控制代码

#include #include "Wire.h" #include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" MPU6050 mpu(0x68); #define center 0x7F char flag=0; char num=0; double time; signed int speeds = 0; signed int oldspeed =0; byte inByte ; // MPU control/status vars bool dmpReady = false; uint8_t mpuIntStatus; uint8_t devStatus; uint16_t packetSize; uint16_t fifoCount; uint8_t fifoBuffer[64]; signed int speedcount=0; // orientation/motion vars Quaternion q; // [w, x, y, z] quaternion container VectorFloat gravity; // [x, y, z] gravity vector float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector float angle; double Setpoint, Input, Output; double kp = 18.8,ki = 185.0,kd = 0.29;//需要你修改的参数 double Setpoints, Inputs, Outputs; double sp = 0.8,si = 0,sd = 0.22;//需要你修改的参数 unsigned char dl=17,count; union{ signed int all; unsigned char s[2]; }data; volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high

两轮自平衡小车设计

两轮自平衡小车设计 一、任务要求 图1两轮自平衡车 两轮自平衡车结构原理如图1所示,主控制器(DSP)通过采集陀螺仪和加速度传感器得到位置信号,通过控制电机的正反转实现保持小车站立。 1、通过控制两个电机正反运动,实现小车在原地站立。 2、实现小车的前进、后退、转弯、原地旋转、停止等运动; 二、方案实现 2.1电机选型 图2直流电机 两轮自平衡车由于需要时刻保持平衡,对于倾角信号做出快速响应,因此对电机转矩要求较大。在此设计中选用国领电机生产的直流电机,其产品型号为GB37Y3530,工作电压6v-12v。为增大转矩,电机配有1:30传动比的减速器。

2.2电机测速方案 图3霍尔测速传感器 在电机测速方案上主流的方案有两种,分别是光电编码器和霍尔传感器。光电编码器测量精度由码盘刻度决定,刻度越多精度越高;霍尔传感器精度由永磁体磁极数目决定,同样是磁极对数越高精度越高。由于两轮自平衡车工作于剧烈震动环境中,光电编码器不适应这种环境,因此选用霍尔传感器来测量速度。电机尾部加装双通道霍尔效应编码器,AB双路输出,单路每圈脉冲16CPR,双路上下沿共输出64CPR,配合1:30的减速器传动比,可以计算出车轮转动一圈输出的脉冲数目为64X30=1920CPR,完全符合测速要求。 2.3电机驱动控制系统概述 本平台电机驱动采用全桥驱动芯片L298N,内部包含4通道逻辑驱动电路,两个H-Bridge的高电压、大电流双全桥式驱动器。本驱动桥能驱动46V、2A 以下的电机。其输出可以同时控制两个电机的正反转,非常适合两轮自平衡车开发,其原理图如下图所示 图4L298N原理图 采用脉宽调制方式(即PWM,Pulse Width Modulation)来调整电机的转速和转向。脉宽调制是通过改变发出的脉冲宽度来调节输入到电机的平均电

自平衡循迹小车设计报告

题图:自平衡小车系统 摘要:本自平衡小车由单片机芯片STC80C52为主控制器。通过电机驱动和寻迹电路完成三轮(两轮为驱动,一轮为万向轮)寻迹来按照竞赛要求来完成基本部分;在运用MMA7455数字加速度传感器和角速度传感器(ENC03陀螺仪)以及运用电磁线性偏差来完成两驱动轮的直立寻迹。 关键词 STC80C52、小车寻迹、自平衡小车。 Abstract:The self balancing car by single-chip microcomputer chip STC80C52 primarily controller. Through the motor drive and tracing circuit complete three (two wheel for drive, round for universal wheel) tracing to according to the competition requirements to complete basic parts; Using MMA7455 digital acceleration sensor and angular velocity sensor (ENC03 gyroscope) and the use of electromagnetic linear deviation to complete two driving wheel of upright tracing. Keywords STC80C52, car tracing, self balancing car

1系统方案 (3) 1.1模块方案比较与论证 (3) 1.2车体设计 (3) 1.3控制器模块 (3) 1.4寻迹模块 (4) 1.5直流电机驱动模块 (4) 1.6小车直立 (5) 1.7小车速度控制 (5) 1.8小车方向控制 (6) 1.9最终方案 (6) 2 理论分析和计算 (6) 2.1直流电机的转速如何控制?(建立数学模型) (6) 2.2电磁线性偏差检测数学模型建立 (8) 3电路设计1(两轮为驱动轮,一轮为万向轮) (9) 3.1电路总设计框图 (9) 3.2介绍单片机最小系统原理图及其功能 (9) 3.3介绍驱动模块原理图及其功能 (10) 3.4介绍寻迹模块原理图及其功能 (11) 3.5怎样来控制车模直立?(建立数学模型) (12) 3.6车模的方向控制 (14) 3.7车模倾角测量 (14) 4 电路设计2(两驱动轮直立行走) (17) 4.1整个电路的框架接结构 (17) 4.2介绍数字三轴加速度传感器模块与陀螺仪原理图及其功能 (18) 4.3介绍电磁线偏差检测系统电路及其原理 (20) 4.4 整个过程的注意事项 (21) 5 测试方案与结果分析 (22) 5.1寻迹测试方案(7个红外对管用TCR5000) (22) 5.2电机驱动测试方案(主芯片L298N) (22) 6.结论 (23) *参考文献 (23) *附录 (24) 附录1主要元器件芯片 (24) 附录2仪器设备清单 (24) 附录3主要程序清单 (24)

双轮自平衡车设计报告

双轮自平衡车设计报告 学院………….......... 班级…………………… 姓名………………..手机号…………………..姓名………………..手机号…………………..姓名………………..手机号…………………..

目录 一、双轮自平衡车原理 二、总体方案 三、电路和程序设计 四、算法分析及参数确定过程

一.双轮自平衡车原理 1.控制小车平衡的直观经验来自于人们日常生活经验。一般的人通过简单练习就可以让一个直木棒在手 指尖上保持直立。这需要两个条件:一个是托着木棒的手掌可以移动;另一个是眼睛可以观察到木棒的倾斜角度和倾斜趋势(角速度)。通过手掌移动抵消木棒的倾斜角度和趋势,从而保持木棒的直立。这两个条 件缺一不可,让木棒保持平衡的过程实际上就是控制中的负反馈控制。 图1 木棒控制原理图 2.小车的平衡和上面保持木棒平衡相比,要简单一些。因为小车是在一维上面保持平衡的,理想状态下,小车只需沿着轮胎方向前后移动保持平衡即可。 图2 平衡小车的三种状态 3.根据图2所示的平衡小车的三种状态,我们把小车偏离平衡位置的角度作为偏差;我们的目标是通过 负反馈控制,让这个偏差接近于零。用比较通俗的话描述就是:小车往前倾时车轮要往前运动,小车往后倾时车轮要往后运动,让小车保持平衡。 4.下面我们分析一下单摆模型,如图4所示。在重力作用下,单摆受到和角度成正比,运动方向相反的回复力。而且在空气中运动的单摆,由于受到空气的阻尼力,单摆最终会停止在垂直平衡位置。空气的阻尼力与单摆运动速度成正比,方向相反。 图4 单摆及其运动曲线

类比到我们的平衡小车,为了让小车能静止在平衡位置附近,我们不仅需要在电机上施加和倾角成正比的回复力,还需要增加和角速度成正比的阻尼力,阻尼力与运动方向相反。 5 平衡小车直立控制原理图 5.根据上面的分析,我们还可以总结得到一些调试的技巧:比例控制是引入了回复力;微分控制是引入了阻尼力,微分系数与转动惯量有关。 在小车质量一定的情况下,重心位置增高,因为需要的回复力减小,所以比例控制系数下降;转动惯量变大,所以微分控制系数增大。在小车重心位置一定的情况下,质量增大,因为需要的回复力增大,比例控制系数增大;转动惯量变大,所以微分控制系数增大。 二.总体方案 ■小车总框图

汽车理论汽车设计课程设计说明书

湖北汽车工业学院 Hubei Automotive Industries Institute 课程设计说明书 课程名称汽车理论 设计题目汽车动力性 班号专业车辆工程学号 学生姓名 指导教师(签字) 起止日期 2011 年 7 月 4 日—— 2011 年 7 月 9 日

目录 1.设计任务及要求.........................................1 2.车辆参数 (2) 3.汽车动力性能计算............. ..... ................... 3.1驱动力-行驶阻力平衡图...................... 3.2最高转速Uamax....................... 3.3加速时间t............................... 3.4汽车加速度倒数图............................... 3.5汽车加速时间图............................... 3.6汽车爬坡度图............................... 3.7汽车动力特性图................................ 3.8汽车功率平衡图.................................. 4.GUI界面设计........................................ 5.归纳与总结........................................ 6.参考文献......................................

(完整版)汽车理论习题Matlab程序

1.3 确定一轻型货车的动力性能(货车可装用4挡或5挡变速器,任选 其中的一种进行整车性能计算): 1)绘制汽车驱动力与行驶阻力平衡图。 2)求汽车最高车速,最大爬坡度及克服该坡度时相应的附着率。 3)绘制汽车行驶加速度倒数曲线,用图解积分法求汽车用2档起步加速行驶至70km/h 的车速-时间曲线,或者用计算机求汽车用2档起步加速行驶至70km/h 的加速时间。 轻型货车的有关数据: 汽油发动机使用外特性的Tq-n 曲线的拟合公式为 234 19.313295.27()165.44()40.874() 3.8445()1000100010001000 q n n n n T =-+-+- 式中,Tq 为发动机转矩(N?m );n 为发动机转速(r/min )。 发动机的最低转速n min =600r/min,最高转速n max =4000r/min 。 装载质量 2000kg 整车整备质量 1800kg 总质量 3880kg 车轮半径 0.367m 传动系机械效率 ηt =0.85 滚动阻力系数 f =0.013 空气阻力系数×迎风面积 C D A =2.77m 2 主减速器传动比 i 0=5.83 飞轮转动惯量 I f =0.218kg?m 2 二前轮转动惯量 I w1=1.798kg?m 2 四后轮转动惯量 I w2=3.598kg?m 2 质心至前轴距离(满载) a=1.974m 质心高(满载) hg=0.9m 解:Matlab 程序: (1) 求汽车驱动力与行驶阻力平衡图和汽车最高车速程序: n=[600:10:4000]; Tq=-19.313+295.27*(n/1000)-165.44*(n/1000).^2+40.874*(n/1000).^3-3.8445*(n/1000).^4; m=3880;g=9.8;nmin=600;nmax=4000; G=m*g; ig=[5.56 2.769 1.644 1.00 0.793];nT=0.85;r=0.367;f=0.013;CDA=2.77;i0=5.83; L=3.2;a=1.947;hg=0.9;If=0.218;Iw1=1.798;Iw2=3.598; Ft1=Tq*ig(1)*i0*nT/r; Ft2=Tq*ig(2)*i0*nT/r; Ft3=Tq*ig(3)*i0*nT/r;

两轮平衡小车说明书

电气电子工程学院自主创新作品 两轮平衡小车

摘要 两轮自平衡小车具有体积小、结构简单、运动灵活的特点,适用于狭小和危险的工作空间,在安防和军事上有广泛的应用前景。两轮自平衡小车是一种两轮左右平衡布置的,像传统倒立摆一样,本身是一种自然不稳定体,其动力学方程具有多变量、非线性、强耦合、时变、参数不确定性等特性,需要施加强有力的控制手段才能使其保持平衡。 本作品采用STM32单片机作为主控制器,用一个陀螺仪传感器来检测车的状态,通过dvr8800控制小车两个电机,来使小车保持平衡状态,通过2.4G模块无线通讯进行遥控来控制小车运行状态。 关键词:智能小车;单片机;陀螺仪。

目录 一.前言 (4) 一.两轮平衡车的平衡原理 (4) 2.1 平衡车的机械结构....................................................................... 错误!未定义书签。 2.2 两轮车倾倒原因的受力分析 (4) 2.3 平衡的方法 (5) 三.系统方案分析与选择论证 (5) 3.1 系统方案设计 (5) 3.1.1 主控芯片方案 (5) 3.1.2 姿态检测传感器方案 (6) 3.1.3 电机选择方案 (6) 3.2 系统最终方案 (7) 四.主要芯片介绍和系统模块硬件设计 (7) 4.1.STM32单片机简介(stm32rbt6) (7) 4.2.陀螺仪传感器 (8) 4.3.TB6612 (8) 4.4.编码器 (9) 4.5. 主控电路 (9) 4.6 电机驱动电路 (10) 五.系统软件设计 (11) 5.1 PID概述 (11) 5.2 数字PID算法 (12) 5.3 PID控制器设计 (13) 六.硬件电路 (14) 七.制作困难 (15) 八.结论 (16) 九.参考文献 (16)

相关文档
最新文档