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