From fe40f11f46c4f7376c66e1fd42bfc3885d2c7f0d Mon Sep 17 00:00:00 2001 From: cha0s Date: Thu, 6 May 2010 16:06:52 +0800 Subject: [PATCH] Implement Module 0--Line control at speed S8 --- sources/app/app_cfg.h | 4 +- sources/app/board.c | 388 ++++++++++++++++++++++++++++++-------------------- sources/app/board.h | 18 ++- sources/app/main.c | 37 +++-- 4 files changed, 274 insertions(+), 173 deletions(-) diff --git a/sources/app/app_cfg.h b/sources/app/app_cfg.h index 89fdc00..0fa5045 100644 --- a/sources/app/app_cfg.h +++ b/sources/app/app_cfg.h @@ -69,8 +69,8 @@ //#define APP_TASK_START_STK_SIZE 512 //#define OS_PROBE_TASK_STK_SIZE 256 #define DATA_Cap_STK_SIZE 500 -#define MOTOR_Ctrl_STK_SIZE 500 -#define OSTimeTick_A_STK_SIZE 500 +#define MOTOR_Ctrl_STK_SIZE 256 +#define OSTimeTick_A_STK_SIZE 256 /* diff --git a/sources/app/board.c b/sources/app/board.c index ee2769c..a308917 100644 --- a/sources/app/board.c +++ b/sources/app/board.c @@ -12,8 +12,13 @@ bool KNOCK = 0; int DR = 0; int datap[LINE]; +int left[5]={3800,4000,4100,4200,4300}; +int right[5]={3650,3500,3400,3300,3100}; + + struct ctrl_data cdata={1,M,S0}; struct mod_data mdata={0,0,PIEX>>1,0,PIEX>>1,PIEX>>1}; +struct pid_data lpid={5,0,0}; /* Functions implement */ @@ -50,7 +55,7 @@ void dvrinit(void){ DDRT_DDRT1=0; DDRT_DDRT2=0; DDRT_DDRT3=1; - PTT_PTT3=0; // 1 enable; 0 disable + PTT_PTT3=1; // 1 enable; 0 disable /* Enable MOTOR control module */ PWM1_Enable(); @@ -58,21 +63,21 @@ void dvrinit(void){ WPLUS_Enable(); /* Initial MOTOR control module */ - WPLUS_SetRatio16(2600); + WPLUS_SetRatio16(3500); PWM2_SetRatio16(S6); PWM1_SetRatio16(0); } // Send image frame data. Debug func void transchar(void){ - byte i,j; - - data_proc(); + int i,j; for(i=0;i>1; int i=0,j=PIEX,t=0,f=0,m=0,top=LINE-1,lft=PIEX,rit=0,cnt=LINE>>1; + byte black=255; if(!ENVB) envb(); @@ -114,66 +120,89 @@ void data_proc(void){ brt=20+ENVB; + for(i=0;ibrt){ - if(j>=i) j=i; - if(t<=i) t=i; + if(j>=i) j=i; // Left + if(t<=i) t=i; // Right f++; } } i=t-j+1; - if(f<3) datap[LINE-1]=-1; + if(f<3) { + datap[LINE-1]=-1; + mdata.MOD=255; + } - else if(i==f){ // There's no black spot in - if(j<2) datap[LINE-1]=0; - if(t>PIEX-2) datap[LINE-1]=PIEX; - }else{ // Otherwise locating blackspot + else if(i==f){ + // Otherwise locating blackspot + if(t<30) { + datap[LINE-1]=0; + mdata.LFT=0; + mdata.RIT=0; + mdata.TOP=LINE-1; + mdata.MOD=1; // 'U' + } + if(j>20) { + datap[LINE-1]=PIEX; + mdata.LFT=PIEX; + mdata.RIT=PIEX; + mdata.TOP=LINE-1; + mdata.MOD=1; + } + + }else{ for(i=j;i=0; i--){ - j=datap[i+1]; - m=0; - f=i+1; - while(j==-1 && f=4) { - datap[i]=-1; - }else{ - - if(data[i][j]=0; i--){ + j=datap[i+1]; + m=0; + f=i+1; + while(j==-1 && f=4) { + datap[i]=-1; + }else{ - j=datap[i]; + if(data[i][j]rit) rit=j; - if(jrit) rit=j; + if(j(LINE>>2)) { + if(mdata.TOP==-1){ + mdata.MOD=1; + } else if(mdata.TOP>20) { mdata.MOD=1; // 'U' module. If uncontinus spot detected? if(mdata.STA==0) mdata.STA=1; if(mdata.STA==2) mdata.STA=2; } - else if(mdata.LFT>=PV2 || mdata.RIT<=PV2 || tempr<=3) { // Trace at only in one side OR offset<=3 + else if( tempr<10) { // Trace at only in one side OR offset<=3 mdata.MOD=0; // LINE module mdata.STA=0; } - else { + else if(tempr<35){ mdata.MOD=2; // 'S' module if(mdata.STA==0) mdata.STA=1; if(mdata.STA==2) mdata.STA=2; @@ -246,131 +277,174 @@ void ctrl_pad(void){ byte PV2=PIEX/2; int w=0; int m=0; - + INT32S foo=0; - // offset=mdata.CNT-(PIEX>>1); - offset = datap[mdata.TOP]; + offset = (datap[mdata.TOP]+datap[LINE-1])>>1; - // Calculating control paraments + // Calculating control paraments switch(mdata.MOD){ case 0: // Line control - // PORTB_PB5=0; - //offset=datap[LINE-1]; // Using NEARest spot as reference - if(offset>PV2-8 && offset0){ - //offset=datap[LINE-1]-datap[mdata.TOP]; // Using NEARest spot as reference - foo=(PV2-offset)*WLFT; - w=foo/PIEX+M; // Most to 50% - cdata.WPLUS=w; - cdata.MOTOR=S4; - }else if(offset>=PV2+8 && offset=PIEX-1){ - foo=(8+mdata.TOP)*WRIT ; - w=foo/16+M; - cdata.WPLUS=w; - cdata.MOTOR=S2; - } - - - + ctrl_0(); break; case 1: // 'U' - // PORTB_PB6=0; - if(offset>PV2){ // Right turning - foo=(mdata.TOP-(PIEX>>2))*WRIT; - w=(foo>>7)+(WRIT>>1); - cdata.WPLUS=w; - cdata.MOTOR=S2; - }else{ - foo=(mdata.TOP-(PIEX>>2))*WLFT; - w=(foo>>7)+(WLFT>>1); - - cdata.WPLUS=w; - cdata.MOTOR=S2; - } + ctrl_1(); break; case 2: // 'S' - // PORTB_PB7=0; - if(offset>PV2-8 && offset0){ - //offset=datap[LINE-1]-datap[mdata.TOP]; // Using NEARest spot as reference - foo=(PV2-offset)*WLFT; - w=foo/PIEX+M; // Most to 50% - cdata.WPLUS=w; - cdata.MOTOR=S4; - }else if(offset>=PV2+4 && offset=PIEX-1){ - foo=(8+mdata.TOP)*WRIT; - w=foo/16+M; - cdata.WPLUS=w; - cdata.MOTOR=S2; - } - - - + ctrl_2(); break; + default: + cdata.WPLUS=M; + cdata.MOTOR=S0; PORTB_PB5=0; PORTB_PB6=0; PORTB_PB7=0; } - if(cdata.WPLUS>M) PORTB_PB5=0; - if(cdata.WPLUS==M) PORTB_PB6=0; - if(cdata.WPLUS=M+WRIT){ + PWM2_SetRatio16(cdata.MOTOR); + WPLUS_SetRatio16(cdata.WPLUS); + }else{ + PWM2_SetRatio16(M); + WPLUS_SetRatio16(cdata.WPLUS); + } + }else{ + PWM2_SetRatio16(S0); + WPLUS_SetRatio16(M); + } - // Implement driver control - - if(cdata.LOR!=1){ - PWM2_SetRatio16(cdata.MOTOR); - WPLUS_SetRatio16(cdata.WPLUS); - }else if(cdata.WPLUS==M){ - WPLUS_SetRatio16(R1); // Reset WPLUS - PWM2_SetRatio16(cdata.MOTOR); - WPLUS_SetRatio16(cdata.WPLUS); - cdata.LOR=0; - - }else if(cdata.WPLUSM){ - PWM2_SetRatio16(cdata.MOTOR); - WPLUS_SetRatio16(cdata.WPLUS); - cdata.LOR=0; +} + +void ctrl_0 (void){ + int offset=0,r=0; + byte PV2=PIEX>>1; + INT32S foo=0; + int w=0; + + PORTB_PB5=0; + offset=datap[mdata.TOP]; // Using NEARest spot as reference + if(offset>PV2-2 && offset0){ + foo=WLFT; + foo=(PV2-offset)*foo; + foo=foo/(PIEX-5); + w=foo+M; // Most to 50% + r=feedback(cdata.LOR, w,lpid.kp); + cdata.WPLUS=r; + cdata.MOTOR=S8; + + cdata.LOR=r; + }else if(datap[mdata.TOP]>PV2 && offset>1; + INT32S foo=0; + offset=(mdata.RIT+mdata.LFT)>>1; + if(mdata.TOP==-1){ + if(mdata.LFT>PV2){ + cdata.WPLUS=right[4]; + cdata.MOTOR=S2; + }else{ + cdata.WPLUS=left[4]; + cdata.MOTOR=S2; + } + }else{ + if(datap[mdata.TOP]>PV2){ + + if(offset<40) cdata.WPLUS=right[0]; + else if(offset<45) cdata.WPLUS=right[0]; + else if(offset<48) cdata.WPLUS=right[2]; + else cdata.WPLUS=right[3]; + }else{ + if(offset>15) cdata.WPLUS=left[1]; + else if(offset>10) cdata.WPLUS=left[4]; + else if(offset>5) cdata.WPLUS=left[4]; + else cdata.WPLUS=left[4]; + } + } + +} + +void ctrl_2(void){ + int offset=0; + byte PV2=PIEX>>1; + INT32S foo=0; + int w=0; + PORTB_PB7=0; + if(offset>PV2-8 && offset0){ + foo=WLFT; + foo=(PV2-offset)*foo; + foo=foo/PIEX; + w=foo+M; // Most to 50% + cdata.WPLUS=w; + cdata.MOTOR=S4; + }else if(offset>=PV2+8 && offset=PIEX-1){ + foo=WRIT; + foo=(15+mdata.TOP)*foo ; + foo=foo/30; + // w=foo+M; + cdata.WPLUS=w; + cdata.MOTOR=S2; + } +} + +/* Feedback WPLUS control */ +int feedback(int last, int cur, struct pid_data pid){ + int kp=pid.kp; // Set full as 10 + INT32S e=0; + int re=0; + if(last<(M+WRIT) || last>(M+WLFT)) last=M; + if(cur<(M+WRIT) || cur>(M+WLFT)) return last; + e=(last-cur)*kp/10; + re=last-e; + + if(re<(M+WRIT) || re>(M+WLFT)) re=last; + return re; } + diff --git a/sources/app/board.h b/sources/app/board.h index d0773a8..98107bb 100644 --- a/sources/app/board.h +++ b/sources/app/board.h @@ -17,14 +17,14 @@ #define S4 38214 #define S6 39321 #define S8 52428 -#define S10 65535 +#define S10 65000 -#define M 4300 // Direction Switcher +#define M 3650 // Direction Switcher #define R1 2200 #define L1 3200 #define R2 1300 #define L2 4200 -#define WLFT 1000 +#define WLFT 700 #define WRIT -800 #define LED1 PORTB_PB4 @@ -75,11 +75,17 @@ struct mod_data{ byte STA; // Route stage. Basically 3 Stage will be implemented. 1: head into; 2: in middle; 3: head out int CNT; // Trace central offset in LINE - INT32U TOP; // Trace top boundary in LINE + int TOP; // Trace top boundary in LINE byte LFT; // Trace left boundary in PIEX byte RIT; // Trace right boundary in PIEX }; +struct pid_data{ + int kp; + int ki; + int kd; +}; + void delay(int ); void dvrinit(void); @@ -88,4 +94,8 @@ void transchar_n(void); void envb(void); void ctrl_pad(void); void data_proc(void); +void ctrl_0(void); +void ctrl_1(void); +void ctrl_2(void); +int feedback(int a, int b, int kp); #endif diff --git a/sources/app/main.c b/sources/app/main.c index 849dd21..069f27e 100644 --- a/sources/app/main.c +++ b/sources/app/main.c @@ -56,11 +56,9 @@ void DATA_Cap(void){ // PJINT_E(); /* Implement functional code in while(TRUE) */ - i=4915; + i=3500; while(TRUE){ - - CNTFLAG=1; LINECNT=0; IRQ_Enable(); @@ -70,6 +68,10 @@ void DATA_Cap(void){ IRQ_Disable(); // You may wanna clean it somtime PORTB=0XF0; + data_proc(); // Processing data before ahead to control pad + mode_g(); // Generating Module + + transchar(); if(!KEY2) { delay(1000); PORTB=~PORTB; @@ -81,16 +83,33 @@ void DATA_Cap(void){ transchar_n(); } - data_proc(); // Processing data before ahead to control pad - mode_g(); // Generating Module - KNOCK=1; - OSTimeDly(2); - + OSTimeDly(1); + } } +void DATA_proc(void){ + + INT32U ct=0; + INT8U i,j; + + PJINT_E(); + + __EI(); + + while(TRUE){ + KNOCK=0; + /* Add code here */ + + while(!KNOCK){ + ct=OSTime; + ctrl_pad(); + } + + } +} void MOTOR_Ctrl(void){ @@ -101,8 +120,6 @@ void MOTOR_Ctrl(void){ __EI(); - //PE_low_level_init(); //INIT board - while(TRUE){ KNOCK=0; /* Add code here */ -- 2.11.4.GIT