autodemo/mirosot_autodemo.c

   1 /*
   2 *  C Implementation: main_test
   3 *
   4 * Description:
   5
   6 *
   7 *
   8 * Author: Petr Kovacik <kovacp1@feld.cvut.cz>, (C) 2006
   9 *
  10 * Copyright:
  11 *
  12 */
  13 #define _USE_EXR_LEVELS 1
  14 #include <types.h>
  15 #include <cpu_def.h>
  16 #include <h8s2638h.h>
  17 #include <periph/sci_rs232.h>
  18 #include <system_def.h>
  19 #include <stdlib.h>
  20 #include <string.h>
  21
  22 #include <pxmc.h>
  23 #include <pxmc_h2638.h>
  24
  25 #include <cmd_proc.h>
  26 #include "cmd_pxmc.h"
  27 #include "timer3.h"
  28 #include <stdio.h>
  29
  30
  31 /*struktury prikazu cmd*/
  32 cmd_des_t const cmd_des_help={0, 0,"HELP","prints help for commands",
  33                         cmd_do_help,{(char*)&cmd_rs232}};
  34
  35 cmd_des_t const *cmd_rs232_default[]={
  36
  37   &cmd_des_help,
  38   CMD_DES_CONTINUE_AT,   /* list continues at new address */
  39   (cmd_des_t*)cmd_pxmc_default,
  40   NULL
  41 };
  42 cmd_des_t const **cmd_bth=cmd_rs232_default;       /*cmd prikazy pro bth*/
  43 cmd_des_t const **cmd_rs232=cmd_rs232_default;     /*cmd prikazy pro PC*/
  44
  45
  46 /*struktury charakterizujici motor 0*/
  47 pxmc_state_t mcsX0={
  48   pxms_flg:0,
  49   pxms_do_inp:0,
  50   pxms_do_con:0,
  51   pxms_do_out:0,
  52   pxms_do_deb:0,
  53   pxms_do_gen:0,
  54   pxms_ap:0, pxms_as:0,
  55   pxms_rp:155l*256,
  56   pxms_rs:0, //pxms_subdiv:8,
  57   pxms_md:8000l<<8, pxms_ms:5000, pxms_ma:10,
  58   pxms_inp_info:(long)TPU_TCNT1,//TPU_TCNT1                     /*chanel TPU A,B*/
  59   pxms_out_info:(long)PWM_PWBFR1A,                      /*chanel PWM A,B*/
  60   pxms_ene:0, pxms_erc:0,
  61   pxms_p:40, pxms_i:0, pxms_d:1, pxms_s1:0, pxms_s2:0,
  62   pxms_me:0x1800, //6144
  63   pxms_ptirc:40, // 2000 irc per rev, 200/4 steps /
  64   pxms_ptper:1,
  65   pxms_ptptr1:NULL,
  66   pxms_ptptr2:NULL,
  67   pxms_cfg:PXMS_CFG_MD2E_m|PXMS_CFG_HLS_m|
  68       PXMS_CFG_HPS_m|PXMS_CFG_HDIR_m|0x1
  69 };
  70
  71
  72 /*struktury charakterizujici motor 1*/
  73 pxmc_state_t mcsX1={
  74   pxms_flg:0,
  75   pxms_do_inp:0,
  76   pxms_do_con:0,
  77   pxms_do_out:0,
  78   pxms_do_deb:0,
  79   pxms_do_gen:0,
  80   pxms_ap:0, pxms_as:0,
  81   pxms_rp:155l*256,
  82   pxms_rs:0, //pxms_subdiv:8,
  83   pxms_md:8000l<<8, pxms_ms:5000, pxms_ma:10,
  84   pxms_inp_info:(long)TPU_TCNT2,                        /*chanel TPU C,D*/
  85   pxms_out_info:(long)PWM_PWBFR1C,                      /*chanel PWM C,D*/
  86   pxms_ene:0, pxms_erc:0,
  87   pxms_p:40, pxms_i:0, pxms_d:1, pxms_s1:0, pxms_s2:0,
  88   pxms_me:0x1800, //6144
  89   pxms_ptirc:40, // 2000 irc per rev, 200/4 steps /
  90   pxms_ptper:1,
  91   pxms_ptptr1:NULL,
  92   pxms_ptptr2:NULL,
  93   pxms_cfg:PXMS_CFG_MD2E_m|PXMS_CFG_HLS_m|      //FIXME: nastavit spravne priznaky pro dalsi motorove struktur
  94       PXMS_CFG_HPS_m|PXMS_CFG_HDIR_m|0x1
  95 };
  96
  97 pxmc_state_t *pxmc_main_arr[] = {&mcsX0,&mcsX1};
  98
  99 pxmc_state_list_t pxmc_main_list = {
 100   pxml_arr:pxmc_main_arr,
 101   pxml_cnt:sizeof(pxmc_main_arr)/sizeof(pxmc_main_arr[0])
 102 };
 103
 104
 105
 106 //*******************************************************
 107
 108 void  unhandled_exception(void) __attribute__ ((interrupt_handler));
 109
 110 /*Interrupt routines*/
 111 void  unhandled_exception(void)
 112 {
 113 };
 114 //********************************************************
 115
 116
 117 int cmd_rs232_processor_run(void)
 118 {
 119   int val;
 120   cmd_io_t* cmd_io;
 121
 122   cmd_io=&cmd_io_rs232;
 123   if(cmd_rs232_line_out(cmd_io))
 124     return 1;
 125
 126   if(cmd_rs232_line_in(cmd_io)<=0)
 127     return 0;
 128
 129   if(cmd_rs232){
 130     val=proc_cmd_line(cmd_io, cmd_rs232, cmd_io->priv.ed_line.in->buf);
 131   }else{
 132     val=-CMDERR_BADCMD;
 133   }
 134
 135   if(cmd_io->priv.ed_line.out->inbuf){
 136     cmd_io_putc(cmd_io,'\r');
 137     cmd_io_putc(cmd_io,'\n');
 138
 139   }else if(val<0){
 140     char s[20];
 141     cmd_io_write(&cmd_io_rs232,"ERROR ",6);
 142     i2str(s,-val,0,0);
 143     cmd_io_write(cmd_io,s,strlen(s));
 144     cmd_io_putc(cmd_io,'\r');
 145     cmd_io_putc(cmd_io,'\n');
 146   }
 147   return 1;
 148 }
 149
 150 extern void _print(char *str);
 151
 152 void
 153 init(void)
 154 {
 155   /********************************************************************************/
 156   *DIO_PJDDR=0xff;      /*output gate*/
 157   *DIO_PEDDR=0xff;      /*output gate*/
 158   *DIO_PEDR=0x60;       /*0x0-LED - light all; 0x6 -ENA,ENB=1, LE33CD=0*/
 159   *DIO_PJDR=0x00;       //rozsviceni vsech diod na */
 160
 161
 162   /*priority preruseni - SCI > TPU*/
 163 /*   *SYS_SYSCR|=SYSCR_INTM1m; */
 164 /*   *INT_IPRA=0x22; *INT_IPRB=0x22; *INT_IPRC=0x04;  */
 165 /*   *INT_IPRD=0x40; *INT_IPRE=0x44; *INT_IPRF=0x55; */
 166 /*   *INT_IPRG=0x55; *INT_IPRH=0x55; *INT_IPRJ=0x06; */
 167 /*   *INT_IPRK=0x67; *INT_IPRM=0x66; */
 168
 169   /*povoleni vsech preruseni atd...*/
 170   cli();
 171   excptvec_initfill(unhandled_exception, 0);
 172
 173   /*nastaveni seriovych linek - Bth, PC*/
 174   //sci_rs232_setmode(RS232_BAUD_RAW | 3, 0, 0, 2);     // HCI - hardcoded 115200
 175   //sci_rs232_setmode(115200, 0, 0, 2); // HCI
 176   sci_rs232_setmode(19200, 0, 0, sci_rs232_chan_default); //PC
 177   sti();
 178
 179   _print("Start\n");
 180
 181   /*nastaveni HW (TPU, PWM)*/
 182   pxmc_set_pwm_tpu();
 183
 184   /*nastaveni DC motoru*/
 185   pxmc_add_pservice_and_mode(4); /*Macro -  mod=4 tj. all motors are DC*/
 186
 187   if (init_timer3((long long)200/*ms*/*1000*1000) < 0) {
 188     /* ERROR */
 189     DEB_LED_OFF(0);
 190   }
 191 }
 192
 193
 194 // Distance of wheels - d
 195 #define WHEEL_DIST 74 /* mm */
 196 #define MAX_R 300 /* mm */
 197 #define MIN_R (WHEEL_DIST/2) /* mm */
 198
 199 void move(int speed, int r)
 200 {
 201   int sl, sr;
 202   int d = WHEEL_DIST;
 203
 204
 205   if (r != 0) {
 206     sr = speed + (long long)speed * (d/2) / r;
 207     sl = speed - (long long)speed * (d/2) / r;
 208   } else {
 209     sr = speed;
 210     sl = speed;
 211   }
 212   pxmc_spd(&mcsX0, +sl, 0);
 213   pxmc_spd(&mcsX1, -sr, 0);
 214   //printf("speed=%5d, r=%5d, sl=%5d, sr=%5d\n", speed, r, sl, sr);
 215 }
 216
 217 void
 218 main_loop(void)
 219 {
 220   long int now = get_timer();
 221   long int before = now;
 222   long int next = now;
 223   int speed = 3000;
 224   int radius = 0;
 225   while (1) {
 226     cmd_rs232_processor_run();  /*sber + odesilani cmd prikazu PC*/
 227
 228     long int now = get_timer();
 229
 230     if (now > before) {
 231       before = now;
 232       DEB_LED_XOR(1);
 233     }
 234     if (now >= next) {
 235       DEB_LED_XOR(3);
 236       next = now + 1 + (rand() % 5);
 237       //printf("%5ld->%5ld ", now, next);
 238       if (rand() % 100 < 20)
 239         speed = -speed;         /* changing direction */
 240       if (rand() % 100 < 50)
 241         move(speed, 0);         /* go stright ahaed */
 242       else {
 243         radius = (rand() % (2*(MAX_R-MIN_R))) - (MAX_R-MIN_R);
 244         if (radius >= 0) radius += MIN_R;
 245         else radius -= MIN_R;
 246         move(speed, radius);    /* turn */
 247 /*         if (abs(radius) < WHEEL_DIST)  */
 248 /*           next = now + 1;/\* sharp cuves are short *\/ */
 249       }
 250     }
 251   }
 252 }
 253
 254
 255 int main()
 256 {
 257   init();
 258   /* TODO initialize rand accoring to voltage read from AD6. */
 259   main_loop();
 260   return 0;
 261 }