sources/app/board.c

   1 #include "includes.h"
   2
   3 /*  Define globle variables   */
   4 byte LINECNT=0;
   5 byte CNTFLAG=0;
   6 byte FRAMECNT=0;
   7 byte ENVB=0;
   8 byte LINEACH = 0;
   9
  10 bool KNOCK = 0;
  11
  12 int DR = 0;
  13 int datap[LINE];
  14
  15 int left[5]={3800,4000,4100,4200,4300};
  16 int right[5]={3650,3500,3400,3300,3100};
  17
  18
  19 struct ctrl_data cdata={1,M,S0};
  20 struct mod_data mdata={0,0,PIEX>>1,0,PIEX>>1,PIEX>>1};
  21 struct pid_data lpid={5,0,0};
  22
  23 /*  Functions implement     */
  24
  25
  26 void delay(int t){
  27
  28         int i,j;
  29         if (t<1)  t=1;
  30         for (i=0; i<t; i++)
  31                 for(j=0; j<3338; j++);
  32 }
  33
  34 // Handly PLL
  35 void SetBusCLK_64M(void)
  36 {
  37         CLKSEL=0X00;//disengage PLL to system
  38         PLLCTL_PLLON=1;//turn on PLL
  39         SYNR =0xc0 | 0x07;
  40         REFDV=0x80 | 0x01;
  41         POSTDIV=0x00; //pllclock=2*osc*(1+SYNR)/(1+REFDV)=128MHz;
  42         _asm(nop); //BUS CLOCK=64M
  43         _asm(nop);
  44         while(!(CRGFLG_LOCK==1)); //when pll is steady ,then use it;
  45         CLKSEL_PLLSEL =1; //engage PLL to system;
  46 }
  47
  48 /*  Initialze drivers   */
  49 void dvrinit(void){
  50
  51         SetBusCLK_64M();
  52         /* Enable modules  */
  53
  54         /* Enable 33886 chip MIDS*/
  55         DDRT_DDRT1=0;
  56         DDRT_DDRT2=0;
  57         DDRT_DDRT3=1;
  58         PTT_PTT3=1;             // 1 enable; 0 disable
  59
  60         /*  Enable MOTOR control module */
  61         PWM1_Enable();
  62         PWM2_Enable();
  63         WPLUS_Enable();
  64
  65         /*  Initial MOTOR control module  */
  66         WPLUS_SetRatio16(3500);
  67         PWM2_SetRatio16(S6);
  68         PWM1_SetRatio16(0);
  69 }
  70
  71 // Send image frame data. Debug func
  72 void transchar(void){
  73         int i,j;
  74
  75         for(i=0;i<LINE;i++){
  76                 //      for(j=0;j<PIEX;j++){
  77                 j=datap[i];
  78                 if(j==-1) j=0;
  79                 while(ERR_OK!=SPI_SendChar(j));
  80                 //      }
  81         }
  82 }
  83 // Send image frame data, Debug func
  84 void transchar_n(void){
  85         byte i,j;
  86
  87         for(i=0;i<LINE;i++){
  88                 for(j=0;j<PIEX;j++){
  89                         while(ERR_OK!=SPI_SendChar(data[i][j]));
  90                 }
  91         }
  92 }
  93
  94 void envb(void){
  95         unsigned int cntl=0;
  96         unsigned int avg=0;
  97         byte i,j;
  98
  99         for (i = 1; i <= LINE; i++){
 100                 for (j = 0; j<PIEX; j++){
 101                         cntl += data[i][j];
 102                 }
 103                 cntl = cntl/PIEX;
 104                 if (avg == 0) avg=cntl;
 105                 else avg = (avg+cntl)>>1;
 106         }
 107
 108         ENVB = avg;                     //Updata ENVB
 109 }
 110 void data_proc(void){
 111         byte brt;
 112         byte PV2=PIEX>>1;
 113         int i=0,j=PIEX,t=0,f=0,m=0,top=LINE-1,lft=PIEX,rit=0,cnt=LINE>>1;
 114         byte black=255;
 115
 116         if(!ENVB) envb();
 117
 118         brt=255>>2;
 119         brt-=255;
 120
 121         brt=20+ENVB;
 122
 123         for(i=0;i<PIEX;i++){
 124                 if(data[LINE-1][i]<black) black=data[LINE-1][i];
 125         }
 126
 127         for(i=0;i<PIEX;i++){                      // Searching white spots
 128                 if(data[LINE-1][i]>brt){
 129                         if(j>=i) j=i;             // Left
 130                         if(t<=i) t=i;             // Right
 131                         f++;
 132                 }
 133         }
 134
 135         i=t-j+1;
 136         if(f<3) {
 137                 datap[LINE-1]=-1;
 138                 mdata.MOD=255;
 139         }
 140
 141         else if(i==f){
 142                 // Otherwise locating blackspot
 143                 if(t<30) {
 144                         datap[LINE-1]=0;
 145                         mdata.LFT=0;
 146                         mdata.RIT=0;
 147                         mdata.TOP=LINE-1;
 148                         mdata.MOD=1;                    // 'U'
 149                 }
 150                 if(j>20) {
 151                         datap[LINE-1]=PIEX;
 152                         mdata.LFT=PIEX;
 153                         mdata.RIT=PIEX;
 154                         mdata.TOP=LINE-1;
 155                         mdata.MOD=1;
 156                 }
 157
 158         }else{
 159                 for(i=j;i<t;i++){
 160                         if(data[LINE-1][i]<black+10) {
 161                                 datap[LINE-1]=i+1;
 162                                 break;
 163                         }
 164                 }
 165
 166
 167                 // Loop searching next one
 168                 for(i=LINE-2; i>=0; i--){
 169                         j=datap[i+1];
 170                         m=0;
 171                         f=i+1;
 172                         while(j==-1 && f<LINE && m<4){
 173                                 j=datap[f];
 174                                 f++;
 175                                 m++;
 176                         }
 177                         if(m>=4) {
 178                                 datap[i]=-1;
 179                         }else{
 180
 181                                 if(data[i][j]<brt){
 182                                         datap[i]=j;
 183                                 }else if(data[i][j-1]<brt && j-1<PIEX) datap[i]=j-1;
 184                                 else if (data[i][j+1]<brt && j+1<PIEX) datap[i]=j+1;
 185                                 else if (data[i][j-2]<brt && j-2<PIEX) datap[i]=j-2;
 186                                 else if (data[i][j+2]<brt && j+2<PIEX) datap[i]=j+2;
 187                                 else datap[i]=-1;
 188
 189                                 j=datap[i];
 190
 191                                 if(j!=-1){
 192                                         if(j>rit) rit=j;
 193                                         if(j<lft) lft=j;
 194                                         if(j==PV2) cnt=j;
 195                                         top--;
 196                                 }
 197                         }
 198                 }
 199
 200                 // Updata mdata
 201                 mdata.TOP=top;
 202                 mdata.LFT=lft;
 203                 mdata.RIT=rit;
 204                 mdata.CNT=cnt;
 205         }
 206
 207         // Debugging code
 208         for(i=0;i<LINE;i++){
 209                 for(j=0;j<PIEX;j++){
 210                         data[i][j]=255;
 211                 }
 212                 if(datap[i]!=-1){
 213                         j=datap[i];
 214                         data[i][j]=0;
 215                 }
 216         }
 217
 218 }
 219
 220 /* Speed acq */
 221 void speed_acq(void){
 222         //      unsigned int temp;
 223         /* add code here */
 224 }
 225
 226 /* PID Algorithm Implement */
 227 void pid_ctrl(void){
 228
 229         //      int e;
 230         //      int ec;
 231
 232         /* Coding here */
 233 }
 234
 235 /* Module generating */
 236
 237 void mode_g(void){
 238         int temp=0,tempr=0;
 239         int i=0,j=0;
 240         byte    PV2=PIEX>>1;
 241
 242         tempr=mdata.RIT-mdata.LFT;
 243         /*
 244            i=LINE-1;
 245            while(datap[i]!=-1 && i>=0){
 246            temp+=datap[i];
 247            i--;
 248            }
 249            temp=temp/(LINE-i);
 250            mdata.CNT=temp;                                      // Updata CNT
 251            */
 252         // Reference spot set as middle one?
 253         if(mdata.TOP==-1){
 254                 mdata.MOD=1;
 255         } else if(mdata.TOP>20) {
 256                 mdata.MOD=1;                            // 'U' module. If uncontinus spot detected?
 257                 if(mdata.STA==0) mdata.STA=1;
 258                 if(mdata.STA==2) mdata.STA=2;
 259         }
 260         else if( tempr<10) {            // Trace at only in one side OR offset<=3
 261                 mdata.MOD=0;                            // LINE module
 262                 mdata.STA=0;
 263         }
 264         else if(tempr<35){
 265                 mdata.MOD=2;                            // 'S' module
 266                 if(mdata.STA==0) mdata.STA=1;
 267                 if(mdata.STA==2) mdata.STA=2;
 268         }
 269 }
 270
 271 /* Motor & WPLUS control pad. */
 272
 273 void ctrl_pad(void){
 274         int total=0;
 275         int i=0;
 276         int offset=0;
 277         byte PV2=PIEX/2;
 278         int w=0;
 279         int m=0;
 280
 281         INT32S foo=0;
 282
 283
 284         offset = (datap[mdata.TOP]+datap[LINE-1])>>1;
 285
 286         // Calculating control paraments
 287         switch(mdata.MOD){
 288                 case 0:                                         // Line control
 289                         ctrl_0();
 290                         break;
 291
 292                 case 1:                                         // 'U'
 293                         ctrl_1();
 294                         break;
 295                 case 2:                                         // 'S'
 296                         ctrl_2();
 297                         break;
 298
 299                 default:
 300                         cdata.WPLUS=M;
 301                         cdata.MOTOR=S0;
 302                         PORTB_PB5=0;
 303                         PORTB_PB6=0;
 304                         PORTB_PB7=0;
 305         }
 306
 307         // Implement driver control
 308         if(mdata.MOD==0){
 309
 310                 if(cdata.WPLUS<= M+WLFT && cdata.WPLUS>=M+WRIT){
 311                         PWM2_SetRatio16(cdata.MOTOR);
 312                         WPLUS_SetRatio16(cdata.WPLUS);
 313                 }else{
 314                         PWM2_SetRatio16(M);
 315                         WPLUS_SetRatio16(cdata.WPLUS);
 316                 }
 317         }else{
 318                 PWM2_SetRatio16(S0);
 319                 WPLUS_SetRatio16(M);
 320         }
 321
 322 }
 323
 324
 325 void ctrl_0 (void){
 326         int offset=0,r=0;
 327         byte PV2=PIEX>>1;
 328         INT32S foo=0;
 329         int w=0;
 330
 331         PORTB_PB5=0;
 332         offset=datap[mdata.TOP];                        // Using NEARest spot as reference
 333         if(offset>PV2-2 && offset<PV2+2){
 334                 r=M;
 335                 r=feedback(cdata.LOR, r, lpid.kp);
 336
 337                 cdata.WPLUS=r;
 338                 cdata.MOTOR=S8;
 339
 340                 cdata.LOR=r;
 341         }else if(datap[mdata.TOP]<=PV2 && offset>0){
 342                 foo=WLFT;
 343                 foo=(PV2-offset)*foo;
 344                 foo=foo/(PIEX-5);
 345                 w=foo+M;                        // Most to 50%
 346                 r=feedback(cdata.LOR, w,lpid.kp);
 347                 cdata.WPLUS=r;
 348                 cdata.MOTOR=S8;
 349
 350                 cdata.LOR=r;
 351         }else if(datap[mdata.TOP]>PV2 && offset<PIEX-1){
 352                 foo=WRIT;
 353                 foo=(offset-PV2)*foo ;
 354                 foo=foo/(PIEX-5);
 355                 w=foo+M;
 356                 r=feedback(cdata.LOR, w,lpid.kp);
 357                 cdata.WPLUS=r;
 358                 cdata.MOTOR=S8;
 359
 360                 cdata.LOR=r;
 361         }
 362 }
 363
 364 void ctrl_1(void){
 365         int offset=0;
 366         byte PV2=PIEX>>1;
 367         INT32S foo=0;
 368         offset=(mdata.RIT+mdata.LFT)>>1;
 369         if(mdata.TOP==-1){
 370                 if(mdata.LFT>PV2){
 371                         cdata.WPLUS=right[4];
 372                         cdata.MOTOR=S2;
 373                 }else{
 374                         cdata.WPLUS=left[4];
 375                         cdata.MOTOR=S2;
 376                 }
 377         }else{
 378                 if(datap[mdata.TOP]>PV2){
 379
 380                         if(offset<40) cdata.WPLUS=right[0];
 381                         else  if(offset<45) cdata.WPLUS=right[0];
 382                         else if(offset<48) cdata.WPLUS=right[2];
 383                         else cdata.WPLUS=right[3];
 384                 }else{
 385                         if(offset>15) cdata.WPLUS=left[1];
 386                         else  if(offset>10) cdata.WPLUS=left[4];
 387                         else if(offset>5) cdata.WPLUS=left[4];
 388                         else cdata.WPLUS=left[4];
 389                 }
 390         }
 391
 392 }
 393
 394 void ctrl_2(void){
 395         int offset=0;
 396         byte PV2=PIEX>>1;
 397         INT32S foo=0;
 398         int w=0;
 399         PORTB_PB7=0;
 400         if(offset>PV2-8 && offset<PV2+8){
 401                 cdata.WPLUS=M;
 402                 cdata.MOTOR=S4;
 403         }else if(offset<=PV2-8 && offset>0){
 404                 foo=WLFT;
 405                 foo=(PV2-offset)*foo;
 406                 foo=foo/PIEX;
 407                 w=foo+M;                        // Most to 50%
 408                 cdata.WPLUS=w;
 409                 cdata.MOTOR=S4;
 410         }else if(offset>=PV2+8 && offset<PIEX-1){
 411                 foo=WRIT;
 412                 foo=(offset-PV2)*foo ;
 413                 foo=foo/PIEX;
 414                 w=foo+M;
 415                 cdata.WPLUS=w;
 416                 cdata.MOTOR=S4;
 417         }else if(offset==0){
 418                 foo=WLFT;
 419                 foo=(15+mdata.TOP)*foo;
 420                 foo=foo/30;
 421                 w=foo+M;                        // Most to 70%
 422
 423                 cdata.WPLUS=w;
 424                 cdata.MOTOR=S2;
 425         }else if(offset>=PIEX-1){
 426                 foo=WRIT;
 427                 foo=(15+mdata.TOP)*foo ;
 428                 foo=foo/30;
 429                 //              w=foo+M;
 430                 cdata.WPLUS=w;
 431                 cdata.MOTOR=S2;
 432         }
 433 }
 434
 435 /* Feedback WPLUS control       */
 436 int feedback(int last, int cur, struct pid_data pid){
 437         int kp=pid.kp;                      // Set full as 10
 438         INT32S e=0;
 439         int re=0;
 440
 441         if(last<(M+WRIT) || last>(M+WLFT)) last=M;
 442         if(cur<(M+WRIT) || cur>(M+WLFT)) return last;
 443
 444         e=(last-cur)*kp/10;
 445         re=last-e;
 446
 447         if(re<(M+WRIT) || re>(M+WLFT)) re=last;
 448         return re;
 449 }
 450