src/motor-control/brushless.c

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