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     /* schedule adjustment of phase table from index/mark signal */
 120     mcs->pxms_cfg |= PXMS_CFG_I2PT_m;
 121
 122     /* Move motor to the initial position */
 123     mcs->pxms_flg = (PXMS_PHA_m * 0)| PXMS_PTI_m; /* Do not update ptindex acording to HALs */
 124     mcs->pxms_ptindx=0;         /* Set fixed index to phase tables*/
 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 + 1000) /* Wait for motor to move */
 128             wdg_clear();
 129
 130     ang_deg=0;
 131     for (dir=0; dir<2; dir++) {
 132         for (rot=0; rot<4; rot++) {
 133             pxmc_initialize(); // reset index mark detection
 134             if ((dir&1) == 0) ang_deg=0;
 135             else ang_deg=360;
 136             while (((dir&1) == 0 && ang_deg < 360) ||
 137                    ((dir&1) != 0 && ang_deg >= 0)) {
 138                 wdg_clear();
 139                 ptindx = ((long)ang_deg*mcs->pxms_ptirc/360) % mcs->pxms_ptirc;
 140
 141                 mcs->pxms_flg = 0;
 142                 pxmc_call(mcs, mcs->pxms_do_inp); /* Read IRCs */
 143                 mcs->pxms_ptindx=ptindx;/* Set fixed index to phase tables*/
 144                 mcs->pxms_flg = (PXMS_PHA_m * 0) | PXMS_PTI_m; /* Do not update ptindex acording to HALs */
 145                 motor_do_output(mcs);   /* Apply mag. field accoring to our index */
 146                 last_time = pxmc_msec_counter;
 147                 while (pxmc_msec_counter < last_time + 4); /* Wait for motor to move */
 148                 hal = hal_read(mcs);    /* read HAL sensors */
 149                 printf("%d %d %ld %d\n", mcs->pxms_ptindx, hal,
 150                        mcs->pxms_ap>>PXMC_SUBDIV(mcs), mcs->pxms_ptofs);
 151                 if (mcs->pxms_ptofs != ptofs) { /* index mark was detected */
 152                     ptofs = mcs->pxms_ptofs;
 153                     if ((int)err_idx < (int)(sizeof(ptmark_errors)/sizeof(*ptmark_errors))) {
 154                         if (err_idx >= 0) {
 155                             ptmark_errors[err_idx] =
 156                                 mcs->pxms_ptofs + mcs->pxms_ptindx - (mcs->pxms_ap>>PXMC_SUBDIV(mcs));
 157                         }
 158                         err_idx++;
 159                     }
 160                 }
 161                 if ((dir&1) == 0) ang_deg++;
 162                 else ang_deg--;
 163             }
 164         }
 165     }
 166
 167     /* enable adjustment of phase table from index/mark signal */
 168     mcs->pxms_cfg |= PXMS_CFG_I2PT_m;
 169
 170     mcs->pxms_ene=0;
 171     mcs->pxms_ptvang = vang;
 172     motor_do_output(mcs);
 173     mcs->pxms_flg = PXMS_ENI_m;
 174
 175     long int avg = 0;
 176     for (i=0; i<err_idx; i++) {
 177             wdg_clear();
 178             printf("ptmark error %d\n", ptmark_errors[i]);
 179             avg+=ptmark_errors[i];
 180     }
 181     mcs->pxms_ptmark = (mcs->pxms_ptmark + avg/err_idx) % mcs->pxms_ptirc;
 182     printf("pxms_ptmark increased by %ld to %d\n", avg/err_idx, mcs->pxms_ptmark);
 183
 184     return 0;
 185 }
 186
 187 int cmd_do_setindex(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 188 {
 189     pxmc_state_t *mcs;
 190     if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 191     int index=atol(param[2]);
 192     printf("index=%d\n", index);
 193     //printf("No errors detected.\n", index);
 194     mcs->pxms_ptindx=index;
 195     return 0;
 196 }
 197
 198 int cmd_do_setvang(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     int index=atol(param[2]);
 203     printf("ptvang=%d\n", index);
 204     //printf("No errors detected.\n", index);
 205     mcs->pxms_ptvang=index;
 206     return 0;
 207 }
 208
 209
 210 int cmd_do_setshift(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 211 {
 212     pxmc_state_t *mcs;
 213     if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 214     int index=atol(param[2]);
 215     printf("ptshift=%d\n", index);
 216     //printf("No errors detected.\n", index);
 217     mcs->pxms_ptshift=index;
 218     return 0;
 219 }
 220
 221 int cmd_do_showene(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 222 {
 223     pxmc_state_t *mcs;
 224     if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 225 //      if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 226     printf("Current ene is: %d\n",mcs->pxms_ene);
 227     printf("Current ptindx is: %d\n",mcs->pxms_ptindx);
 228     printf("Current ptvang is: %d\n",mcs->pxms_ptvang);
 229
 230     printf("Current ptshift is: %d\n",mcs->pxms_ptshift);
 231     printf("Current ptofs is: %d\n",mcs->pxms_ptofs);
 232
 233
 234     printf("Actual pos is: %ld\n",mcs->pxms_ap>>PXMC_SUBDIV(mcs));
 235     printf("Request pos is: %ld\n",mcs->pxms_rp>>PXMC_SUBDIV(mcs));
 236
 237     printf("inp_info is: %d\n",*(volatile short*)mcs->pxms_inp_info);
 238
 239     //printf("hal is: %d\n", hal_read(mcs) );
 240
 241     if((mcs->pxms_flg&PXMS_PTI_m)) printf("PXMS_PTI_m is: enabled\n"); else printf("PXMS_PTI_m is: disabled\n");
 242     if((mcs->pxms_flg&PXMS_PHA_m)) printf("PXMS_PHA_m is: enabled\n"); else printf("PXMS_PHA_m is: disabled\n");
 243     return 0;
 244 }
 245
 246 int cmd_do_nopxmc(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 247 {
 248     pxmc_state_t *mcs;
 249     if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 250     pxmc_clear_flag(mcs,PXMS_ENR_b);
 251     pxmc_clear_flag(mcs,PXMS_ENO_b);
 252     *PWM_PWBFR1A=0;
 253     *PWM_PWBFR1C=0;
 254     *PWM_PWBFR1E=0;
 255     return 0;
 256 }
 257
 258 int cmd_do_mypwm(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 259 {
 260     int r;
 261     int val;
 262 #define _PXMC_MAX_OUT_ 0x6f
 263     const unsigned long ph1[6]={2, 1, 1, 0, 1, 1};
 264     const unsigned long ph2[6]={1, 0, 1, 1, 2, 1};
 265     const unsigned long ph3[6]={1, 1, 2, 1, 1, 0};
 266 #undef _PMXC_MAX_OUT_
 267
 268     r=atol(param[1]);
 269     val=atol(param[2])/2;
 270     if (val==0) val = 0;
 271     printf("r=%d val=%d\n", r, val);
 272
 273     unsigned pwm1=val*ph1[r];
 274     unsigned pwm2=val*ph2[r];
 275     unsigned pwm3=val*ph3[r];
 276     *PWM_PWBFR1A=(pwm1&0x3ff);
 277     *PWM_PWBFR1C=(pwm2&0x3ff);
 278     *PWM_PWBFR1E=(pwm3&0x3ff);
 279     return 0;
 280 }
 281
 282 /**
 283  * Implementation of help command with watchdog
 284  */
 285 static INLINE char *skip_white(char *p)
 286 {
 287   while(isspace((__u8)*p)) p++;
 288   return p;
 289 }
 290
 291
 292 #define CMD_ARR_STACK_SIZE 4
 293 int cmd_do_help_wd(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 294 {
 295   char *help;
 296   char *filt=param[1];
 297   const cmd_des_t **des_arr=*(const cmd_des_t ***)des->info[0];
 298   cmd_des_t const **arr_stack[CMD_ARR_STACK_SIZE];
 299   int arr_stack_sp=0;
 300
 301   if (cmd_io->priv.ed_line.io_stack)
 302     cmd_io = cmd_io->priv.ed_line.io_stack;
 303
 304   if(filt) {
 305     filt=skip_white(filt);
 306     if(!*filt) filt=NULL;
 307   }
 308   cmd_io_puts(cmd_io,"Help for commands\n");
 309   while(1){
 310     wdg_clear();                /* Added to avoid reseting while listing commands */
 311     des=*(des_arr++);
 312     if(!des){
 313       if(!arr_stack_sp) break;
 314       des_arr=arr_stack[--arr_stack_sp];
 315       continue;
 316     }
 317     if(des==CMD_DES_CONTINUE_AT_ID){
 318       /* list continues at new address */
 319       des_arr=(const cmd_des_t **)*des_arr;
 320       continue;
 321     }
 322     if(des==CMD_DES_INCLUDE_SUBLIST_ID){
 323       /* list includes commands from sublists */
 324       if(arr_stack_sp>=CMD_ARR_STACK_SIZE){
 325         des_arr++;
 326       }else{
 327         arr_stack[arr_stack_sp++]=des_arr+1;
 328         des_arr=(const cmd_des_t **)*des_arr;
 329         continue;
 330       }
 331     }
 332     if(des->name){
 333       if(!filt || !strncmp(des->name,filt,strlen(filt))) {
 334         help=des->help;
 335         if(!help) help="?";
 336         cmd_io_puts(cmd_io,des->name);
 337         cmd_io_puts(cmd_io," - ");
 338         cmd_io_puts(cmd_io,help);
 339         cmd_io_puts(cmd_io, "\r\n");
 340       }
 341     }
 342   }
 343   return 0;
 344 }
 345
 346 /* selection of debug messages */
 347
 348 #define DBGPF_AXES      0x00ff  /* mask for all axes */
 349 #define DBGPF_PER_TIME  0x0100  /* periodic time print */
 350 #define DBGPF_PER_POS   0x0800  /* periodic position info */
 351 #define DBGPF_CMD_PROC  0x1000  /* command proccessing */
 352 unsigned dbg_prt_flg=0;
 353
 354 typedef struct dbg_prt_des{
 355   unsigned mask;
 356   char *name;
 357 }dbg_prt_des_t;
 358
 359 const dbg_prt_des_t dbg_prt_des[]={
 360   {~0,"all"},
 361   {1,"A"},{2,"B"},
 362   {DBGPF_PER_TIME,"time"},
 363   {DBGPF_PER_POS, "pos"},
 364   {DBGPF_CMD_PROC,"cmd"},
 365   {0,NULL}
 366 };
 367
 368
 369 void run_dbg_prt(cmd_io_t *cmd_io)
 370 {
 371   char s[20];
 372   int i;
 373   pxmc_state_t *mcs;
 374
 375   if(dbg_prt_flg & DBGPF_PER_POS) {
 376     char reg_ascii_id[4];
 377     short reg_mask;
 378     reg_ascii_id[1]='!';
 379     reg_ascii_id[2]=0;
 380
 381     for(i=0,reg_mask=1;i<pxmc_main_list.pxml_cnt; i++,reg_mask<<=1){
 382       if(dbg_prt_flg&reg_mask&DBGPF_AXES){
 383         reg_ascii_id[0]='A'+i;
 384         cmd_io_puts(cmd_io,reg_ascii_id);
 385         mcs=pxmc_main_list.pxml_arr[i];
 386         i2str(s,mcs->pxms_ap>>PXMC_SUBDIV(mcs),8,0);
 387         cmd_io_puts(cmd_io,s);
 388         cmd_io_puts(cmd_io,mcs->pxms_flg&PXMS_ERR_m?" E":
 389                               mcs->pxms_flg&PXMS_BSY_m?" B":" -");
 390
 391         cmd_io_puts(cmd_io," hal");
 392         i2str(s,mcs->pxms_hal,2,0);
 393         cmd_io_puts(cmd_io,s);
 394
 395         cmd_io_puts(cmd_io,mcs->pxms_flg&PXMS_PHA_m?" A":
 396                               mcs->pxms_flg&PXMS_PTI_m?" I":" -");
 397
 398         cmd_io_puts(cmd_io," i");
 399         i2str(s,mcs->pxms_ptindx,4,0);
 400         cmd_io_puts(cmd_io,s);
 401
 402         cmd_io_puts(cmd_io," o");
 403         i2str(s,mcs->pxms_ptofs,6,0);
 404         cmd_io_puts(cmd_io,s);
 405
 406         cmd_io_puts(cmd_io,"\r\n");
 407       }
 408     }
 409   }
 410 }
 411
 412 int cmd_do_switches(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 413 {
 414   unsigned val,*pval;
 415   long l;
 416   dbg_prt_des_t *pdes,*pd;
 417   char *ps;
 418   char str[20];
 419   char pm_flag;
 420   pval=(unsigned*)(des->info[0]);
 421   pdes=(dbg_prt_des_t*)(des->info[1]);
 422   ps=(des->mode&CDESM_OPCHR)?param[3]:param[1];
 423   val=*pval;
 424   if(*ps=='?'){
 425     while(pdes->name){
 426       printf(" %s",pdes->name);
 427       pdes++;
 428     }
 429     printf("\n");
 430   }
 431   while(*ps){
 432     si_skspace(&ps);
 433     if(!*ps) break;
 434     pm_flag=0;
 435     if(*ps=='+'){ps++;}
 436     else if(*ps=='-'){pm_flag=1;ps++;}
 437     else val=0;
 438     if(isdigit((uint8_t)*ps)){if(si_long(&ps,&l,0)<0) return -1;}
 439     else{
 440       si_alnumn(&ps,str,20);
 441       pd=pdes;
 442       do{
 443         if(!pd->name) return -1;
 444         if(!strcmp(pd->name,str)){
 445           l=pd->mask; break;
 446         }
 447         pd++;
 448       }while(1);
 449     }
 450     if(pm_flag) val&=~l;
 451     else val|=l;
 452   }
 453   *pval=val;
 454   return 0;
 455 }
 456
 457 int cmd_do_pthalign(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 458 {
 459     pxmc_state_t *mcs;
 460     int res;
 461
 462     if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 463
 464     if(*param[2]=='?') {
 465       return cmd_opchar_replong(cmd_io, param, (long)mcs->pxms_ptmark, 0, 0);
 466     }
 467
 468     if(*param[2]!=':') return -CMDERR_OPCHAR;
 469
 470     res = motor_pthalalign(mcs, 20);
 471
 472     if(res < 0)
 473        return -CMDERR_BADDIO;
 474
 475     return 0;
 476 }
 477
 478
 479 int runtime_display = 0;
 480 int slowgo = 0;
 481
 482 cmd_des_t const **cmd_list;
 483
 484 /* command descriptions */
 485 cmd_des_t const cmd_des_help={0, 0,"HELP","prints help for commands",
 486                               cmd_do_help_wd,{(char*)&cmd_list}};
 487 cmd_des_t const cmd_des_showene={0, 0,"SHOWENE?","shows ene and couple others parameters",
 488                                  cmd_do_showene};
 489
 490 cmd_des_t const cmd_des_mypwm={0, 0,"x#","test",
 491                                cmd_do_mypwm};
 492 cmd_des_t const cmd_des_nopxmc={0, 0,"NOPXMC?","switch of pxmc controller and output",
 493                                 cmd_do_nopxmc};
 494 cmd_des_t const cmd_des_disp={0, CDESM_OPCHR|CDESM_RW,"DISP","switch on/off runtime display of variables",
 495                               cmd_do_rw_int, {(char*)&runtime_display}};
 496 cmd_des_t const cmd_des_slowgo={0, CDESM_OPCHR|CDESM_RW,"SLOWGO","program controlled movement",
 497                                 cmd_do_rw_int, {(char*)&slowgo}};
 498 cmd_des_t const cmd_des_setindex={0, 0,"SETINDEX?","changes pxms_ptindx",
 499                                   cmd_do_setindex};
 500 cmd_des_t const cmd_des_setvang={0, 0,"SETVANG?","changes pxms_ptvang",
 501                                  cmd_do_setvang};
 502 cmd_des_t const cmd_des_halindex={0, 0,"HALTEST?","show hal state with respect to ptindex",
 503                                   cmd_do_haltest};
 504 cmd_des_t const cmd_des_pthalign={0, CDESM_OPCHR|CDESM_RW,"PTHALIGN?","run alignement of phase according to HAL",
 505                                   cmd_do_pthalign};
 506 cmd_des_t const cmd_des_setshift={0, 0,"SETSHIFT?","changes pxms_ptshift",
 507                                   cmd_do_setshift};
 508 cmd_des_t const cmd_des_quit={0, 0,"QUIT","exit",
 509                               cmd_do_quit};
 510 cmd_des_t const cmd_des_canst={0, 0,"CANST","Print CAN controller status",
 511                               cmd_do_canst};
 512
 513 cmd_des_t const cmd_des_dprint={0, 0,"dprint","enable debug messages to print, use + - to add remove, list types ?",
 514                         cmd_do_switches,
 515                         {(char*)&dbg_prt_flg,(char*)dbg_prt_des}};
 516
 517 cmd_des_t const cmd_des_setflags={0, 0,"setflags","set some flags",
 518                                   cmd_do_setflags};
 519
 520
 521 extern cmd_io_t cmd_io_rs232_line;
 522 cmd_des_t const *cmd_list_default[]={
 523
 524     &cmd_des_help,
 525     &cmd_des_quit,
 526     &cmd_des_setflags,
 527     &cmd_des_mypwm,
 528     &cmd_des_setindex,
 529     &cmd_des_setvang,
 530     &cmd_des_halindex,
 531     &cmd_des_pthalign,
 532     &cmd_des_setshift,
 533     &cmd_des_showene,
 534     &cmd_des_nopxmc,
 535     &cmd_des_disp,
 536     &cmd_des_slowgo,
 537     &cmd_des_canst,
 538     &cmd_des_dprint,
 539     (cmd_des_t*)1,
 540     (cmd_des_t*)cmd_stm_default,
 541     NULL
 542 };
 543
 544 cmd_des_t const **cmd_list=cmd_list_default;     /*cmd prikazy pro PC*/
 545
 546 //Interrupt routines
 547 void  unhandled_exception(void) __attribute__ ((interrupt_handler));
 548 void  unhandled_exception(void)
 549 {
 550 #ifdef LED_ERROR
 551     DEB_LED_ON(LED_ERROR);
 552     FlWait(20000);
 553     DEB_LED_OFF(LED_ERROR);
 554     FlWait(20000);
 555 #endif
 556 }
 557
 558 void print_mcs_err(pxmc_state_t *mcs, char *name)
 559 {
 560     const char *errmsg[] = {
 561         [PXMS_E_COMM&0xff] = "COMM",
 562         [PXMS_E_MAXPD&0xff] = "MAXPD",
 563         [PXMS_E_OVERL&0xff] = "OVERL",
 564         [PXMS_E_HAL&0xff] = "HAL",
 565         [PXMS_E_POWER_STAGE&0xff] = "POWER"};
 566         printf("PXMC ERROR: %s - %s\n", name, errmsg[mcs->pxms_errno&0xff]);
 567 }
 568
 569 short idxRecive =0;
 570 short idxStore=0;
 571 #define ODOMETRY_TABLE_SIZE 10
 572 struct odometryData_t
 573 {
 574     short dx,dy;
 575     long angle;
 576 } oddata[ODOMETRY_TABLE_SIZE];
 577
 578 #if 0
 579 static INLINE short sendOdometry()
 580 {
 581         static short idxSend=0;
 582         static short prevIdxRecive=0xff;
 583
 584         if(prevIdxRecive==idxRecive)
 585         {
 586                 // Re-send can frames.
 587         }
 588         else
 589         {
 590                 prevIdxRecive=idxRecive;
 591                 // Here should be function to send.
 592         }
 593
 594         return 0;
 595 }
 596 #endif
 597
 598
 599 static INLINE void blink_odo_send(void)
 600 {
 601 #ifdef LED_ODO_SEND
 602     static bool led;
 603     if (led) DEB_LED_ON(LED_ODO_SEND);
 604     else DEB_LED_OFF(LED_ODO_SEND);
 605     led = !led;
 606 #endif
 607 }
 608
 609 /**
 610  * Sends only pxms_ap. Odometry is calculated elsewhere and since we
 611  * don't send differences, sequence numbers are not necessary.
 612  */
 613 static INLINE void sendOdometrySimple()
 614 {
 615     /* If the variable is static, it is initialized to zero, so we don't have to initialize it all the time again. */
 616     static Message m;
 617     int32_t lap, rap;
 618
 619     lap = mcs_left.pxms_ap;
 620     rap = mcs_right.pxms_ap;
 621
 622     blink_odo_send();
 623
 624     m.cob_id.w = CAN_MOTION_ODOMETRY_SIMPLE;
 625     m.len = 7;
 626     /* Data in big-endian, the least significant byte is always zero -
 627      * we don't send it */
 628     memcpy(&m.data[0], &lap, sizeof(lap)-1);
 629     memcpy(&m.data[3], &rap, sizeof(lap)-1);
 630     m.data[6] = trig_seq;
 631     canMsgTransmit(0, m);
 632 }
 633
 634 #if 0
 635 static INLINE short storeOdometryInTable()
 636 {
 637         /*FIXME: add detection of error when idxStore>idxRecive.*/
 638         oddata[idxStore].dx=x;
 639         oddata[idxStore].dy=y;
 640         oddata[idxStore].angle=alfa;
 641         // Start to count from zero ;)
 642         x=0;    y=0;    alfa=0;
 643         idxStore++;
 644         idxStore%=ODOMETRY_TABLE_SIZE;
 645
 646         return 0;
 647 }
 648 #endif
 649
 650
 651 static INLINE void blink_err_led(void)
 652 {
 653 #ifdef LED_ERROR
 654         static bool err_led = false;
 655         err_led = !err_led;
 656         if (err_led) DEB_LED_ON(LED_ERROR);
 657         else DEB_LED_OFF(LED_ERROR);
 658 #endif
 659 }
 660
 661 static inline bool recoverable_error(pxmc_state_t *mcs)
 662 {
 663     return (mcs->pxms_flg &PXMS_ERR_m &&
 664             (mcs->pxms_errno == PXMS_E_MAXPD ||
 665              mcs->pxms_errno == PXMS_E_OVERL));
 666 }
 667
 668 static INLINE void handle_motor_errors() {
 669     static unsigned last_msg_time=0;
 670     int i;
 671     pxmc_state_t *mcs;
 672
 673     if (mcs_left.pxms_flg&PXMS_ERR_m||mcs_right.pxms_flg&PXMS_ERR_m) {
 674         pxmc_stop(&mcs_left, 0);
 675         pxmc_stop(&mcs_right, 0);
 676         if (pxmc_msec_counter - last_msg_time >= 2000) {
 677                 blink_err_led();
 678             last_msg_time = pxmc_msec_counter;
 679             if (mcs_left.pxms_flg&PXMS_ERR_m) print_mcs_err(&mcs_left, "L");
 680             if (mcs_right.pxms_flg&PXMS_ERR_m) print_mcs_err(&mcs_right, "R");
 681         }
 682
 683         /* In case of PXMS_E_MAXPD or PXMS_E_OVERL, we wait 3 seconds and then purge the error. */
 684         if (recoverable_error(&mcs_left) ||
 685             recoverable_error(&mcs_right)) {
 686             static unsigned last_err_time=0;
 687             if (last_err_time == 0)
 688                 last_err_time = pxmc_msec_counter;
 689             if (last_err_time != 0 && (pxmc_msec_counter - last_err_time > 3000)) {
 690                 pxmc_for_each_mcs(i, mcs) {
 691                     if (recoverable_error(mcs)) {
 692                         pxmc_set_const_out(mcs,0);
 693                         pxmc_clear_flag(mcs,PXMS_ERR_b);
 694                         last_err_time=0;
 695                     }
 696                 }
 697             }
 698         }
 699     }
 700 }
 701
 702 void led_can_rec(unsigned duration_msec)
 703 {
 704 #ifdef LED_CAN_REC
 705         static unsigned last_rec;
 706         static unsigned duration;
 707         if (duration_msec) {
 708                 last_rec = pxmc_msec_counter;
 709                 duration = MAX(duration_msec, duration);
 710                 DEB_LED_ON(LED_CAN_REC);
 711         } else {
 712                 if (pxmc_msec_counter - last_rec > duration) {
 713                         duration = 0;
 714                         DEB_LED_OFF(LED_CAN_REC);
 715                 }
 716         }
 717 #endif
 718 }
 719
 720 static INLINE void blink_corr_trig(void)
 721 {
 722 #ifdef LED_CORR_TRIG
 723         static bool led;
 724         if (led) DEB_LED_ON(LED_CORR_TRIG);
 725         else DEB_LED_OFF(LED_CORR_TRIG);
 726         led = !led;
 727 #endif
 728 }
 729
 730
 731 static INLINE void handle_can_receive(void)
 732 {
 733     Message msg_rcv;
 734
 735     while (f_can_receive(0, &msg_rcv)) {
 736         switch (msg_rcv.cob_id.w) {
 737             case CAN_MOTION_CMD: {
 738                 short spd_left, spd_right;
 739                 spd_left = (msg_rcv.data[0] << 8) | msg_rcv.data[1];
 740                 spd_right= (msg_rcv.data[2] << 8) | msg_rcv.data[3];
 741
 742                 pxmc_spd(&mcs_left, -spd_left,  pxmc_sfikhz*1000 /*timeout in sampling periods = 0.5ms*/);
 743                 pxmc_spd(&mcs_right,+spd_right, pxmc_sfikhz*1000);
 744 /*                 printf("Left motor speed command: %08x\n",spd_left); */
 745 /*                 printf("Right motor speed command: %08x\n",spd_right); */
 746                 led_can_rec(50);
 747                 break;
 748             }
 749
 750             case CAN_MOTION_RESET: {
 751                 unsigned now = pxmc_msec_counter;
 752                 wdg_enable(0);              /* 25 us */
 753                 while (pxmc_msec_counter - now < 10);
 754                 /* Hopefully, we are reset now. */
 755                 break;
 756             }
 757
 758         }
 759
 760         led_can_rec(5);
 761
 762 /*         int i; */
 763 /*         printf("Received message: id=%lx\n", msg_rcv.cob_id.w); */
 764 /*         for (i = 0 ; i < 8; i++) printf("%02x ", msg_rcv.data[i]); */
 765 /*         printf("\n"); */
 766     }
 767     led_can_rec(0);
 768 }
 769
 770 static INLINE void handle_odometry_send()
 771 {
 772 #define ODOMETRY_PERIOD  43
 773 #define ODOMETRY_TIMEOUT (3*ODOMETRY_PERIOD)
 774     static unsigned
 775         timeout = ODOMETRY_TIMEOUT,
 776         last_send_time = 0;
 777
 778     if (odometry_triggered) {
 779         odometry_triggered = false;
 780         last_send_time = pxmc_msec_counter;
 781         sendOdometrySimple();
 782         timeout = ODOMETRY_TIMEOUT;
 783     }
 784
 785     if (pxmc_msec_counter - last_send_time >= timeout) {
 786         last_send_time = pxmc_msec_counter;
 787         sendOdometrySimple();
 788         timeout = ODOMETRY_PERIOD;
 789     }
 790 }
 791
 792 static INLINE void handle_status_send()
 793 {
 794     static unsigned last_send_time=0;
 795     Message m;
 796
 797     if (pxmc_msec_counter - last_send_time >= 500) {
 798         last_send_time = pxmc_msec_counter;
 799         memset(&m, 0, sizeof(m));
 800         m.cob_id.w = CAN_MOTION_STATUS;
 801         m.len = 4;
 802         m.data[0] = mcs_left.pxms_flg  & PXMS_ERR_m ? mcs_left.pxms_errno  >> 8   : 0;
 803         m.data[1] = mcs_left.pxms_flg  & PXMS_ERR_m ? mcs_left.pxms_errno  & 0xff : 0;
 804         m.data[2] = mcs_right.pxms_flg & PXMS_ERR_m ? mcs_right.pxms_errno >> 8   : 0;
 805         m.data[3] = mcs_right.pxms_flg & PXMS_ERR_m ? mcs_right.pxms_errno & 0xff : 0;
 806         canMsgTransmit(0, m);
 807     }
 808 }
 809
 810 static INLINE void blink_main_loop()
 811 {
 812 #ifdef LED_MAIN_LOOP
 813         static bool led = false;
 814         led = !led;
 815         if (led) DEB_LED_ON(LED_MAIN_LOOP);
 816         else DEB_LED_OFF(LED_MAIN_LOOP);
 817 #endif
 818
 819 }
 820
 821 static INLINE void handle_leds()
 822 {
 823 #define PERIOD 1000
 824         static unsigned last=0;
 825         static bool on=false;
 826         if (pxmc_msec_counter-last >= PERIOD/2) {
 827                 last += PERIOD/2;
 828                 on = !on;
 829                 if (on) {
 830 #ifdef LED_LIVE
 831                         DEB_LED_ON(LED_LIVE);
 832 #endif
 833                 } else {
 834 #ifdef LED_LIVE
 835                         DEB_LED_OFF(LED_LIVE);
 836 #endif
 837 #ifdef LED_RESET
 838                         DEB_LED_OFF(LED_RESET);
 839 #endif
 840                 }
 841         }
 842
 843 #undef PERIOD
 844
 845         blink_main_loop();
 846 }
 847
 848 void _print(char *ptr);
 849 int main()
 850 {
 851     int dbg_prt_last_msec;
 852
 853     cli();
 854     excptvec_initfill(unhandled_exception, 0);
 855     sci_rs232_setmode(19200, 0, 0, sci_rs232_chan_default); //PC
 856     DEB_LED_ON(LED_RESET);
 857     wdg_enable(6);              /* 420 ms */
 858     sti();
 859
 860
 861     _print("CPU initialized\r\n\r\n");
 862
 863     printf("Eurobot motor control application.\n"
 864            "Copyright (C) 2005-2011 PiKRON s.r.o., P. Pisa, M. Sojka and others.\n"
 865            "This is free software; see the source code for copying conditions.\n"
 866            "There is ABSOLUTELY NO WARRANTY; not even for MERCHANTABILITY or\n"
 867            "FITNESS FOR A PARTICULAR PURPOSE.\n\n");
 868
 869     pxmc_initialize();
 870     printf("PXMC initialized (motor: %s)", pxmc_variant);
 871     printf("\n");
 872
 873     pxmc_set_const_out(&mcs_left,0);
 874     pxmc_set_const_out(&mcs_right,0);
 875
 876     int32_t receive_id[] = { CAN_MOTION_CMD, CAN_MOTION_RESET, -1 };
 877     canInit(0, 1000000, receive_id);
 878     printf("CAN initialized\n");
 879
 880     dbg_prt_last_msec = pxmc_msec_counter;
 881
 882     do {
 883         int res;
 884
 885         wdg_clear();
 886         handle_can_receive();
 887         handle_odometry_send();
 888         handle_status_send();
 889         handle_motor_errors();
 890
 891         res = cmd_processor_run(&cmd_io_rs232_line, cmd_list_default);  // run command processor on serial line
 892
 893         if(!res && ((int)(pxmc_msec_counter - dbg_prt_last_msec) > 2000) &&
 894            !cmd_io_rs232_line.priv.ed_line.in->lastch) {
 895            cmd_io_t *cmd_io = &cmd_io_rs232_line;
 896            dbg_prt_last_msec = pxmc_msec_counter;
 897
 898            if (cmd_io->priv.ed_line.io_stack)
 899              cmd_io = cmd_io->priv.ed_line.io_stack;
 900            run_dbg_prt(cmd_io);
 901         }
 902
 903         handle_leds();
 904
 905         if (runtime_display) {
 906             //printf("c=%d idx=%d\n", test_counter, test_index);
 907             //printf("ene=%d\n", mcs_left.pxms_ene);
 908             //printf("rs=%ld as=%ld\n", mcs_left.pxms_rs,  mcs_left.pxms_as);
 909         }
 910     } while (1);
 911
 912     return 0;
 913 };