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 <pxmcbsp.h>
  24
  25 #include <cmd_proc.h>
  26 #include "cmd_pxmc.h"
  27 #include "timer3.h"
  28 #include <stdio.h>
  29
  30 cmd_des_t const *cmd_rs232_default[];
  31
  32 /*struktury prikazu cmd*/
  33 cmd_des_t const cmd_des_help={0, 0,"HELP","prints help for commands",
  34                         cmd_do_help,{(char*)&cmd_rs232_default}};
  35
  36 cmd_des_t const *cmd_rs232_default[]={
  37
  38   &cmd_des_help,
  39   CMD_DES_CONTINUE_AT(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 extern cmd_io_t cmd_io_rs232_line;
 117
 118 extern void _print(char *str);
 119
 120 void
 121 init(void)
 122 {
 123   /********************************************************************************/
 124   *DIO_PJDDR=0xff;      /*output gate*/
 125   *DIO_PEDDR=0xff;      /*output gate*/
 126   *DIO_PEDR=0x60;       /*0x0-LED - light all; 0x6 -ENA,ENB=1, LE33CD=0*/
 127   *DIO_PJDR=0x00;       //rozsviceni vsech diod na */
 128
 129
 130   /*priority preruseni - SCI > TPU*/
 131 /*   *SYS_SYSCR|=SYSCR_INTM1m; */
 132 /*   *INT_IPRA=0x22; *INT_IPRB=0x22; *INT_IPRC=0x04;  */
 133 /*   *INT_IPRD=0x40; *INT_IPRE=0x44; *INT_IPRF=0x55; */
 134 /*   *INT_IPRG=0x55; *INT_IPRH=0x55; *INT_IPRJ=0x06; */
 135 /*   *INT_IPRK=0x67; *INT_IPRM=0x66; */
 136
 137   /*povoleni vsech preruseni atd...*/
 138   cli();
 139   excptvec_initfill(unhandled_exception, 0);
 140
 141   /*nastaveni seriovych linek - Bth, PC*/
 142   //sci_rs232_setmode(RS232_BAUD_RAW | 3, 0, 0, 2);     // HCI - hardcoded 115200
 143   //sci_rs232_setmode(115200, 0, 0, 2); // HCI
 144   sci_rs232_setmode(19200, 0, 0, sci_rs232_chan_default); //PC
 145   sti();
 146
 147   _print("Start\n");
 148
 149   pxmc_initialize();
 150
 151   if (init_timer3((long long)200/*ms*/*1000*1000) < 0) {
 152     /* ERROR */
 153     DEB_LED_OFF(0);
 154   }
 155 }
 156
 157
 158 // Distance of wheels - d
 159 #define WHEEL_DIST 74 /* mm */
 160 #define MAX_R 300 /* mm */
 161 #define MIN_R (WHEEL_DIST/2) /* mm */
 162
 163 void move(int speed, int r)
 164 {
 165   int sl, sr;
 166   int d = WHEEL_DIST;
 167
 168
 169   if (r != 0) {
 170     sr = speed + (long long)speed * (d/2) / r;
 171     sl = speed - (long long)speed * (d/2) / r;
 172   } else {
 173     sr = speed;
 174     sl = speed;
 175   }
 176   pxmc_spd(&mcsX0, +sl, 0);
 177   pxmc_spd(&mcsX1, -sr, 0);
 178   //printf("speed=%5d, r=%5d, sl=%5d, sr=%5d\n", speed, r, sl, sr);
 179 }
 180
 181 void
 182 main_loop(void)
 183 {
 184   long int now = get_timer();
 185   long int before = now;
 186   long int next = now;
 187   int speed = 3000;
 188   int radius = 0;
 189   while (1) {
 190     cmd_processor_run(&cmd_io_rs232_line, cmd_rs232_default);
 191
 192     long int now = get_timer();
 193
 194     if (now > before) {
 195       before = now;
 196 //      DEB_LED_XOR(1);
 197     }
 198     if (now >= next) {
 199 //      DEB_LED_XOR(3);
 200       next = now + 1 + (rand() % 5);
 201       //printf("%5ld->%5ld ", now, next);
 202       if (rand() % 100 < 20)
 203         speed = -speed;         /* changing direction */
 204       if (rand() % 100 < 50)
 205         move(speed, 0);         /* go stright ahaed */
 206       else {
 207         radius = (rand() % (2*(MAX_R-MIN_R))) - (MAX_R-MIN_R);
 208         if (radius >= 0) radius += MIN_R;
 209         else radius -= MIN_R;
 210         move(speed, radius);    /* turn */
 211 /*         if (abs(radius) < WHEEL_DIST)  */
 212 /*           next = now + 1;/\* sharp cuves are short *\/ */
 213       }
 214     }
 215   }
 216 }
 217
 218
 219 int main()
 220 {
 221   init();
 222   /* TODO initialize rand accoring to voltage read from AD6. */
 223   main_loop();
 224   return 0;
 225 }