sw/app/rocon/appl_pxmc.c

   1 /*******************************************************************
   2   Motion and Robotic System (MARS) aplication components.
   3
   4   appl_pxmc.c - position controller subsystem core generic
   5                 and LP_MPW1 hardware specific support
   6
   7   Copyright (C) 2001-2013 by Pavel Pisa - originator
   8                           pisa@cmp.felk.cvut.cz
   9             (C) 2001-2013 by PiKRON Ltd. - originator
  10                           http://www.pikron.com
  11
  12   This file can be used and copied according to next
  13   license alternatives
  14    - GPL - GNU Public License
  15    - other license provided by project originators
  16
  17  *******************************************************************/
  18
  19 #include <cpu_def.h>
  20 #include <system_def.h>
  21 #include <pxmc.h>
  22 #include <pxmc_lpc_qei.h>
  23 #include <pxmc_internal.h>
  24 #include <pxmc_inp_common.h>
  25 #include <hal_gpio.h>
  26 #include <hal_machperiph.h>
  27 #include <stdlib.h>
  28
  29 #include "appl_defs.h"
  30
  31 #define PXML_MAIN_CNT 1
  32
  33 #define PXMC_WITH_PT_ZIC 1
  34 #define PXMC_PT_ZIC_MASK 0x8000
  35
  36 #define LPCPWM1_FREQ 20000
  37
  38 #define HAL_ERR_SENSITIVITY 20
  39 #define HAL_ERR_MAX_COUNT    5
  40
  41 unsigned pxmc_lpcpwm1_magnitude;
  42
  43 int pxmc_hh_gi(pxmc_state_t *mcs);
  44
  45 int
  46 pxmc_inp_rocon_inp(struct pxmc_state *mcs)
  47 {
  48   /* int chan=mcs->pxms_inp_info */
  49   long irc;
  50   long pos;
  51   irc= 0; /* FIXME - read position from hardware */
  52   pos=irc<<PXMC_SUBDIV(mcs);
  53   mcs->pxms_as=pos-mcs->pxms_ap;
  54   mcs->pxms_ap=pos;
  55
  56   /* Running of the motor commutator */
  57   if(mcs->pxms_flg&PXMS_PTI_m)
  58     pxmc_irc_16bit_commindx(mcs,irc);
  59
  60   return 0;
  61 }
  62
  63 int
  64 pxmc_inp_rocon_ap2hw(struct pxmc_state *mcs)
  65 {
  66   /* int chan=mcs->pxms_inp_info */
  67   /* Optional set of the actual position to the HW */
  68   /* We do not want to change real HW counter at any situation */
  69   /* It can be used only to set some software maintained offset */
  70   return 0;
  71 }
  72
  73 inline unsigned
  74 pxmc_rocon_bdc_hal_rd(pxmc_state_t *mcs)
  75 {
  76   unsigned h = 0;
  77   /* FIXME */
  78   /* return 3 bits corresponding to the HAL senzor input */
  79   return h;
  80 }
  81
  82 #if 1
  83 const unsigned char pxmc_lpc_bdc_hal_pos_table[8]=
  84 {
  85   [0]=0xff,
  86   [7]=0xff,
  87   [1]=0,/*0*/
  88   [5]=1,/*1*/
  89   [4]=2,/*2*/
  90   [6]=3,/*3*/
  91   [2]=4,/*4*/
  92   [3]=5,/*5*/
  93 };
  94 #else
  95 const unsigned char pxmc_lpc_bdc_hal_pos_table[8]=
  96 {
  97   [0]=0xff,
  98   [7]=0xff,
  99   [1]=0,/*0*/
 100   [5]=5,/*1*/
 101   [4]=4,/*2*/
 102   [6]=3,/*3*/
 103   [2]=2,/*4*/
 104   [3]=1,/*5*/
 105 };
 106 #endif
 107
 108 /**
 109  * pxmc_rocon_pwm3ph_wr - Output of the 3-phase PWM to the hardware
 110  * @mcs:        Motion controller state information
 111  */
 112 /*static*/ inline void
 113 pxmc_rocon_pwm3ph_wr(pxmc_state_t *mcs, short pwm1, short pwm2, short pwm3)
 114 {
 115   unsigned long out_info=mcs->pxms_out_info;
 116
 117   /* FIXME */
 118 }
 119
 120 static inline void
 121 pxmc_rocon_process_hal_error(struct pxmc_state *mcs)
 122 {
 123   if (mcs->pxms_halerc >= HAL_ERR_SENSITIVITY*HAL_ERR_MAX_COUNT) {
 124     pxmc_set_errno(mcs, PXMS_E_HAL);
 125     mcs->pxms_ene = 0;
 126     mcs->pxms_halerc--;
 127   } else
 128     mcs->pxms_halerc+=HAL_ERR_SENSITIVITY;
 129 }
 130
 131 /**
 132  * pxmc_rocon_pwm_out - Phase output for brush-less 3-phase motor
 133  * @mcs:        Motion controller state information
 134  */
 135 int
 136 pxmc_rocon_pwm_out(pxmc_state_t *mcs)
 137 {
 138   unsigned char hal_pos;
 139   short pwm1;
 140   short pwm2;
 141   short pwm3;
 142   int indx = 0;
 143   short ene;
 144   int wind_current[4];
 145
 146   if(!(mcs->pxms_flg&PXMS_PTI_m) || !(mcs->pxms_flg&PXMS_PHA_m) ||
 147      (mcs->pxms_flg&PXMS_PRA_m)){
 148     short ptindx;
 149     short ptirc=mcs->pxms_ptirc;
 150     short divisor=mcs->pxms_ptper*6;
 151
 152     hal_pos=pxmc_lpc_bdc_hal_pos_table[pxmc_rocon_bdc_hal_rd(mcs)];
 153
 154     if(hal_pos == 0xff) {
 155       if(mcs->pxms_ene)
 156         pxmc_rocon_process_hal_error(mcs);
 157     } else {
 158       if(mcs->pxms_halerc)
 159         mcs->pxms_halerc--;
 160
 161       ptindx=(hal_pos*ptirc+divisor/2)/divisor;
 162
 163       if(!(mcs->pxms_flg&PXMS_PTI_m) || (mcs->pxms_flg&PXMS_PRA_m)){
 164         if(((hal_pos!=mcs->pxms_hal)&&(mcs->pxms_hal!=0x40)) && 1){
 165           short ptindx_prev=(mcs->pxms_hal*ptirc+divisor/2)/divisor;;
 166           if((ptindx>ptindx_prev+ptirc/2) ||
 167              (ptindx_prev>ptindx+ptirc/2)){
 168             ptindx=(ptindx_prev+ptindx-ptirc)/2;
 169            if(ptindx<0)
 170              ptindx+=ptirc;
 171           }else{
 172             ptindx=(ptindx_prev+ptindx)/2;
 173           }
 174
 175           mcs->pxms_ptindx=ptindx;
 176
 177           mcs->pxms_ptofs=(mcs->pxms_ap>>PXMC_SUBDIV(mcs))+mcs->pxms_ptshift-ptindx;
 178
 179           pxmc_set_flag(mcs,PXMS_PTI_b);
 180           pxmc_clear_flag(mcs,PXMS_PRA_b);
 181
 182           /* if phase table position to mask is know do fine phase table alignment */
 183           if(mcs->pxms_cfg & PXMS_CFG_I2PT_m)
 184             lpc_qei_setup_index_catch(&lpc_qei_state);
 185
 186         }else{
 187           if(!(mcs->pxms_flg&PXMS_PTI_m))
 188             mcs->pxms_ptindx=ptindx;
 189         }
 190       }
 191       mcs->pxms_hal = hal_pos;
 192     }
 193   }
 194
 195   {
 196     /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
 197     /* FIXME - check winding current against limit */
 198     /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
 199   }
 200
 201   ene=mcs->pxms_ene;
 202   if(ene){
 203     indx=mcs->pxms_ptindx;
 204    #if 1
 205     /* tuning of magnetic field/voltage advance angle */
 206     indx+=(mcs->pxms_s1*mcs->pxms_as)>>(PXMC_SUBDIV(mcs)+8);
 207    #endif
 208     if(ene<0){
 209       /* Generating direction of stator mag. field for backward torque */
 210       ene=-ene;
 211       if((indx-=mcs->pxms_ptvang)<0)
 212         indx+=mcs->pxms_ptirc;
 213     }else{
 214       /* Generating direction of stator mag. field for forward torque */
 215       if((indx+=mcs->pxms_ptvang)>=mcs->pxms_ptirc)
 216         indx-=mcs->pxms_ptirc;
 217     }
 218
 219     if(mcs->pxms_ptscale_mult)
 220       indx=((unsigned long)indx*mcs->pxms_ptscale_mult)>>mcs->pxms_ptscale_shift;
 221
 222     pwm1=mcs->pxms_ptptr1[indx];
 223     pwm2=mcs->pxms_ptptr2[indx];
 224     pwm3=mcs->pxms_ptptr3[indx];
 225
 226    #ifdef PXMC_WITH_PT_ZIC
 227     if(labs(mcs->pxms_as)>(10<<PXMC_SUBDIV(mcs))) {
 228       if(pwm1&PXMC_PT_ZIC_MASK){
 229         /*hal_gpio_set_value(PWM2_EN_PIN,0);*/
 230       } else {
 231         /*hal_gpio_set_value(PWM2_EN_PIN,1);*/
 232       }
 233       if(pwm2&PXMC_PT_ZIC_MASK){
 234         /*hal_gpio_set_value(PWM4_EN_PIN,0);*/
 235       } else {
 236         /*hal_gpio_set_value(PWM4_EN_PIN,1);*/
 237       }
 238       if(pwm3&PXMC_PT_ZIC_MASK){
 239         /*hal_gpio_set_value(PWM6_EN_PIN,0);*/
 240       } else {
 241         /*hal_gpio_set_value(PWM6_EN_PIN,1);*/
 242       }
 243     }else{
 244       /*hal_gpio_set_value(PWM2_EN_PIN,1);*/
 245       /*hal_gpio_set_value(PWM4_EN_PIN,1);*/
 246       /*hal_gpio_set_value(PWM6_EN_PIN,1);*/
 247     }
 248     pwm1&=~PXMC_PT_ZIC_MASK;
 249     pwm2&=~PXMC_PT_ZIC_MASK;
 250     pwm3&=~PXMC_PT_ZIC_MASK;
 251    #endif /*PXMC_WITH_PT_ZIC*/
 252
 253     /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
 254     /* Initialized CTM4 PWM period is 0x200 => divide by value about 2097024 */
 255     {
 256       unsigned long pwm_dc = pxmc_lpcpwm1_magnitude*(unsigned long)ene;
 257       pwm1=((unsigned long long)pwm1 * pwm_dc)>>(15+15);
 258       pwm2=((unsigned long long)pwm2 * pwm_dc)>>(15+15);
 259       pwm3=((unsigned long long)pwm3 * pwm_dc)>>(15+15);
 260     }
 261     pxmc_rocon_pwm3ph_wr(mcs,pwm1,pwm2,pwm3);
 262   }else{
 263     /*pxmc_lpc_wind_current_over_cnt=0;*/
 264     pxmc_rocon_pwm3ph_wr(mcs,0,0,0);
 265    #ifdef PXMC_WITH_PT_ZIC
 266     /*hal_gpio_set_value(PWM2_EN_PIN,1);*/
 267     /*hal_gpio_set_value(PWM4_EN_PIN,1);*/
 268     /*hal_gpio_set_value(PWM6_EN_PIN,1);*/
 269    #endif /*PXMC_WITH_PT_ZIC*/
 270   }
 271
 272   return 0;
 273 }
 274
 275 /* PWM outputs placed on (PWM1), PWM2, PWM4, PWM6 */
 276 int
 277 pxmc_rocon_pwm_init(pxmc_state_t *mcs, int mode)
 278 {
 279
 280   /* hal_pin_conf(PWM1_EN_PIN); */
 281
 282   /* hal_gpio_set_value(PWM1_EN_PIN,1); */
 283
 284   return 0;
 285 }
 286
 287 pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
 288 {
 289   /*return pxmc_hh_gi;*/
 290   return NULL;
 291 }
 292
 293 pxmc_state_t mcs0={
 294   pxms_flg:PXMS_ENI_m,
 295   pxms_do_inp:pxmc_inp_rocon_inp,
 296   pxms_do_con:pxmc_pid_con,
 297   pxms_do_out:pxmc_rocon_pwm_out,
 298   pxms_do_deb:0,
 299   pxms_do_gen:0,
 300   pxms_do_ap2hw:pxmc_inp_rocon_ap2hw,
 301   pxms_ap:0, pxms_as:0,
 302   pxms_rp: 55*256, pxms_rs:0, pxms_subdiv:8,
 303   pxms_md:800<<8, pxms_ms:1000, pxms_ma:10,
 304   pxms_inp_info:8,
 305   pxms_out_info:0x00000201,
 306   pxms_ene:0, pxms_erc:0,
 307   pxms_p:80, pxms_i:30, pxms_d:200, pxms_s1:200, pxms_s2:0,
 308   pxms_me:0x7e00/*0x7fff*/,
 309   pxms_cfg:PXMS_CFG_SMTH_m|PXMS_CFG_MD2E_m|PXMS_CFG_HLS_m|PXMS_CFG_I2PT_m*0|
 310           0x1,
 311
 312   pxms_ptper:1,
 313   pxms_ptirc:1000,
 314   /*pxms_ptamp: 0x7fff,*/
 315
 316   pxms_hal: 0x40,
 317 };
 318
 319
 320 pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT]=
 321                         {&mcs0};
 322
 323
 324 pxmc_state_list_t  pxmc_main_list={
 325   pxml_arr:pxmc_main_arr,
 326   pxml_cnt:0
 327 };
 328
 329 void pxmc_lpc_qei_callback_index(struct lpc_qei_state_t *qst, void *context)
 330 {
 331   pxmc_state_t *mcs=(pxmc_state_t *)context;
 332   short ofs;
 333   short irc;
 334   irc=qst->index_pos;
 335
 336   if((mcs->pxms_cfg & PXMS_CFG_I2PT_m) && (mcs->pxms_flg&PXMS_PTI_m)) {
 337     short diff;
 338     ofs=irc-mcs->pxms_ptmark;
 339     diff=ofs-mcs->pxms_ptofs;
 340     if(diff>=mcs->pxms_ptirc/2)
 341       diff-=mcs->pxms_ptirc;
 342     if(diff<=-mcs->pxms_ptirc/2)
 343       diff+=mcs->pxms_ptirc;
 344     if(diff<0)
 345       diff=-diff;
 346     if(diff>=mcs->pxms_ptirc/6) {
 347       pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
 348     } else {
 349       mcs->pxms_ptofs = ofs;
 350       pxmc_set_flag(mcs,PXMS_PHA_b);
 351     }
 352   } /*else {
 353     ofs=irc-mcs->pxms_ptofs;
 354     if((unsigned short)ofs>=(unsigned short)mcs->pxms_ptirc) {
 355       if(ofs>0) {
 356         ofs-=mcs->pxms_ptirc;
 357       } else {
 358         ofs+=mcs->pxms_ptirc;
 359       }
 360     }
 361     mcs->pxms_ptmark=ofs;
 362   }*/
 363
 364   /*lpc_qei_set_cmpos(qst, 0, qst->index_pos-4000);*/
 365 }
 366
 367 int pxmc_lpc_pthalalign_callback(pxmc_state_t *mcs)
 368 {
 369   short ptofs;
 370   short ptmark;
 371   lpc_qei_state_t *qst = &lpc_qei_state;
 372   int irc = lpc_qei_get_pos(qst);
 373   int idx_rel;
 374
 375   if(!qst->index_occ)
 376     return 0;
 377
 378   idx_rel=qst->index_pos-irc;
 379   idx_rel%=mcs->pxms_ptirc;
 380   if(idx_rel<0)
 381     idx_rel+=mcs->pxms_ptirc;
 382
 383   ptofs=irc-mcs->pxms_ptofs;
 384   ptmark=ptofs+idx_rel;
 385
 386   if((unsigned short)ptmark>=mcs->pxms_ptirc) {
 387     if(ptmark>0)
 388       ptmark-=mcs->pxms_ptirc;
 389     else
 390       ptmark+=mcs->pxms_ptirc;
 391   }
 392
 393   if((unsigned short)ptmark<mcs->pxms_ptirc) {
 394     mcs->pxms_ptmark=ptmark;
 395   }
 396   return 0;
 397 }
 398
 399 int pxmc_lpc_pthalalign(pxmc_state_t *mcs, int periods)
 400 {
 401   int res;
 402   long r2acq;
 403   long spd;
 404   pxmc_call_t *fin_fnc=pxmc_lpc_pthalalign_callback;
 405   lpc_qei_state_t *qst = &lpc_qei_state;
 406
 407   mcs->pxms_cfg&=~PXMS_CFG_I2PT_m;
 408   lpc_qei_setup_index_catch(qst);
 409
 410   r2acq=((long)mcs->pxms_ptirc<<PXMC_SUBDIV(mcs))*periods;
 411   spd=1<<PXMC_SUBDIV(mcs);
 412
 413   res=pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
 414
 415   return res;
 416 }
 417
 418 int pxmc_lpc_pthalalign_run(void)
 419 {
 420   return pxmc_lpc_pthalalign(pxmc_main_list.pxml_arr[0], 20);
 421 }
 422
 423 static inline void pxmc_sfi_input(void)
 424 {
 425   int var;
 426   pxmc_state_t *mcs;
 427   pxmc_for_each_mcs(var, mcs) {
 428     /* PXMS_ENI_m - check if input (IRC) update is enabled */
 429     if (mcs->pxms_flg&PXMS_ENI_m) {
 430       pxmc_call(mcs, mcs->pxms_do_inp);
 431     }
 432   }
 433 }
 434
 435 static inline void pxmc_sfi_controller_and_output(void)
 436 {
 437   int var;
 438   pxmc_state_t *mcs;
 439   pxmc_for_each_mcs(var, mcs) {
 440     /* PXMS_ENR_m - check if controller is enabled */
 441     if (mcs->pxms_flg&PXMS_ENR_m || mcs->pxms_flg&PXMS_ENO_m) {
 442
 443       /* If output only is enabled, we skip the controller */
 444       if (mcs->pxms_flg&PXMS_ENR_m) {
 445
 446         pxmc_call(mcs, mcs->pxms_do_con);
 447         /* PXMS_ERR_m - if axis in error state */
 448         if (mcs->pxms_flg&PXMS_ERR_m) mcs->pxms_ene = 0;
 449       }
 450
 451       /* for bushless motors, it is necessary to call do_out
 452         even if the controller is not enabled and PWM should be provided. */
 453       pxmc_call(mcs, mcs->pxms_do_out);
 454     }
 455   }
 456 }
 457
 458 static inline void pxmc_sfi_generator(void)
 459 {
 460   int var;
 461   pxmc_state_t *mcs;
 462   pxmc_for_each_mcs(var, mcs) {
 463     /* PXMS_ENG_m - check if requested value (position) generator is enabled */
 464     if (mcs->pxms_flg&PXMS_ENG_m) {
 465       pxmc_call(mcs, mcs->pxms_do_gen);
 466     }
 467   }
 468 }
 469
 470 static inline void pxmc_sfi_dbg(void)
 471 {
 472   int var;
 473   pxmc_state_t *mcs;
 474   pxmc_for_each_mcs(var, mcs) {
 475     if (mcs->pxms_flg&PXMS_DBG_m) {
 476       pxmc_call(mcs, mcs->pxms_do_deb);
 477     }
 478   }
 479 }
 480
 481 int
 482 pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
 483 {
 484   int res;
 485
 486   mcs->pxms_ptvang=pxmc_ptvang_deg2irc(mcs,90);
 487
 488   /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
 489  #ifndef PXMC_WITH_PT_ZIC
 490   res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
 491  #else /*PXMC_WITH_PT_ZIC*/
 492   res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
 493  #endif /*PXMC_WITH_PT_ZIC*/
 494
 495   return res;
 496 }
 497
 498 /**
 499  * pxmc_axis_mode - Sets axis mode.[extern API]
 500  * @mcs:        Motion controller state information
 501  * @mode:       0 .. previous mode, 1 .. stepper motor mode,
 502  *              2 .. stepper motor with IRC feedback and PWM ,
 503  *              3 .. stepper motor with PWM control
 504  *              4 .. DC motor with IRC feedback and PWM
 505  *
 506  */
 507 int
 508 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
 509 {
 510   int res;
 511
 512   pxmc_set_const_out(mcs,0);
 513   pxmc_clear_flag(mcs,PXMS_ENI_b);
 514   pxmc_clear_flag(mcs,PXMS_PHA_b);
 515   pxmc_clear_flag(mcs,PXMS_PTI_b);
 516
 517   if(!mode){
 518     /*mode=pxmc_axis_rdmode(mcs);*/
 519     if(!mode) mode=PXMC_AXIS_MODE_BLDC;
 520   }
 521
 522   res=pxmc_axis_pt4mode(mcs, mode);
 523
 524   pxmc_set_flag(mcs,PXMS_ENI_b);
 525   return res;
 526 }
 527
 528 void pxmc_sfi_isr(void)
 529 {
 530   pxmc_sfi_input();
 531   pxmc_sfi_controller_and_output();
 532   pxmc_sfi_generator();
 533   pxmc_sfi_dbg();
 534 }
 535
 536 int pxmc_initialize(void)
 537 {
 538   int res;
 539
 540   pxmc_state_t *mcs=&mcs0;
 541   lpc_qei_state_t *qst = &lpc_qei_state;
 542
 543   /* Initialize QEI module for IRC counting */
 544   pxmc_inp_lpc_qei_init(mcs->pxms_inp_info);
 545
 546   /* Initialize QEI events processing */
 547   if(lpc_qei_setup_irq(qst)<0)
 548     return -1;
 549
 550   res=pxmc_axis_pt4mode(mcs, PXMC_AXIS_MODE_BLDC);
 551
 552   /*pxmc_ctm4pwm3f_wr(mcs, 0, 0, 0);*/
 553   pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
 554
 555   pxmc_rocon_pwm_init(mcs, 0);
 556
 557   pxmc_main_list.pxml_cnt=0;
 558   pxmc_dbg_hist=NULL;
 559   __memory_barrier();
 560   pxmc_main_list.pxml_cnt=PXML_MAIN_CNT;
 561
 562   return 0;
 563 }