src/motor-control/brushless.c

   1 /* procesor H8S/2638 ver 1.1  */
   2 #include <types.h>
   3 #include <cpu_def.h>
   4 #include <mcu_regs.h>
   5 #include <system_def.h>
   6 #include <string.h>
   7 #include <boot_fn.h>
   8 #include <pxmc.h>
   9 //#include <pxmc_internal.h>
  10 #include <pxmcbsp.h>
  11
  12 #include <ctype.h>
  13 #include <stdlib.h>
  14 #include <utils.h>
  15 #include <periph/sci_rs232.h>
  16 #include <periph/sci_channels.h>
  17 #include <stdio.h>
  18 #include <stdbool.h>
  19
  20 #include <cmd_proc.h>
  21
  22 #include "cmd_pxmc.h"
  23 //#include <math.h>
  24 #include <cmd_proc.h>
  25 #include <candriver.h>
  26 #include <canOpenDriver.h>
  27 #include <can_ids.h>
  28
  29 /* Meaning of LEDs */
  30 #define LED_LIVE   0            /* Blinks once per second when software is alive */
  31 #define LED_CAN_REC 1           /* Blinks for 0.1 sec when a CAN message is received */
  32 #define LED_ERROR  2            /* Light: Unhandled exception, Slow blink (4s): motor error */
  33 #define LED_RESET  3            /* Blinks for 1 s after reset */
  34
  35
  36 int cmd_do_quit(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
  37 {
  38     printf("Good bye!\n");
  39     *HCAN1_MCR=1;       // software reset
  40     //exit(0);
  41     return 0;
  42 }
  43
  44 int cmd_do_setflags(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
  45 {
  46     pxmc_state_t *mcs;
  47
  48     if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
  49
  50     pxmc_set_flag(mcs,PXMS_PTI_b);      // start to updating index pointing to the phase table (ptindx)
  51     pxmc_set_flag(mcs,PXMS_PHA_b);      // FIXME: is it really need here
  52     printf("Flags were set!\n");
  53     return 0;
  54 }
  55 /**
  56  * Rotates motor slowly four turns forward and than two turns backward
  57  * (in a feedforward manner). During movement it displays values read
  58  * from HAL sensors and tries to detect correct index mark position.
  59  */
  60 int cmd_do_haltest(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
  61 {
  62     pxmc_state_t *mcs;
  63     int i, vang, ptindx, ang_deg, dir, rot;
  64     char hal;
  65     int ptmark_errors[10];
  66     int err_idx=-1;             /* Ignore the first change because */
  67     short ptofs = 1;            /* it is unlikely that initial offset is 1 (after reset it will be zero) */
  68     unsigned last_time = 0;
  69
  70     if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
  71
  72     vang = mcs->pxms_ptvang;    /* Remember vang */
  73
  74     mcs->pxms_ptvang=0;
  75     mcs->pxms_ene=mcs->pxms_me/3;
  76     printf("ptindx hal ap ptofs\n");
  77
  78     /* Move motor to the initial position */
  79     mcs->pxms_flg = PXMS_PHA_m | PXMS_PTI_m; /* Do not update ptindex acording to HALs */
  80     mcs->pxms_ptindx=0;         /* Set fixed index to phase tables*/
  81     motor_do_output(mcs); /* Apply mag. field accoring to our index */
  82     last_time = pxmc_msec_counter;
  83     while (pxmc_msec_counter < last_time + 1000) /* Wait for motor to move */
  84             wdg_clear();
  85
  86     ang_deg=0;
  87     for (dir=0; dir<2; dir++) {
  88         for (rot=0; rot<4; rot++) {
  89             pxmc_initialize(); // reset index mark detection
  90             if ((dir&1) == 0) ang_deg=0;
  91             else ang_deg=360;
  92             while (((dir&1) == 0 && ang_deg < 360) ||
  93                    ((dir&1) != 0 && ang_deg >= 0)) {
  94                 wdg_clear();
  95                 ptindx = ((long)ang_deg*mcs->pxms_ptirc/360) % mcs->pxms_ptirc;
  96
  97                 mcs->pxms_flg = 0;
  98                 pxmc_call(mcs, mcs->pxms_do_inp); /* Read IRCs */
  99                 mcs->pxms_ptindx=ptindx;/* Set fixed index to phase tables*/
 100                 mcs->pxms_flg = PXMS_PHA_m | PXMS_PTI_m; /* Do not update ptindex acording to HALs */
 101                 motor_do_output(mcs);   /* Apply mag. field accoring to our index */
 102                 last_time = pxmc_msec_counter;
 103                 while (pxmc_msec_counter < last_time + 4); /* Wait for motor to move */
 104                 hal = hal_read(mcs);    /* read HAL sensors */
 105                 printf("%d %d %ld %d\n", mcs->pxms_ptindx, hal,
 106                        mcs->pxms_ap>>PXMC_SUBDIV(mcs), mcs->pxms_ptofs);
 107                 if (mcs->pxms_ptofs != ptofs) { /* index mark was detected */
 108                     ptofs = mcs->pxms_ptofs;
 109                     if ((int)err_idx < (int)(sizeof(ptmark_errors)/sizeof(*ptmark_errors))) {
 110                         if (err_idx >= 0) {
 111                             ptmark_errors[err_idx] =
 112                                 mcs->pxms_ptofs + mcs->pxms_ptindx - (mcs->pxms_ap>>PXMC_SUBDIV(mcs));
 113                         }
 114                         err_idx++;
 115                     }
 116                 }
 117                 if ((dir&1) == 0) ang_deg++;
 118                 else ang_deg--;
 119             }
 120         }
 121     }
 122     mcs->pxms_ene=0;
 123     mcs->pxms_ptvang = vang;
 124     motor_do_output(mcs);
 125     mcs->pxms_flg = PXMS_ENI_m;
 126
 127     long int avg = 0;
 128     for (i=0; i<err_idx; i++) {
 129             wdg_clear();
 130             printf("ptmark error %d\n", ptmark_errors[i]);
 131             avg+=ptmark_errors[i];
 132     }
 133     mcs->pxms_ptmark = (mcs->pxms_ptmark + avg/err_idx) % mcs->pxms_ptirc;
 134     printf("pxms_ptmark increased by %ld to %d\n", avg/err_idx, mcs->pxms_ptmark);
 135
 136     return 0;
 137 }
 138
 139 int cmd_do_setindex(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 140 {
 141     pxmc_state_t *mcs;
 142     if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 143     int index=atol(param[2]);
 144     printf("index=%d\n", index);
 145     //printf("No errors detected.\n", index);
 146     mcs->pxms_ptindx=index;
 147     return 0;
 148 }
 149
 150 int cmd_do_setvang(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 151 {
 152     pxmc_state_t *mcs;
 153     if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 154     int index=atol(param[2]);
 155     printf("ptvang=%d\n", index);
 156     //printf("No errors detected.\n", index);
 157     mcs->pxms_ptvang=index;
 158     return 0;
 159 }
 160
 161
 162 int cmd_do_setshift(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 163 {
 164     pxmc_state_t *mcs;
 165     if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 166     int index=atol(param[2]);
 167     printf("ptshift=%d\n", index);
 168     //printf("No errors detected.\n", index);
 169     mcs->pxms_ptshift=index;
 170     return 0;
 171 }
 172
 173 int cmd_do_showene(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 174 {
 175     pxmc_state_t *mcs;
 176     if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 177 //      if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 178     printf("Current ene is: %d\n",mcs->pxms_ene);
 179     printf("Current ptindx is: %d\n",mcs->pxms_ptindx);
 180     printf("Current ptvang is: %d\n",mcs->pxms_ptvang);
 181
 182     printf("Current ptshift is: %d\n",mcs->pxms_ptshift);
 183     printf("Current ptofs is: %d\n",mcs->pxms_ptofs);
 184
 185
 186     printf("Actual pos is: %ld\n",mcs->pxms_ap>>PXMC_SUBDIV(mcs));
 187     printf("Request pos is: %ld\n",mcs->pxms_rp>>PXMC_SUBDIV(mcs));
 188
 189     printf("inp_info is: %d\n",*(volatile short*)mcs->pxms_inp_info);
 190
 191     //printf("hal is: %d\n", hal_read(mcs) );
 192
 193     if((mcs->pxms_flg&PXMS_PTI_m)) printf("PXMS_PTI_m is: enabled\n"); else printf("PXMS_PTI_m is: disabled\n");
 194     if((mcs->pxms_flg&PXMS_PHA_m)) printf("PXMS_PHA_m is: enabled\n"); else printf("PXMS_PHA_m is: disabled\n");
 195     return 0;
 196 }
 197
 198 int cmd_do_nopxmc(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 199 {
 200     pxmc_state_t *mcs;
 201     if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 202     pxmc_clear_flag(mcs,PXMS_ENR_b);
 203     pxmc_clear_flag(mcs,PXMS_ENO_b);
 204     *PWM_PWBFR1A=0;
 205     *PWM_PWBFR1C=0;
 206     *PWM_PWBFR1E=0;
 207     return 0;
 208 }
 209
 210 int cmd_do_mypwm(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 211 {
 212     int r;
 213     int val;
 214 #define _PXMC_MAX_OUT_ 0x6f
 215     const unsigned long ph1[6]={2, 1, 1, 0, 1, 1};
 216     const unsigned long ph2[6]={1, 0, 1, 1, 2, 1};
 217     const unsigned long ph3[6]={1, 1, 2, 1, 1, 0};
 218 #undef _PMXC_MAX_OUT_
 219
 220     r=atol(param[1]);
 221     val=atol(param[2])/2;
 222     if (val==0) val = 0;
 223     printf("r=%d val=%d\n", r, val);
 224
 225     unsigned pwm1=val*ph1[r];
 226     unsigned pwm2=val*ph2[r];
 227     unsigned pwm3=val*ph3[r];
 228     *PWM_PWBFR1A=(pwm1&0x3ff);
 229     *PWM_PWBFR1C=(pwm2&0x3ff);
 230     *PWM_PWBFR1E=(pwm3&0x3ff);
 231     return 0;
 232 }
 233
 234 /**
 235  * Implementation of help command with watchdog
 236  */
 237 static inline char *skip_white(char *p)
 238 {
 239   while(isspace((__u8)*p)) p++;
 240   return p;
 241 }
 242
 243
 244 #define CMD_ARR_STACK_SIZE 4
 245 int cmd_do_help_wd(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 246 {
 247   char *help;
 248   char *filt=param[1];
 249   const cmd_des_t **des_arr=*(const cmd_des_t ***)des->info[0];
 250   cmd_des_t const **arr_stack[CMD_ARR_STACK_SIZE];
 251   int arr_stack_sp=0;
 252
 253   if (cmd_io->priv.ed_line.io_stack)
 254     cmd_io = cmd_io->priv.ed_line.io_stack;
 255
 256   if(filt) {
 257     filt=skip_white(filt);
 258     if(!*filt) filt=NULL;
 259   }
 260   cmd_io_puts(cmd_io,"Help for commands\n");
 261   while(1){
 262     wdg_clear();                /* Added to avoid reseting while listing commands */
 263     des=*(des_arr++);
 264     if(!des){
 265       if(!arr_stack_sp) break;
 266       des_arr=arr_stack[--arr_stack_sp];
 267       continue;
 268     }
 269     if(des==CMD_DES_CONTINUE_AT_ID){
 270       /* list continues at new address */
 271       des_arr=(const cmd_des_t **)*des_arr;
 272       continue;
 273     }
 274     if(des==CMD_DES_INCLUDE_SUBLIST_ID){
 275       /* list includes commands from sublists */
 276       if(arr_stack_sp>=CMD_ARR_STACK_SIZE){
 277         des_arr++;
 278       }else{
 279         arr_stack[arr_stack_sp++]=des_arr+1;
 280         des_arr=(const cmd_des_t **)*des_arr;
 281         continue;
 282       }
 283     }
 284     if(des->name){
 285       if(!filt || !strncmp(des->name,filt,strlen(filt))) {
 286         help=des->help;
 287         if(!help) help="?";
 288         cmd_io_puts(cmd_io,des->name);
 289         cmd_io_puts(cmd_io," - ");
 290         cmd_io_puts(cmd_io,help);
 291         cmd_io_puts(cmd_io, "\r\n");
 292       }
 293     }
 294   }
 295   return 0;
 296 }
 297
 298 int runtime_display = 0;
 299 int slowgo = 0;
 300
 301 cmd_des_t const **cmd_list;
 302
 303 /* command descriptions */
 304 cmd_des_t const cmd_des_help={0, 0,"HELP","prints help for commands",
 305                               cmd_do_help_wd,{(char*)&cmd_list}};
 306 cmd_des_t const cmd_des_showene={0, 0,"SHOWENE?","shows ene and couple others parameters",
 307                                  cmd_do_showene};
 308
 309 cmd_des_t const cmd_des_mypwm={0, 0,"x#","test",
 310                                cmd_do_mypwm};
 311 cmd_des_t const cmd_des_nopxmc={0, 0,"NOPXMC?","switch of pxmc controller and output",
 312                                 cmd_do_nopxmc};
 313 cmd_des_t const cmd_des_disp={0, CDESM_OPCHR|CDESM_RW,"DISP","switch on/off runtime display of variables",
 314                               cmd_do_rw_int, {(char*)&runtime_display}};
 315 cmd_des_t const cmd_des_slowgo={0, CDESM_OPCHR|CDESM_RW,"SLOWGO","program controlled movement",
 316                                 cmd_do_rw_int, {(char*)&slowgo}};
 317 cmd_des_t const cmd_des_setindex={0, 0,"SETINDEX?","changes pxms_ptindx",
 318                                   cmd_do_setindex};
 319 cmd_des_t const cmd_des_setvang={0, 0,"SETVANG?","changes pxms_ptvang",
 320                                  cmd_do_setvang};
 321 cmd_des_t const cmd_des_halindex={0, 0,"HALTEST?","show hal state with respect to ptindex",
 322                                   cmd_do_haltest};
 323 cmd_des_t const cmd_des_setshift={0, 0,"SETSHIFT?","changes pxms_ptshift",
 324                                   cmd_do_setshift};
 325 cmd_des_t const cmd_des_quit={0, 0,"QUIT","exit",
 326                               cmd_do_quit};
 327
 328 cmd_des_t const cmd_des_setflags={0, 0,"setflags","set some flags",
 329                                   cmd_do_setflags};
 330
 331
 332 extern cmd_io_t cmd_io_rs232_line;
 333 cmd_des_t const *cmd_list_default[]={
 334
 335     &cmd_des_help,
 336     &cmd_des_quit,
 337     &cmd_des_setflags,
 338     &cmd_des_mypwm,
 339     &cmd_des_setindex,
 340     &cmd_des_setvang,
 341     &cmd_des_halindex,
 342     &cmd_des_setshift,
 343     &cmd_des_showene,
 344     &cmd_des_nopxmc,
 345     &cmd_des_disp,
 346     &cmd_des_slowgo,
 347     (cmd_des_t*)1,
 348     (cmd_des_t*)cmd_stm_default,
 349     NULL
 350 };
 351
 352 cmd_des_t const **cmd_list=cmd_list_default;     /*cmd prikazy pro PC*/
 353
 354 //Interrupt routines
 355 void  unhandled_exception(void) __attribute__ ((interrupt_handler));
 356 void  unhandled_exception(void)
 357 {
 358     DEB_LED_ON(LED_ERROR);
 359     FlWait(20000);
 360     DEB_LED_OFF(LED_ERROR);
 361     FlWait(20000);
 362 }
 363
 364 void print_mcs_err(pxmc_state_t *mcs, char *name)
 365 {
 366     const char *errmsg[] = {
 367         [PXMS_E_COMM&0xff] = "COMM",
 368         [PXMS_E_MAXPD&0xff] = "MAXPD",
 369         [PXMS_E_OVERL&0xff] = "OVERL",
 370         [PXMS_E_HAL&0xff] = "HAL",
 371         [PXMS_E_POWER_STAGE&0xff] = "POWER"};
 372         printf("PXMC ERROR: %s - %s\n", name, errmsg[mcs->pxms_errno&0xff]);
 373 }
 374
 375 short idxRecive =0;
 376 short idxStore=0;
 377 #define ODOMETRY_TABLE_SIZE 10
 378 struct odometryData_t
 379 {
 380     short dx,dy;
 381     long angle;
 382 } oddata[ODOMETRY_TABLE_SIZE];
 383
 384 #if 0
 385 static inline short sendOdometry()
 386 {
 387         static short idxSend=0;
 388         static short prevIdxRecive=0xff;
 389
 390         if(prevIdxRecive==idxRecive)
 391         {
 392                 // Re-send can frames.
 393         }
 394         else
 395         {
 396                 prevIdxRecive=idxRecive;
 397                 // Here should be function to send.
 398         }
 399
 400         return 0;
 401 }
 402 #endif
 403
 404 /**
 405  * Sends only pxms_ap. Odometry is calculated elsewhere and since we
 406  * don't send defferences, sequence numbers are not necessary.
 407  */
 408 static inline void sendOdometrySimple()
 409 {
 410     /* If the variable is static, it is initialized to zero, co we don't have to initialize it all the time again. */
 411     static Message m;
 412     int32_t lap, rap;
 413
 414     lap = mcs_left.pxms_ap;
 415     rap = mcs_right.pxms_ap;
 416
 417
 418     m.cob_id.w = CAN_MOTION_ODOMETRY_SIMPLE;
 419     m.len = 8;
 420     /* Data in big-endian */
 421     memcpy(&m.data[0], &lap, sizeof(lap));
 422     memcpy(&m.data[4], &rap, sizeof(lap));
 423     canMsgTransmit(0, m);
 424 }
 425
 426 #if O
 427 static inline short storeOdometryInTable()
 428 {
 429         /*FIXME: add detection of error when idxStore>idxRecive.*/
 430         oddata[idxStore].dx=x;
 431         oddata[idxStore].dy=y;
 432         oddata[idxStore].angle=alfa;
 433         // Start to count from zero ;)
 434         x=0;    y=0;    alfa=0;
 435         idxStore++;
 436         idxStore%=ODOMETRY_TABLE_SIZE;
 437
 438         return 0;
 439 }
 440 #endif
 441
 442
 443 static inline void handle_motor_errors() {
 444     static unsigned last_msg_time=0;
 445     static bool err_led = false;
 446     int i;
 447     pxmc_state_t *mcs;
 448
 449     if (mcs_left.pxms_flg&PXMS_ERR_m||mcs_right.pxms_flg&PXMS_ERR_m) {
 450         pxmc_stop(&mcs_left, 0);
 451         pxmc_stop(&mcs_right, 0);
 452         if (pxmc_msec_counter - last_msg_time >= 2000) {
 453             err_led = !err_led;
 454             if (err_led) DEB_LED_ON(LED_ERROR);
 455             else DEB_LED_OFF(LED_ERROR);
 456             last_msg_time = pxmc_msec_counter;
 457             if (mcs_left.pxms_flg&PXMS_ERR_m) print_mcs_err(&mcs_left, "L");
 458             if (mcs_right.pxms_flg&PXMS_ERR_m) print_mcs_err(&mcs_right, "R");
 459         }
 460
 461         /* In case of PXMS_E_MAXPD, we wait 3 seconds and then purge the error. */
 462         if (( mcs_left.pxms_flg&PXMS_ERR_m &&  mcs_left.pxms_errno == PXMS_E_MAXPD) ||
 463             (mcs_right.pxms_flg&PXMS_ERR_m && mcs_right.pxms_errno == PXMS_E_MAXPD)) {
 464             static unsigned last_err_time=0;
 465             if (last_err_time == 0)
 466                 last_err_time = pxmc_msec_counter;
 467             if (last_err_time != 0 && (pxmc_msec_counter - last_err_time > 3000)) {
 468                 pxmc_for_each_mcs(i, mcs) {
 469                     if (mcs->pxms_flg&PXMS_ERR_m &&  mcs->pxms_errno == PXMS_E_MAXPD) {
 470                         pxmc_set_const_out(mcs,0);
 471                         pxmc_clear_flag(mcs,PXMS_ERR_b);
 472                     }
 473                 }
 474             }
 475         }
 476     }
 477 }
 478
 479 static inline void handle_can_receive(void)
 480 {
 481     Message msg_rcv;
 482     static unsigned last_rec;
 483
 484     if (f_can_receive(0, &msg_rcv)) {
 485         switch (msg_rcv.cob_id.w) {
 486             case CAN_MOTION_CMD: {
 487                 short spd_left, spd_right;
 488                 spd_left = (msg_rcv.data[0] << 8) | msg_rcv.data[1];
 489                 spd_right= (msg_rcv.data[2] << 8) | msg_rcv.data[3];
 490
 491                 pxmc_spd(&mcs_left, -spd_left,  pxmc_sfikhz*1000 /*timeout in sampling periods = 0.5ms*/);
 492                 pxmc_spd(&mcs_right,+spd_right, pxmc_sfikhz*1000);
 493 /*                 printf("Left motor speed command: %08x\n",spd_left); */
 494 /*                 printf("Right motor speed command: %08x\n",spd_right); */
 495                 break;
 496             }
 497             case CAN_MOTION_RESET: {
 498                 unsigned now = pxmc_msec_counter;
 499                 wdg_enable(0);              /* 25 us */
 500                 while (pxmc_msec_counter - now < 10);
 501                 /* Hopefully, we are reset now. */
 502                 break;
 503             }
 504         }
 505
 506 #ifdef LED_CAN_REC
 507         last_rec = pxmc_msec_counter;
 508         DEB_LED_ON(LED_CAN_REC);
 509 #endif
 510
 511
 512 /*         int i; */
 513 /*         printf("Received message: id=%lx\n", msg_rcv.cob_id.w); */
 514 /*         for (i = 0 ; i < 8; i++) printf("%02x ", msg_rcv.data[i]); */
 515 /*         printf("\n"); */
 516     }
 517 #ifdef LED_CAN_REC
 518     if (pxmc_msec_counter - last_rec > 100)
 519         DEB_LED_OFF(LED_CAN_REC);
 520 #endif
 521 }
 522
 523 static inline void handle_odometry_send()
 524 {
 525     static unsigned last_send_time=0;
 526
 527     if (pxmc_msec_counter - last_send_time >= 50) {
 528         last_send_time = pxmc_msec_counter;
 529         //sendOdometry();
 530         sendOdometrySimple();
 531     }
 532 }
 533
 534 static inline void handle_status_send()
 535 {
 536     static unsigned last_send_time=0;
 537     Message m;
 538
 539     if (pxmc_msec_counter - last_send_time >= 500) {
 540         last_send_time = pxmc_msec_counter;
 541         memset(&m, 0, sizeof(m));
 542         m.cob_id.w = CAN_MOTION_STATUS;
 543         m.len = 4;
 544         m.data[0] = mcs_left.pxms_errno >> 8;
 545         m.data[1] = mcs_left.pxms_errno & 0xff;
 546         m.data[2] = mcs_right.pxms_errno >> 8;
 547         m.data[3] = mcs_right.pxms_errno & 0xff;
 548         canMsgTransmit(0, m);
 549     }
 550 }
 551
 552 void handle_leds()
 553 {
 554 #define PERIOD 1000
 555         static unsigned last=0;
 556         static bool on=false;
 557         if (pxmc_msec_counter-last >= PERIOD/2) {
 558                 last += PERIOD/2;
 559                 on = !on;
 560                 if (on)
 561                         DEB_LED_ON(LED_LIVE);
 562                 else {
 563                         DEB_LED_OFF(LED_LIVE);
 564                         DEB_LED_OFF(LED_RESET);
 565                 }
 566         }
 567
 568 #undef PERIOD
 569 }
 570
 571 void _print(char *ptr);
 572 int main()
 573 {
 574     cli();
 575     excptvec_initfill(unhandled_exception, 0);
 576     sci_rs232_setmode(19200, 0, 0, sci_rs232_chan_default); //PC
 577     DEB_LED_ON(LED_RESET);
 578     wdg_enable(6);              /* 420 ms */
 579     sti();
 580     _print("CPU initialized\r\n");
 581
 582     pxmc_initialize();
 583     printf("PXMC initialized (motor: %s)", pxmc_variant);
 584     printf("\n");
 585
 586     canInit(0, 1000000);
 587     printf("CAN initialized\n");
 588
 589     do {
 590         wdg_clear();
 591         handle_can_receive();
 592         handle_odometry_send();
 593         handle_status_send();
 594         handle_motor_errors();
 595
 596         cmd_processor_run(&cmd_io_rs232_line, cmd_list_default);  // run command processor on serial line
 597
 598         handle_leds();
 599
 600         if (runtime_display) {
 601             //printf("c=%d idx=%d\n", test_counter, test_index);
 602             //printf("ene=%d\n", mcs_left.pxms_ene);
 603             printf("rs=%ld as=%ld\n", mcs_left.pxms_rs,  mcs_left.pxms_as);
 604         }
 605     } while (1);
 606
 607     return 0;
 608 };