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