navratil_ad/navratil_ad.c

   1 /*
   2 *
   3 * Author funkci na pohyb motoru:    Petr Kovacik <kovacp1@feld.cvut.cz>, (C) 2006
   4 *
   5 * Autor funkci na autonomni pohyb:  Milan Navratil <navram1@fel.cvut.cz> 2008
   6 *
   7 * Popis: main vola funkce autonomni_pohyb() , nebo sleduj_caru(),
   8 * obe funkce pak volaji funkce na vycteni dat z 10-bitoveho A/D prevodniku
   9 *
  10 */
  11 #define _USE_EXR_LEVELS 1
  12 #include <types.h>
  13 #include <cpu_def.h>
  14 #include <h8s2638h.h>
  15 #include <periph/sci_rs232.h>
  16 #include <system_def.h>
  17 #include <stdlib.h>
  18 #include <string.h>
  19
  20 #include <pxmc.h>
  21 #include <pxmcbsp.h>
  22
  23 #include <cmd_proc.h>
  24 #include "cmd_pxmc.h"
  25 #include "timer3.h"
  26 #include <stdio.h>
  27
  28 uint16_t read_GP1(void);                        // deklarace funkce read_GP1
  29 uint16_t read_GP2(void);                        // deklarace funkce read_GP2
  30 uint16_t read_QRD1(void);                       // deklarace funkce read_QRD1
  31 uint16_t read_QRD2(void);                       // deklarace funkce read_QRD2
  32 uint16_t read_QRD3(void);                       // deklarace funkce read_QRD3
  33
  34
  35
  36 cmd_des_t const *cmd_rs232_default[];
  37
  38 /*struktury prikazu cmd*/
  39 cmd_des_t const cmd_des_help={0, 0,"HELP","prints help for commands",
  40                         cmd_do_help,{(char*)&cmd_rs232_default}};
  41
  42 cmd_des_t const *cmd_rs232_default[]={
  43
  44   &cmd_des_help,
  45   CMD_DES_CONTINUE_AT(cmd_pxmc_default),
  46   NULL
  47 };
  48 cmd_des_t const **cmd_bth=cmd_rs232_default;       /*cmd prikazy pro bth*/
  49 cmd_des_t const **cmd_rs232=cmd_rs232_default;     /*cmd prikazy pro PC*/
  50
  51
  52 /*struktury charakterizujici motor 0*/
  53 pxmc_state_t mcsX0={
  54   pxms_flg:0,
  55   pxms_do_inp:0,
  56   pxms_do_con:0,
  57   pxms_do_out:0,
  58   pxms_do_deb:0,
  59   pxms_do_gen:0,
  60   pxms_ap:0, pxms_as:0,
  61   pxms_rp:155l*256,
  62   pxms_rs:0, //pxms_subdiv:8,
  63   pxms_md:8000l<<8, pxms_ms:5000, pxms_ma:5,            // pxms_ma je akcelerace robota byla nastavena na 10
  64   pxms_inp_info:(long)TPU_TCNT1,//TPU_TCNT1                     /*chanel TPU A,B*/
  65   pxms_out_info:(long)PWM_PWBFR1A,                      /*chanel PWM A,B*/
  66   pxms_ene:0, pxms_erc:0,
  67   pxms_p:40, pxms_i:0, pxms_d:1, pxms_s1:0, pxms_s2:0,
  68   pxms_me:0x1800, //6144
  69   pxms_ptirc:40, // 2000 irc per rev, 200/4 steps /
  70   pxms_ptper:1,
  71   pxms_ptptr1:NULL,
  72   pxms_ptptr2:NULL,
  73   pxms_cfg:PXMS_CFG_MD2E_m|PXMS_CFG_HLS_m|
  74       PXMS_CFG_HPS_m|PXMS_CFG_HDIR_m|0x1
  75 };
  76
  77
  78 /*struktury charakterizujici motor 1*/
  79 pxmc_state_t mcsX1={
  80   pxms_flg:0,
  81   pxms_do_inp:0,
  82   pxms_do_con:0,
  83   pxms_do_out:0,
  84   pxms_do_deb:0,
  85   pxms_do_gen:0,
  86   pxms_ap:0, pxms_as:0,
  87   pxms_rp:155l*256,
  88   pxms_rs:0, //pxms_subdiv:8,
  89   pxms_md:8000l<<8, pxms_ms:5000, pxms_ma:5,
  90   pxms_inp_info:(long)TPU_TCNT2,                        /*chanel TPU C,D*/
  91   pxms_out_info:(long)PWM_PWBFR1C,                      /*chanel PWM C,D*/
  92   pxms_ene:0, pxms_erc:0,
  93   pxms_p:40, pxms_i:0, pxms_d:1, pxms_s1:0, pxms_s2:0,
  94   pxms_me:0x1800, //6144
  95   pxms_ptirc:40, // 2000 irc per rev, 200/4 steps /
  96   pxms_ptper:1,
  97   pxms_ptptr1:NULL,
  98   pxms_ptptr2:NULL,
  99   pxms_cfg:PXMS_CFG_MD2E_m|PXMS_CFG_HLS_m|      //FIXME: nastavit spravne priznaky pro dalsi motorove struktur
 100       PXMS_CFG_HPS_m|PXMS_CFG_HDIR_m|0x1
 101 };
 102
 103 pxmc_state_t *pxmc_main_arr[] = {&mcsX0,&mcsX1};
 104
 105 pxmc_state_list_t pxmc_main_list = {
 106   pxml_arr:pxmc_main_arr,
 107   pxml_cnt:sizeof(pxmc_main_arr)/sizeof(pxmc_main_arr[0])
 108 };
 109
 110
 111
 112 //*******************************************************
 113
 114 void  unhandled_exception(void) __attribute__ ((interrupt_handler));
 115
 116 /*Interrupt routines*/
 117 void  unhandled_exception(void)
 118 {
 119 };
 120 //********************************************************
 121
 122 extern cmd_io_t cmd_io_rs232_line;
 123
 124 extern void _print(char *str);
 125
 126 void
 127 init(void)
 128 {
 129   /********************************************************************************/
 130   *DIO_PJDDR=0xff;      /*output gate*/
 131   *DIO_PEDDR=0xff;      /*output gate*/
 132   *DIO_PEDR=0x6f;       /*0x0-LED - light all; 0x6 -ENA,ENB=1, LE33CD=0*/
 133   *DIO_PJDR=0x00;       //zhasnuti vsech diod na */
 134
 135
 136   /*priority preruseni - SCI > TPU*/
 137 /*   *SYS_SYSCR|=SYSCR_INTM1m; */
 138 /*   *INT_IPRA=0x22; *INT_IPRB=0x22; *INT_IPRC=0x04;  */
 139 /*   *INT_IPRD=0x40; *INT_IPRE=0x44; *INT_IPRF=0x55; */
 140 /*   *INT_IPRG=0x55; *INT_IPRH=0x55; *INT_IPRJ=0x06; */
 141 /*   *INT_IPRK=0x67; *INT_IPRM=0x66; */
 142
 143   /*povoleni vsech preruseni atd...*/
 144   cli();
 145   excptvec_initfill(unhandled_exception, 0);
 146
 147   /*nastaveni seriovych linek - Bth, PC*/
 148   //sci_rs232_setmode(RS232_BAUD_RAW | 3, 0, 0, 2);     // HCI - hardcoded 115200
 149   //sci_rs232_setmode(115200, 0, 0, 2); // HCI
 150   sci_rs232_setmode(19200, 0, 0, sci_rs232_chan_default); //PC
 151   sti();
 152
 153   _print("Start\n");
 154
 155   pxmc_initialize();
 156
 157   if (init_timer3((long long)100/*ms*/*1000*1000) < 0) {
 158     /* ERROR */
 159     DEB_LED_ON(0);
 160   }
 161 }
 162
 163
 164 // Distance of wheels - d
 165 #define WHEEL_DIST 74 /* mm */
 166 #define MAX_R 300 /* mm */
 167 #define MIN_R (WHEEL_DIST/2) /* mm */
 168
 169 void move(int speed, int r)
 170 {
 171   int sl, sr;
 172   int d = WHEEL_DIST;
 173
 174
 175   if (r != 0) {
 176     sr = speed + (long long)speed * (d/2) / r;
 177     sl = speed - (long long)speed * (d/2) / r;
 178   } else {
 179     sr = speed;
 180     sl = speed + 10;            // minus 100 je kompenzace zataceni
 181   }
 182   pxmc_spd(&mcsX0, +sl, 0);
 183   pxmc_spd(&mcsX1, -sr, 0);
 184   //printf("speed=%5d, r=%5d, sl=%5d, sr=%5d\n", speed, r, sl, sr);
 185 }
 186
 187 #define DIST_TRESHOLD 280
 188
 189 void autonomni_pohyb(void)
 190 {
 191   long int now = get_timer();
 192   long int next = now;
 193   long int straight_timout = 0;
 194   int speed = 3000;
 195   int radius = 70;
 196   enum { TURN, STRAIGHT } stav = TURN;                          // podle priznaku stavu se rozhoduje,zda pojede rovne, nebo zatoci
 197   uint16_t voltage1, voltage2;
 198
 199   while (1) {
 200
 201     long int now = get_timer();
 202
 203     voltage1 = read_GP2();
 204     voltage2 = read_GP1();
 205
 206     if (voltage1 >= DIST_TRESHOLD) DEB_LED_ON(1);
 207     else DEB_LED_OFF(1);
 208
 209     if (voltage2 >= DIST_TRESHOLD) DEB_LED_ON(2);
 210     else DEB_LED_OFF(2);
 211
 212     if (stav == TURN && now >= next){                           // podminka pro jizdu
 213       stav = STRAIGHT;
 214       straight_timout = 10/*sec*/*10 + now;
 215       move(speed,0);
 216     }
 217
 218     if (stav == STRAIGHT) {
 219       if (now >= straight_timout) {
 220         //Go backward
 221         speed = -speed;
 222         move(speed,0);                                                  //otoc normalne je pouzit o if cyklus drive, ted si ho jen pujcuji
 223       }
 224       if ((voltage1 >= DIST_TRESHOLD && speed > 0) ||
 225           (voltage2 >= DIST_TRESHOLD && speed < 0)) {           // podminka pro zatoceni 260 odpovida (mensi nez 20cm)
 226         //Obstacle detected
 227         if (rand() % 100 < 50)                                  // 50 % predpoklad pro zatoceni doprava
 228           move(speed,radius);                                   // zatoc rychlosti speed a polomerem radius(doprava)
 229         else
 230           move(speed,-radius);                                  // zatoc rychlosti speed a polomerem -radius (doleva)
 231         stav = TURN;
 232
 233         next = now + ((rand() % 5)+8);                          // zataceni po dobu 5 - 12 (nahodne cislo)
 234       }                                                         // konec if
 235     }
 236   }                                                             //konec while
 237 }                                                               //konec funkce
 238
 239
 240 void sleduj_caru(void){
 241   long int now = get_timer();
 242   long int next = now;
 243   long int time_next = now;
 244   int speed = 2000;
 245   int radius = 100;
 246   uint16_t leva, stred, prava, cas=0;
 247
 248   move(2000,0);
 249   while(1){
 250
 251     long int now = get_timer();
 252
 253     stred = read_QRD3();                                        // pohled zpredu, cteni senzoru odrazu QRD1114 1-3
 254     leva = read_QRD1();
 255     prava = read_QRD2();
 256
 257     if (stred > 1000 && leva < 500 && prava <500 && now >= next){ // rozhodovani
 258       move(-speed,0);
 259     }
 260
 261     if (stred < 1000 && leva > 500){
 262       move(-speed,radius);
 263       next = now + 1;
 264     }
 265
 266     if (stred < 1000 && prava > 500){
 267       move(-speed,-radius);
 268       next = now + 1;
 269     }
 270
 271     if (now >= time_next){                                      // podminka vypisu, jednou za vterinu
 272       time_next = now + 10;
 273       cas++;
 274       /*Vypsani vzdalenosti na terminal*/
 275       //printf("V case %ds\tLeva: %d\tStredni: %d\tPrava: %d\n", cas, leva, stred, prava);      // vypisuje hodnoty povrchu po 1s
 276     }                                                           // konec if podminklz na vypis za 1s
 277   }                                                             // konec hlavni smycky
 278 }                                                               // konec funkce sleduj_caru()
 279
 280
 281 void jezdi_ctverec(void)                                        // funkce jezdeni, zmena podle casovace
 282 {
 283   long int now = get_timer();
 284   long int next = now;
 285   int speed = 3000;
 286   int radius = 100;
 287   int stav = 1;                                                 // podle priznaku stavu se rozhoduje,zda pojede rovne, nebo zatoci
 288   while (1) {
 289
 290     long int now = get_timer();
 291
 292       if (stav == 1 && now >= next){                            // podminka pro jizdu rovne po dobu 5s
 293       stav = 0;
 294       move(-speed,0);
 295       next = now + 50;
 296     }
 297
 298     if (stav == 0 && now >= next){                              // podminka pro zatoceni
 299       stav = 1;
 300       if (rand() % 100 < 50)
 301       move(-speed,radius);
 302       else
 303       move(-speed,-radius);
 304       next = now + 10;
 305     }
 306
 307   }
 308 }
 309
 310 uint16_t read_QRD1(void)                                        // funkce na vycteni informaci z AD prevodniku z ADDRC kde je nastaven AN6 tedy QRD114-1-leva zpredu
 311 {
 312   *SYS_MSTPCRA = *SYS_MSTPCRA & ~MSTPCRA_MSTPA1m;               // Zapnuti AD prevodniku
 313
 314   *AD_ADCSR = ADCSR_CH2m | ADCSR_CH1m;                          // nastaveni analog vstupu AN8 - tedy vystup ze senzoru QRD114-3-prostredni. CH3=0,CH2=1,CH1=1,CH0=0
 315
 316   uint8_t AN6h, AN6d;
 317   uint16_t AN6;                                                 // AN6h je horni byte AD prevodu, AN6d je dolni byte. Jsou cteny z ADDRC (viz tabulka 4-2 [navratil], AN6 je cele 10-bitove cislo)
 318   int i;
 319
 320   for(i = 0; i < 5; i++){                                       // for cyklus na 5 krat probehnuti smicky
 321     *AD_ADCSR = *AD_ADCSR & ~ADCSR_ADFm;                        // softwarove nastaveni ADF do 0
 322     *AD_ADCSR = *AD_ADCSR | ADCSR_ADSTm;                        // spusteni AD prevodu
 323
 324     while((*AD_ADCSR & ADCSR_ADFm) == 0) {                      // cekaci smycka dokud neskonci AD prevod
 325       ;
 326     }
 327
 328     /*Precteni vysledku AD prevodu z ADDRC*/
 329     AN6h = *AD_ADDRCH;                                          // Cteni horniho bytu
 330     AN6d = *AD_ADDRCL;                                          // Cteni dolniho bytu
 331     AN6 = (AN6d >> 6) | (AN6h << 2);                            // Posun jednotlivych bitu, abychom dostali 10-bitove cislo
 332
 333    }                                                            // konec for
 334    return (AN6);                                                // return pro main_loop, muzu vypnout while(1) a }pro main
 335 }                                                               // konec funkce
 336
 337
 338 uint16_t read_QRD2(void)                                        // funkce na vycteni informaci z AD prevodniku z ADDRD kde je nastaven AN7 tedy QRD114-2-prava zpredu
 339 {
 340   *SYS_MSTPCRA = *SYS_MSTPCRA & ~MSTPCRA_MSTPA1m;               // Zapnuti AD prevodniku
 341
 342   *AD_ADCSR = ADCSR_CH2m | ADCSR_CH1m | ADCSR_CH0m;             // nastaveni analog vstupu AN8 - tedy vystup ze senzoru QRD114-3-prostredni. CH3=0,CH2=1,CH1=1,CH0=1
 343
 344   uint8_t AN7h, AN7d;
 345   uint16_t AN7;                                                 // AN7h je horni byte AD prevodu, AN7d je dolni byte. Jsou cteny z ADDRD (viz tabulka 4-2 [navratil], AN7 je cele 10-bitove cislo)
 346   int i;
 347
 348   for(i = 0; i < 5; i++){                                       // for cyklus na 5 krat probehnuti smicky
 349     *AD_ADCSR = *AD_ADCSR & ~ADCSR_ADFm;                        // softwarove nastaveni ADF do 0
 350     *AD_ADCSR = *AD_ADCSR | ADCSR_ADSTm;                        // spusteni AD prevodu
 351
 352     while((*AD_ADCSR & ADCSR_ADFm) == 0) {                      // cekaci smycka dokud neskonci AD prevod
 353       ;
 354     }
 355
 356     /*Precteni vysledku AD prevodu z ADDRD*/
 357     AN7h = *AD_ADDRDH;                                          //Cteni horniho bytu
 358     AN7d = *AD_ADDRDL;                                          //Cteni dolniho bytu
 359     AN7 = (AN7d >> 6) | (AN7h << 2);                            //Posun jednotlivych bitu, abychom dostali 10-bitove cislo
 360
 361    }                                                            //konec for
 362    return (AN7);                                                // return pro main_loop, muzu vypnout while(1) a }pro main
 363 }                                                               // konec funkce
 364
 365
 366 uint16_t read_QRD3(void)                                        // funkce na vycteni informaci z AD prevodniku z ADDRA kde je nastaven AN8 tedy QRD114-3-prostredni
 367 {
 368   *SYS_MSTPCRA = *SYS_MSTPCRA & ~MSTPCRA_MSTPA1m;               //Zapnuti AD prevodniku
 369
 370   *AD_ADCSR = ADCSR_CH3m;                                       // nastaveni analog vstupu AN8 - tedy vystup ze senzoru QRD114-3-prostredni. CH3=1,CH2=0,CH1=0,CH0=0
 371
 372   uint8_t AN8h, AN8d;
 373   uint16_t AN8;                                                 //AN8h je horni byte AD prevodu, AN8d je dolni byte. Jsou cteny z ADDRA (viz tabulka 4-2 [navratil], AN8 je cele 10-bitove cislo)
 374   int i;
 375
 376   for(i = 0; i < 5; i++){                                       // for cyklus na 5 krat probehnuti smicky
 377     *AD_ADCSR = *AD_ADCSR & ~ADCSR_ADFm;                        // softwarove nastaveni ADF do 0
 378     *AD_ADCSR = *AD_ADCSR | ADCSR_ADSTm;                        // spusteni AD prevodu
 379
 380     while((*AD_ADCSR & ADCSR_ADFm) == 0) {                      // cekaci smycka dokud neskonci AD prevod
 381       ;
 382     }
 383
 384     /*Precteni vysledku AD prevodu z ADDRA*/
 385     AN8h = *AD_ADDRAH;                                          // Cteni horniho bytu
 386     AN8d = *AD_ADDRAL;                                          // Cteni dolniho bytu
 387     AN8 = (AN8d >> 6) | (AN8h << 2);                            // Posun jednotlivych bitu, abychom dostali 10-bitove cislo
 388
 389    }                                                            // konec for
 390    return (AN8);                                                // return pro main_loop, muzu vypnout while(1) a }pro main
 391 }                                                               // konec funkce
 392
 393
 394 uint16_t read_GP1(void)                                         // funkce na vycteni informaci z AD prevodniku z ADDRB kde je nastaven AN9 tedy GP2D12 predni
 395 {                                                               // vraci hodnotu vzdalenosti podle tabulky 4-neco
 396   *SYS_MSTPCRA = *SYS_MSTPCRA & ~MSTPCRA_MSTPA1m;               // Zapnuti AD prevodniku
 397
 398    *AD_ADCSR = ADCSR_CH3m | ADCSR_CH0m;                         // nastaveni analog vstupu AN9 - tedy vzstup ze senzoru GP2D12-predni. CH3=1,CH2=0,CH1=0,CH0=1
 399
 400   uint8_t AN9h, AN9d;
 401   uint16_t AN9;                                                 // AN9h je horni byte AD prevodu, AN9d je dolni byte. Jsou cteny z ADDRB (viz tabulka 4-2 [navratil], AN9 je cele 10-bitove cislo)
 402   int i;
 403
 404   for (i = 0; i < 5; i++){                                      // for cyklus na 5 krat probehnuti smicky , aby se ustalily hodnoty ze senzoru a nedavaly nahodnou hodnotu
 405     *AD_ADCSR = *AD_ADCSR & ~ADCSR_ADFm;                        // softwarove nastaveni ADF do 0
 406     *AD_ADCSR = *AD_ADCSR | ADCSR_ADSTm;                        // spusteni AD prevodu
 407
 408     while((*AD_ADCSR & ADCSR_ADFm) == 0) {                      // cekaci smycka dokud neskonci AD prevod
 409       ;
 410     }
 411
 412     /*Precteni vysledku AD prevodu z ADDRB*/
 413     AN9h = *AD_ADDRBH;                                          // Cteni horniho bytu
 414     AN9d = *AD_ADDRBL;                                          // Cteni dolniho bytu
 415     AN9 = (AN9d >> 6) | (AN9h << 2);                            // Posun jednotlivych bitu, abychom dostali 10-bitove cislo
 416
 417    }                                                            // konec for
 418    return (AN9);                                                // return pro main_loop, muzu vypnout while(1) a }pro main
 419 }                                                               // konec funkce
 420
 421
 422 uint16_t read_GP2(void)                                         // funkce na vycteni informaci z AD prevodniku z ADDRC kde je nastaven AN10 tedy GP2D12 zadni
 423 {
 424   *SYS_MSTPCRA = *SYS_MSTPCRA & ~MSTPCRA_MSTPA1m;               // Zapnuti AD prevodniku
 425   *AD_ADCSR = ADCSR_CH3m | ADCSR_CH1m;                          // nastaveni analog vstupu AN10 - tedy vstup ze senzoru GP2D12-predni. CH3=1,CH2=0,CH1=1,CH0=0
 426
 427   uint8_t AN10h, AN10d;
 428   uint16_t AN10;                                                //AN10h je horni byte AD prevodu, AN10d je dolni byte. Jsou cteny z ADDRC (viz tabulka 4-2 [navratil], AN10 je cele 10-bitove cislo)
 429   int i;
 430
 431    for(i = 0; i < 5; i++){                                      // for cyklus na 5 krat probehnuti smicky, aby se ustalily hodnoty ze senzoru a nedavali nahodne hodnoty
 432     *AD_ADCSR = *AD_ADCSR & ~ADCSR_ADFm;                        // softwarove nastaveni ADF do 0
 433     *AD_ADCSR = *AD_ADCSR | ADCSR_ADSTm;                        // spusteni AD prevodu
 434
 435     while((*AD_ADCSR & ADCSR_ADFm) == 0) {                      // cekaci smycka dokud neskonci AD prevod
 436       ;
 437     }
 438
 439     /*Precteni vysledku AD prevodu z ADDRC*/
 440     AN10h = *AD_ADDRCH;                                         // Cteni horniho bytu
 441     AN10d = *AD_ADDRCL;                                         // Cteni dolniho bytu
 442     AN10 = (AN10d >> 6) | (AN10h << 2);                         // Posun jednotlivych bitu, abychom dostali 10-bitove cislo*/
 443    }                                                             //konec for
 444    return (AN10);
 445 }
 446
 447
 448 int main()
 449 {
 450   _print("Snowforce for life ! ! !\n");
 451   init();
 452   /* TODO initialize rand accoring to voltage read from AD6. */
 453
 454   autonomni_pohyb();                                            // volani hlavni funkce autonomniho pohybu robota
 455   //sleduj_caru();                                              // volani funkce na sledovani cary
 456   for (;;);                                                     // nekonecna smycka, pro odchyceni nahodneho skoku zpet do main
 457   return 0;
 458 }