--- /dev/null
+../../src/pxmc
\ No newline at end of file
--- /dev/null
+../../src/pxmcbsp
\ No newline at end of file
--- /dev/null
+../../src/pxmc
\ No newline at end of file
--- /dev/null
+../../src/pxmcbsp
\ No newline at end of file
+++ /dev/null
-;; -*- emacs-lisp -*-
-;;
-;; This file is processed by the dirvars emacs package. Each variable
-;; setting below is performed when this dirvars file is loaded.
-;;
-c-basic-offset: 2
-indent-tabs-mode: nil
-tab-width: 2
-;show-trailing-whitespace: t
-;indicate-empty-lines: t
-
+++ /dev/null
-# Generic directory or leaf node makefile for OCERA make framework
-
-ifndef MAKERULES_DIR
-MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
-endif
-
-ifeq ($(MAKERULES_DIR),)
-all : default
-.DEFAULT::
- @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
-else
-include $(MAKERULES_DIR)/Makefile.rules
-endif
-
+++ /dev/null
-lib_LIBRARIES = pxmc
-
-include_HEADERS = pxmc.h
-
-pxmc_SOURCES = pxmc_base.c pxmc_con_pid.c pxmc_gen_spdtrp.c pxmc_deb.c \
- pxmc_ptable.c pxmc_hh.c pxmc_hh_basic.c
-
-SUBDIRS = board/$(BOARD)
-
-utils_PROGRAMS = gen_phase_table
-gen_phase_table_SOURCES = gen_phase_table.c
-gen_phase_table_LIBS = m
\ No newline at end of file
+++ /dev/null
-# Generic directory or leaf node makefile for OCERA make framework
-
-ifndef MAKERULES_DIR
-MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
-endif
-
-ifeq ($(MAKERULES_DIR),)
-all : default
-.DEFAULT::
- @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
-else
-include $(MAKERULES_DIR)/Makefile.rules
-endif
-
+++ /dev/null
-
-default_CONFIG = CONFIG_PXMC_VARIANT=eurobot
-LOCAL_CONFIG_H = pxmc_eurobot_config.h
-
-renamed_include_HEADERS += eurobot.h->pxmcbsp.h
-renamed_include_HEADERS += pxmc_config_h8mirosot.h->pxmc_config.h
-
-lib_LIBRARIES = pxmcbsp
-pxmcbsp_SOURCES = bldc.c
-
-ifeq ($(CONFIG_PXMC_VARIANT),eurobot)
-INCLUDES += -DMOTOR_EUROBOT
-endif
-
-ifeq ($(CONFIG_PXMC_VARIANT),hal1000)
-INCLUDES += -DMOTOR_HAL1000
-endif
-
-ptables:
- $(USER_UTILS_DIR)/gen_phase_table --ptirc=4000 --shape=sin3phup --phasenum=3 --format=oneline --name=bldc --output=pt4000.h
- $(USER_UTILS_DIR)/gen_phase_table --ptirc=2000 --shape=sin3phup --phasenum=3 --format=oneline --name=bldc --output=pt2000.h
+++ /dev/null
-/*******************************************************************
- Copyright (c)2001 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (c)2006-2007 by Konrad Skup ahblitz@yahoo.de
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
- *******************************************************************/
-
-#include <pxmc.h>
-#include "pxmc_eurobot_config.h"
-
-#include <types.h>
-#include <cpu_def.h>
-#include <h8s2638h.h>
-#include <system_def.h>
-#include <string.h>
-#include <pxmc.h>
-#include "../../pxmc_internal.h"
-#include "../../pxmc_inp_common.h"
-#include "eurobot.h"
-
-#ifdef MOTOR_HAL1000
-#include "pt4000.h"
-#endif
-
-#ifdef MOTOR_EUROBOT
-#include "pt2000.h"
-#endif
-
-//=================================
-/* Definitions of some basic but
- usefull variables. */
-//=================================
-int pxmc_sfikhz = 2; // Sampling freq. interrupt - how often we want to collect data
-int pxmc_sfi2msec;
-volatile unsigned pxmc_msec_counter; // miliseconds counter
-#define PWM_HZ 20000
-#define PWM_MAX (((CPU_SYS_HZ / PWM_HZ) <= PWBFR2A_DTxm) ? (CPU_SYS_HZ / PWM_HZ) : PWBFR2A_DTxm)
-#define HAL_ERR_SENSITIVITY 20
-#define HAL_ERR_MAX_COUNT 1000
-static int motor_do_input(struct pxmc_state *mcs);
-#ifndef DEBUG
-static
-#endif
-int motor_do_output(struct pxmc_state *mcs);
-static int tpu_irc_ap2hw(struct pxmc_state *mcs);
-
-#ifdef MOTOR_EUROBOT
-#define PXMS_MD 20000l<<8
-#define MAX_RPS 2 /* Wheel rotations per second */
-#define GEAR_RATIO 28
-#define PXMS_MS (((long)MAX_RPS*GEAR_RATIO)<<8)
-#define PXMS_MA 30
-#define PXMS_P 100
-#define PXMS_I 0
-#define PXMS_D 0
-#endif
-
-#ifdef MOTOR_HAL1000
-#error HAL_1000
-#define GEAR_RATIO 159
-#define PXMS_MD 20000l<<8
-#define PXMS_MS 125<<8
-#define PXMS_MA 40
-#define PXMS_P 100
-#define PXMS_I 20
-#define PXMS_D 100
-#endif
-
-const short sintab[256]={0,3,6,9,13,16,19,22,25,28,31,34,37,40,43,46,49,52,55,58,60,63,66,68,71,74,76,79,81,84,86,88,91,93,95,97,99 , 101,103,105,106,108,110,111,113,114,116,117,\
-118,119,121,122,122,123,124,125,126,126,127,127,127,128,128,128,128,128,128,128,127,127,127,126,\
-126,125,124,123,122,122,121,119,118,117,116,114,113,111,110,108,106,105,103,101,99,97,95,93,\
-91,88,86,84,81,79,76,74,71,68,66,63,60,58,55,52,49,46,43,40,37,34,31,28,\
-25,22,19,16,13, 9, 6, 3, 0,-3,-6,-9,-13,-16,-19,-22,-25,-28,-31,-34,-37,-40,-43,-46,\
--49,-52,-55,-58,-60,-63,-66,-68,-71,-74,-76,-79,-81,-84,-86,-88,-91,-93,-95,-97,-99 -101,-103,-105,\
--106,-108,-110,-111,-113,-114,-116,-117,-118,-119,-121,-122,-122,-123,-124,-125,-126,-126,-127,-127,-127,-128,-128,-128,\
--128,-128,-128,-128,-127,-127,-127,-126,-126,-125,-124,-123,-122,-122,-121,-119,-118,-117,-116,-114,-113,-111,-110,-108,\
- -106,-105,-103,-101,-99,-97,-95,-93,-91,-88,-86,-84,-81,-79,-76,-74,-71,-68,-66,-63,-60,-58,-55,-52,\
--49,-46,-43,-40,-37,-34,-31,-28,-25,-22,-19,-16,-13,-9,-6,-3, 0}; // sinus table
-
-pxmc_state_t mcs_right = {
- pxms_flg:PXMS_ENI_m,
- pxms_do_inp: motor_do_input, // set pointer to funct. for measuring IRC
- pxms_do_out: motor_do_output, //pxmc_do_out_brushless; // set pointer to funct. control energi for outpu (PWM)
- pxms_do_con: pxmc_pid_con, // set pointer to PID con. function (implicit (PXMC.c) or user
- pxms_do_ap2hw: tpu_irc_ap2hw,
- pxms_md: PXMS_MD, pxms_ms: PXMS_MS, pxms_ma: PXMS_MA,
- pxms_inp_info: (long)TPU_TCNT1,//TPU_TCNT1 /*chanel TPU A,B*/
- pxms_out_info: 0, /*chanel PWM A,B*/
- pxms_p: PXMS_P, pxms_i: PXMS_I, pxms_d: PXMS_D, pxms_s1: 0, pxms_s2: 0,
- pxms_ptirc: bldc_ptirc, // IRC count per phase table
- pxms_ptper: bldc_ptper, // Number of periods per table
-
- //* @pxms_ptofs: Offset between table and IRC counter
- pxms_ptshift: 0, // Shift of generated phase curves
- pxms_ptvang: bldc_ptirc/4, // Angle (in irc) between rotor and stator mag. fld.
-
- pxms_ptptr1: bldc_phase1,
- pxms_ptptr2: bldc_phase3,
- pxms_ptptr3: bldc_phase2,
- pxms_ptamp: 0x7fff,
- pxms_me:PWM_MAX << 5,
- pxms_hal: 0x40,
- pxms_cfg: PXMS_CFG_HLS_m | PXMS_CFG_HPS_m | PXMS_CFG_HDIR_m| PXMS_CFG_SMTH_m| 0x1
-};
-
-pxmc_state_t mcs_left = {
- pxms_flg:PXMS_ENI_m,
- pxms_do_inp: motor_do_input, // set pointer to funct. for measuring IRC
- pxms_do_out: motor_do_output, //pxmc_do_out_brushless; // set pointer to funct. control energi for outpu (PWM)
- pxms_do_con: pxmc_pid_con, // set pointer to PID con. function (implicit (PXMC.c) or user
- pxms_do_ap2hw: tpu_irc_ap2hw,
- pxms_md: PXMS_MD, pxms_ms: PXMS_MS, pxms_ma: PXMS_MA,
- pxms_inp_info: (long)TPU_TCNT2,//TPU_TCNT1 /*chanel TPU A,B*/
- pxms_out_info: 1, /*chanel PWM A,B*/
- pxms_p: PXMS_P, pxms_i: PXMS_I, pxms_d: PXMS_D, pxms_s1: 0, pxms_s2: 0,
- pxms_ptirc: bldc_ptirc, // IRC count per phase table
- pxms_ptper: bldc_ptper, // Number of periods per table
-
- //* @pxms_ptofs: Offset between table and IRC counter
- pxms_ptshift: 0, // Shift of generated phase curves
- pxms_ptvang: bldc_ptirc/4, // Angle (in irc) between rotor and stator mag. fld.
-
- pxms_ptptr1: bldc_phase1,
- pxms_ptptr2: bldc_phase3,
- pxms_ptptr3: bldc_phase2,
- pxms_ptamp: 0x7fff,
- pxms_me:PWM_MAX << 5,
- pxms_hal: 0x40,
- pxms_cfg: PXMS_CFG_HLS_m | PXMS_CFG_HPS_m | PXMS_CFG_HDIR_m |PXMS_CFG_SMTH_m |0x1
-};
-
-pxmc_state_t *pxmc_main_arr[] = {&mcs_left, &mcs_right};
-
-pxmc_state_list_t pxmc_main_list = {
- pxml_arr: pxmc_main_arr,
- pxml_cnt: sizeof(pxmc_main_arr) / sizeof(pxmc_main_arr[0])
- };
-
-const unsigned char eurobot_bdc_hal_pos_table[8] =
- {
- [0] = 0xff,
- [7] = 0xff,
- [6] = 0,/*0*/
- [2] = 1,/*1*/
- [3] = 2,/*2*/
- [1] = 3,/*3*/
- [5] = 4,/*4*/
- [4] = 5,/*5*/
- };
-
-/**
- * Function used to call pxms_do_inp functions for all motors.
- * It's called by pxmc_sfi_isr during interrupt event.
- */
-static inline void interupt_input()
-{
- int var;
- pxmc_state_t *mcs;
- pxmc_for_all_axes(var, mcs) {
- // PXMS_ENI_m - check if input (IRC) update is enabled
- if (mcs->pxms_flg&PXMS_ENI_m) {
- pxmc_call(mcs, mcs->pxms_do_inp);
-// error_led(~1);
- }
- }
-}
-
-/**
- * Function used to call pxms_do_con and pxms_do_out
- * functions for all motors.
- * It's called by pxmc_sfi_isr during interrupt event.
- */
-static inline void interupt_controller_and_output()
-{
- int var;
- pxmc_state_t *mcs;
- pxmc_for_all_axes(var, mcs) {
- // PXMS_ENR_m - check if controller is enabled
- if (mcs->pxms_flg&PXMS_ENR_m || mcs->pxms_flg&PXMS_ENO_m) {
-
- /* If output only is enabled, we skip the controller */
- if (mcs->pxms_flg&PXMS_ENR_m) {
- //static int i=0;
- //i=(i+1)%200;
- //if (i==0) *DIO_PJDR ^= 2;
-
- pxmc_call(mcs, mcs->pxms_do_con);
- // PXMS_ERR_m - if axis in error state
- if (mcs->pxms_flg&PXMS_ERR_m) mcs->pxms_ene = 0;
- }
-
- // FIXME: For bushless motors, it is necessary to call do_out
- // even if the controller is not enabled.
- pxmc_call(mcs, mcs->pxms_do_out);
- // error_led(1);
- }
- }
-}
-
-/**
- * Function used to call pxms_do_gen functions for all motors.
- * It's called by pxmc_sfi_isr during interrupt event.
- */
-static inline void interupt_generator()
-{
- int var;
- pxmc_state_t *mcs;
- pxmc_for_all_axes(var, mcs) {
- // PXMS_ENG_m - check if requested value (position) generator is enabled
- if (mcs->pxms_flg&PXMS_ENG_m) {
- pxmc_call(mcs, mcs->pxms_do_gen);
- // error_led(~2);
- }
- }
-}
-
-/**
- * Function used to call pxms_do_deb functions for all motors.
- * It's called by pxmc_sfi_isr during interrupt event.
- */
-static inline void interupt_dbg()
-{
- int var;
- pxmc_state_t *mcs;
- pxmc_for_all_axes(var, mcs) {
- if (mcs->pxms_flg&PXMS_DBG_m) {
- pxmc_call(mcs, mcs->pxms_do_deb);
- // error_led(2);
- }
- if (--pxmc_sfi2msec <= 0) {
- msec_time++;
- pxmc_sfi2msec = pxmc_sfikhz;
- /* Hook for external finite state machine */
- } else {
- /* Run ADC measurements */
- }
- }
-}
-
-int x=0,y=0; // initial position of the robot
-long alfa=0.0; // angle in which robot is turned at the begin
-
-/**
- * Sampling Frequency Interrupt
- * ----------------------------
- * This function is used to call functions for different services,
- * like pxms_do_inp,pxms_do_out...
- * It works due to interrupts, and is called with freq. = pxmc_sfikhz.
- * Is is setting up by: PXMC_TPU_SFI_INIT - macro.
- */
-/* int intno, void *dev_id, struct pt_regs *regs */
-void pxmc_sfi_isr(void) __attribute__ ((interrupt_handler));
-void pxmc_sfi_isr(void) // sampling frequency
-{
- //*TPU_TSR5&=~TSR5_TGFAm;
- /* Clear interrupt source */
- /* PXMS_ENI_m = enable input (IRC) update */
- interupt_input();
-
- /* PXMS_ENG_m - enable requested value (position) generator*/
- interupt_generator();
-
- /*PXMS_ENR_m - enable controller*/
- interupt_controller_and_output();
-
- /* PXMS_DBG_m - enable debugging */
- interupt_dbg();
-
- static int submsec = 0;
- submsec++;
- if (submsec==pxmc_sfikhz) {
- submsec = 0;
- pxmc_msec_counter++;
- }
-
-#if 0
- /* Odometry */
- /* FIXME: all time constants, radius, sampling times, units .... */
- #define PI_MULT 256
- #define SINTAB_LEN 256
- #define SINTAB_AMP 128
- const short r=10; // radius of wheel [mm]
- const short l=150; // distance between wheels [mm]
- const short PI=804; // 3.14*256 = 3.14*PI_MULT
- const short dt=1; // sampling time [10ms] 0.2 ms
-
- static int xl=-l,yl=0; // initial position of wheels
- static int xr=0,yr=0; // it is strong correlated with alfa
-
- short O=2*PI*r; // [256mm]
-
- short vl=mcs_left.pxms_as; // [rot/ms] /*FIXME: should be recalculation from IRC to rot/10ms */
- short vr=mcs_right.pxms_as;
-
- //da=(vl-vr)*dt; % difference between motors [rot]
- //ds=(vl+vr)/2*O*dt/256/100; % distance went by the robot [mm]
-
- alfa=alfa+(vl-vr)*dt*((double)O/(double)PI_MULT )/l; //[rad]
- // keep alfa in range (0; 3.14)
- while(alfa>3.14) alfa-=3.14;
- while(alfa<0) alfa+=3.14;
- int Oo=O/PI_MULT;
- short angx=((int)(alfa*SINTAB_LEN/(2*3.14)))%SINTAB_LEN;
- short angy=angx+SINTAB_LEN/4;
-
- xl=xl+vl*Oo*dt*sintab[angx]/SINTAB_AMP; yl=yl+vl*Oo*dt*sintab[angy]/SINTAB_AMP;
- xr=xr+vr*Oo*dt*sintab[angx]/SINTAB_AMP; yr=yr+vr*Oo*dt*sintab[angy]/SINTAB_AMP;
-
- x=(xl+xr)/2;
- y=(yl+yr)/2;
- #undef SINTAB_AMP
- #undef SINTAB_LEN
- #undef PI_MULT
-#endif
- // Allow next interrupt...
- *TPU_TSR4 &= ~TSR4_TGFAm; //Acknowledge an interrupt
-}
-
-#define TEMP_TPU_PHASE_CNT_INIT(n) \
- do{ \
- /*TimerControlRegistr - TCNT clearing disabled; count rasing adge; internal clock fi/1 */\
- *TPU_TCR##n=0; \
- /*phase counting mode 1 - how will be TCNT incrementd */\
- *TPU_TMDR##n=TPMDR_MD_PHACN1; \
- /* Input capture at rising edge on TIOCAx pin (index mark) */\
- *TPU_TIOR##n = TIOR##n##_IOA3m; \
- /*AD conversion disabled; interrupt TGIA enabled.; */\
- /* *TPU_TIER##n=TIER##n##_TGIEAm;*/ \
- /*excptvec_set(EXCPTVEC_TGI##n##A,index_mark_isr##n);*/ \
- /*start TCNT*/\
- *TPU_TSTR|=(1<<n); \
-}while (0)
-
-// TRICK: See cpp info pages, node Argument Prescan,
-// parahraph "Macros that call other macros that stringify or concatenate."
-#define TPU_PHASE_CNT_INIT(n) TEMP_TPU_PHASE_CNT_INIT(n)
-
-
-static void init_pwm(void)
-{
- /* setup pwm */
- *SYS_MSTPCRD &= ~MSTPCRD_PWMm;
-
- *PWM_PWOCR2 =
- PWOCR2_OE2Am | PWOCR2_OE2Bm | PWOCR2_OE2Cm |
- PWOCR2_OE2Em | PWOCR2_OE2Fm | PWOCR2_OE2Gm; // switch on needed channels
-
- *PWM_PWPR2 =
- PWPR2_OPS2Am | PWPR2_OPS2Bm | PWPR2_OPS2Cm |
- PWPR2_OPS2Em | PWPR2_OPS2Fm | PWPR2_OPS2Gm; // reverse polarity of PWM outputs
-
- *PWM_PWCYR2 = PWM_MAX;
-
- *PWM_PWCR2 = PWCR2_CSTm | PWCR2_CKS_F1; /* Start PWM timer, clock = phi/1 */
-}
-
-void index_mark_isr1(void) __attribute__((interrupt_handler));
-void index_mark_isr2(void) __attribute__((interrupt_handler));
-void index_mark_isr1(void)
-{
- static int count = 2;
- pxmc_state_t *mcs = &mcs_left;
-
- // I think we can detect index mark after detection of hal edge.
- if (mcs->pxms_flg&PXMS_PTI_m && --count==0) { //FIXME constant is different for different motors
- mcs->pxms_ptofs = (*TPU_TGR1A - 5l/*degrees*/*mcs->pxms_ptirc/360);
- //pxmc_set_flag(mcs, PXMS_PTI_b); // see the 1st comment
- pxmc_set_flag(mcs, PXMS_PHA_b);
- count = 2; //If it is necessary to find index mark later
- //*TPU_TIER1=0; //Disable future index mark interrupts
- }
-#ifdef LED_INDEX1
- static int led = 0;
- if (led ^= 1) DEB_LED_ON(LED_INDEX1);
- else DEB_LED_OFF(LED_INDEX1);
-#endif
- *TPU_TSR1 &= ~TSR1_TGFAm;
-}
-void index_mark_isr2(void)
-{
- static int count = 2;
- pxmc_state_t *mcs = &mcs_right;
-
- if (--count==0) {
- mcs->pxms_ptofs = (*TPU_TGR2A - 5l/*degrees*/*mcs->pxms_ptirc/360);
- pxmc_set_flag(mcs, PXMS_PTI_b);
- pxmc_set_flag(mcs, PXMS_PHA_b);
- count = 2; //If it is necessary to find index mark later
- *TPU_TIER2=0;
- }
-#ifdef LED_INDEX2
- static int led = 0;
- if (led ^= 1) DEB_LED_ON(LED_INDEX2);
- else DEB_LED_OFF(LED_INDEX2);
-#endif
- *TPU_TSR2 &= ~TSR2_TGFAm;
-}
-
-static void init_irc(void)
-{
- /* setup tpu */
- *SYS_MSTPCRA &= ~MSTPCRA_TPUm; /* Switch on TPU module */
- TPU_PHASE_CNT_INIT(1);
- TPU_PHASE_CNT_INIT(2);
-}
-
-static void init_hal(void)
-{
- *DIO_PBDDR = 0; // hal and fault signals are inputs
-}
-
-static void init_sampling(void)
-{
- *SYS_MSTPCRA &= ~MSTPCRA_TPUm; /* Switch on TPU module */
-
- *TPU_TSTR &= ~TSTR_CST4m; /*counter n is stoped*/
- /*CNTN cleared by by TGRA, rising edge; source TGRA compare match/input capture */
- *TPU_TCR4 = (TPCR_TPSC_F1 | TPCR_CKEG_RIS | TPCR_CCLR_TGRA);
- /*TGRA is output compare registr - . 0 output at compare match*/
- *TPU_TIOR4 |= TIOR4_IOA1m;
- /*MDn = 0x000 normal operation */
- *TPU_TMDR4 = TPMDR_MD_NORMAL;
- /*seting interupt vector - from TGRA at registr TSRn*/
- excptvec_set(EXCPTVEC_TGI4A, pxmc_sfi_isr);
- /*TimerStatusRegistr - clear match flag */
- *TPU_TSR4 &= ~TSR4_TGFAm;
- /*TimerInteruptEnableReg - enable IRQ generation from TGRA*/
- *TPU_TIER4 |= TIER4_TGIEAm;
- /*setup TGRA - sampling period*/
- *TPU_TGR4A = (CPU_SYS_HZ / 1000) / pxmc_sfikhz;
- /*Start TCNTn*/
- *TPU_TSTR |= TSTR_CST4m;
-}
-
-
-/**
- * This function set up pwm for out and hal sensors as i/o.
- * TODO: change form hex to names
- * generalization for more than one motor
- */
-void pxmc_set_pwm_for_rotation()
-{
- *DIO_PJDDR &= ~0xf8; // hal(d,e,f and g,h) is in as i/o
- *DIO_PJDDR |= 0x07; // led-s are out as i/o
- //*DIO_PHDDR|=0x15; // 3 windings as out
-
- *PWM_PWOCR1 |= 0x15; // enable output as pwm
- *PWM_PWOCR2 &= ~0x07; // switch off: A,B,C for PWM - leds - now as i/o
- *PWM_PWOCR2 &= ~0xf8; // switch off: D,E,F(and g,h) for PWM - hal - now as i/o
-
- //*PWM_PWPR1=0x00; // normal polarity
- *PWM_PWCR1 &= ~0x08; // be sure clock is off
- //*PWM_PWCYR1=0x0f; // minimal duty
- *PWM_PWCR1 |= 0xc8; // 8-start clock, ph/1
- *PWM_PWCR2 |= 0xc8;
- *SYS_MSTPCRD &= ~MSTPCRD_PWMm;
-}
-//=========================================
-/* Control functions for brushless motor...*/
-//=========================================
-
-int tpu_irc_ap2hw(struct pxmc_state *mcs)
-{
- short old_pos;
- if (mcs->pxms_inp_info)
- {
-#ifdef PXMC_WITH_PHASE_TABLE
- pxmc_clear_flag(mcs, PXMS_PHA_b);
- old_pos = *(short *)mcs->pxms_inp_info;
- mcs->pxms_ptofs += (mcs->pxms_ap>>PXMC_SUBDIV(mcs)) - old_pos;
-#endif
- *(short *)mcs->pxms_inp_info = (mcs->pxms_ap>>PXMC_SUBDIV(mcs));
- }
- return 0;
-}
-
-static int motor_do_input(struct pxmc_state *mcs)
-{
- short irc;
- irc = *(volatile short*)mcs->pxms_inp_info;
- pxmc_irc_16bit_update(mcs, irc);
-
- /* Running of the motor commutator */
- //irc=mcs->pxms_rp>>PXMC_SUBDIV(mcs); // when enabled means without feedback
- if (mcs->pxms_flg&PXMS_PTI_m) pxmc_irc_16bit_commindx(mcs, irc); //pxmc_irc_16bit_commindx(mcs,irc);
-
- return 0;
-}
-
-//FIXME for multiple motors
-#ifndef DEBUG
-static
-#endif
-inline unsigned char hal_read(struct pxmc_state *mcs)
-{
- char hal = *DIO_PORTB;
- if (mcs->pxms_out_info==0)
- return (hal&0x14)>>2 | (hal&0x20)>>4;
- else
- return (hal&0x0a)>>1 | (hal&0x01)<<1;
-}
-
-
-int motor_do_output(struct pxmc_state *mcs)
-{
- unsigned short pwm1, pwm2, pwm3;
- short ene;
- int indx = 0;
-
- ene = mcs->pxms_ene;
-
- if (!(mcs->pxms_flg&PXMS_PTI_m))
- {
- short ptindx;
- short ptirc = mcs->pxms_ptirc;
- short divisor = mcs->pxms_ptper * 6;
- unsigned char hal_pos;
-
- hal_pos = eurobot_bdc_hal_pos_table[hal_read(mcs)];
-
- if (hal_pos == 0xff && ene > 0) {
- if (mcs->pxms_halerc >= HAL_ERR_SENSITIVITY*HAL_ERR_MAX_COUNT) {
- pxmc_set_errno(mcs, PXMS_E_HAL);
- mcs->pxms_ene = 0; //FIXME is it correct to stop the motor this way?
- mcs->pxms_halerc--;
- } else
- mcs->pxms_halerc+=HAL_ERR_SENSITIVITY;
- } else {
- if (mcs->pxms_halerc > 0) mcs->pxms_halerc--;
- ptindx = (hal_pos * ptirc + divisor / 2) / divisor;
-
- if (!(mcs->pxms_flg&PXMS_PTI_m)) {
- if(0 && ((hal_pos!=mcs->pxms_hal)&&(mcs->pxms_hal!=0x40))){
- if ((ptindx > mcs->pxms_ptindx+ptirc/2) /* underrun */||
- (mcs->pxms_ptindx>ptindx+ptirc/2) /* overrun */){
- ptindx=(mcs->pxms_ptindx+ptindx-ptirc)/2;
- if (ptindx<0)
- ptindx+=ptirc;
- }else{
- ptindx=(mcs->pxms_ptindx+ptindx)/2;
- }
- mcs->pxms_ptindx=ptindx;
-
- mcs->pxms_ptofs=(mcs->pxms_ap>>PXMC_SUBDIV(mcs))+mcs->pxms_ptshift-ptindx;
-
- pxmc_set_flag(mcs,PXMS_PTI_b);
- }else{
- mcs->pxms_ptindx=ptindx;
- }
- mcs->pxms_hal = hal_pos;
- }
- }
- }
-
- if (ene)
- {
- indx = mcs->pxms_ptindx;
- if (ene < 0) {
- // Generating direction of stator mag. field for backward torque
- ene = -ene;
- indx -= mcs->pxms_ptvang;
- if (indx < 0) indx += mcs->pxms_ptirc;
- } else {
- // Generating direction of stator mag. field for forward torque
- indx += mcs->pxms_ptvang;
- if (indx >= mcs->pxms_ptirc) indx -= mcs->pxms_ptirc;
- }
- }
-
- //indx %= mcs->pxms_ptirc;
- pwm1 = mcs->pxms_ptptr1[indx];
- pwm2 = mcs->pxms_ptptr2[indx];
- pwm3 = mcs->pxms_ptptr3[indx];
-
- /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
- /* Initialized CTM4 PWM period is 0x200 => divide by value about 2097024 */
- pwm1 = (((unsigned long)pwm1*(unsigned int)ene) >> (15+5));
- pwm2 = (((unsigned long)pwm2*(unsigned int)ene) >> (15+5));
- pwm3 = (((unsigned long)pwm3*(unsigned int)ene) >> (15+5));
-
- if (mcs->pxms_out_info==0) {
- /* Don't overwrite second motor PWM values. Wait for next pwm cycle */
- while ((*PWM_PWCR2 & PWCR2_CMFm) == 0);
-
- *PWM_PWBFR2A = pwm1;
- *PWM_PWBFR2B = pwm2;
- *PWM_PWBFR2C = pwm3;
-
- *PWM_PWCR2 &= ~PWCR2_CMFm; /* Clear compare match flag */
- } else {
- /* Don't overwrite second motor PWM values. Wait for next pwm cycle */
- while ((*PWM_PWCR2 & PWCR2_CMFm) == 0);
-
- *PWM_PWBFR2A = PWBFR2A_TDSm | pwm1;
- *PWM_PWBFR2B = PWBFR2B_TDSm | pwm2;
- *PWM_PWBFR2C = PWBFR2C_TDSm | pwm3;
-
- *PWM_PWCR2 &= ~PWCR2_CMFm; /* Clear compare match flag */
- }
-
- //mcs->pxms_ptindx=indx;
-
- return 0;
-}
-
-
-int pxmc_initialize(void)
-{
- init_pwm(); //nastaveni HW (TPU, PWM)
- init_hal();
- init_irc();
- init_sampling();
- return 0;
-};
-
-/* Local Variables: */
-/* c-basic-offset: 2 */
-/* End */
+++ /dev/null
-#ifndef _PXMC_EUROBOT_H
-#define _PXMC_EUROBOT_H
-
-extern pxmc_state_t mcs_right, mcs_left;
-extern volatile unsigned pxmc_msec_counter; // miliseconds counter
-
-extern long alfa; // angle in which robot is turned at the begin
-extern int x,y; // initial position of the robot
-//extern int xl,yl; // initial position of wheels
-//extern int xr,yr; // it is strong correlated with alfa
-
-#define LED_CAN_REC 0
-/* #define LED_INDEX1 0 */
-/* #define LED_INDEX2 1 */
-#define LED_LIVE 2
-#define LED_ERROR 3
-
-extern int pxmc_sfikhz; // Sampling freq. interrupt - how often we want to collect data
-
-#define DEBUG
-#ifdef DEBUG
-int motor_do_output(struct pxmc_state *mcs);
-inline unsigned char hal_read(struct pxmc_state *mcs);
-#endif
-
-#endif
+++ /dev/null
-/* Generated phase table, do not edit */
-
-/* ptirc=2000 ptper=1 ptamp=32767 phasenum=3 combined=0*/
-/* shape="sin3phup" */
-/* ptable_name="bldc" */
-
-#define bldc_ptirc 2000
-#define bldc_ptper 1
-
-const short
-bldc_phase1[]={
-28377,28427,28478,28530,28580,28630,28680,28729,28779,28828,28876,28925,28973,29022,29069,29117,29164,29211,29256,29303,29349,29394,29439,29485,29530,29574,29618,29663,29705,29749,29792,29835,29877,29919,29961,30002,30044,30085,30125,30166,30205,30246,30285,30324,30363,30402,30439,30478,30515,30553,30590,30626,30663,30699,30735,30770,30806,30840,30875,30909,30943,30977,31011,31043,31076,31108,31141,31173,31204,31236,31267,31297,31328,31357,31388,31417,31446,31475,31503,31531,31559,31586,31613,31641,31668,31693,31719,31745,31770,31796,31820,31845,31869,31893,31916,31939,31962,31984,32007,32029,32050,32071,32093,32113,32133,32153,32172,32192,32211,32230,32248,32266,32284,32301,32318,32335,32352,32368,32383,32399,32414,32429,32444,32459,32472,32485,32499,32512,32524,32537,32549,32561,32572,32583,32594,32604,32614,32624,32633,32642,32651,32660,32668,32675,32682,32690,32697,32704,32709,32715,32722,32727,32731,32736,32740,32744,32748,32751,32753,32756,32759,32761,32763,32764,32765,32765,32766,32766,32765,32765,32764,32763,32761,32760,32758,32755,32752,32748,32745,32741,32738,32733,32728,32723,32718,32711,32706,32699,32693,32686,32678,32671,32662,32654,32645,32636,32627,32618,32607,32597,32587,32576,32564,32553,32541,32529,32516,32503,32490,32477,32463,32449,32435,32420,32405,32389,32373,32357,32341,32324,32307,32290,32272,32254,32235,32217,32198,32179,32159,32139,32119,32099,32078,32057,32036,32014,31992,31970,31946,31924,31900,31876,31853,31828,31803,31779,31754,31728,31702,31676,31650,31622,31595,31568,31540,31513,31484,31455,31426,31397,31368,31337,31307,31277,31246,31215,31184,31151,31119,31087,31054,31022,30988,30955,30921,30887,30852,30817,30782,30747,30711,30674,30638,30602,30565,30528,30490,30452,30414,30375,30337,30297,30258,30219,30179,30139,30099,30057,30016,29975,29933,29891,29849,29807,29763,29720,29677,29633,29589,29544,29500,29455,29409,29364,29319,29272,29225,29179,29132,29084,29037,28989,28942,28893,28844,28795,28746,28697,28647,28597,28546,28495,28444,28394,28341,28290,28238,28185,28133,28080,28026,27973,27920,27866,27811,27757,27702,27647,27591,27536,27480,27423,27367,27310,27254,27196,27139,27080,27023,26964,26905,26847,26788,26728,26669,26609,26548,26488,26427,26366,26305,26243,26182,26120,26057,25995,25932,25869,25806,25742,25679,25614,25550,25486,25420,25356,25290,25224,25158,25093,25027,24960,24893,24826,24759,24691,24623,24555,24486,24418,24349,24281,24211,24142,24072,24002,23932,23861,23791,23721,23648,23577,23506,23434,23362,23289,23217,23145,23071,22999,22925,22852,22777,22703,22629,22554,22480,22404,22329,22254,22178,22102,22026,21950,21873,21797,21720,21642,21565,21488,21409,21332,21254,21175,21097,21017,20938,20859,20779,20700,20620,20540,20459,20379,20298,20217,20136,20055,19973,19892,19810,19728,19646,19563,19481,19397,19314,19231,19148,19064,18980,18897,18812,18727,18643,18559,18474,18388,18303,18218,18132,18046,17960,17874,17788,17701,17614,17528,17441,17353,17266,17178,17091,17002,16914,16827,16738,16649,16560,16472,16383,16294,16205,16115,16026,15935,15846,15756,15665,15575,15484,15393,15302,15211,15120,15028,14937,14846,14753,14662,14570,14477,14385,14292,14199,14106,14014,13921,13828,13733,13640,13547,13453,13359,13265,13171,13076,12981,12887,12792,12697,12602,12508,12413,12317,12222,12126,12030,11935,11839,11743,11646,11550,11454,11358,11261,11164,11067,10970,10873,10776,10678,10581,10484,10386,10288,10191,10093,9995,9897,9799,9701,9602,9504,9405,9306,9207,9109,9010,8910,8812,8712,8613,8514,8415,8315,8215,8116,8016,7916,7816,7716,7615,7516,7416,7315,7215,7114,7014,6913,6813,6712,6612,6510,6409,6309,6207,6106,6005,5904,5802,5701,5600,5499,5397,5295,5194,5092,4990,4889,4786,4685,4583,4481,4379,4277,4175,4073,3970,3869,3766,3664,3561,3459,3357,3254,3152,3049,2947,2844,2742,2639,2536,2434,2331,2229,2126,2023,1920,1818,1715,1612,1509,1406,1304,1201,1098,995,892,789,687,584,481,378,274,171,69,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,69,171,274,378,481,584,687,789,892,995,1098,1201,1304,1406,1509,1612,1715,1818,1920,2023,2126,2229,2331,2434,2536,2639,2742,2844,2947,3049,3152,3254,3357,3459,3561,3664,3766,3869,3970,4073,4175,4277,4379,4481,4583,4685,4786,4889,4990,5092,5194,5295,5397,5499,5600,5701,5802,5904,6005,6106,6207,6309,6409,6510,6612,6712,6813,6913,7014,7114,7215,7315,7416,7516,7615,7716,7816,7916,8016,8116,8215,8315,8415,8514,8613,8712,8812,8910,9010,9109,9207,9306,9405,9504,9602,9701,9799,9897,9995,10093,10191,10288,10386,10484,10581,10678,10776,10873,10970,11067,11164,11261,11358,11454,11550,11646,11743,11839,11935,12030,12126,12222,12317,12413,12508,12602,12697,12792,12887,12981,13076,13171,13265,13359,13453,13547,13640,13733,13828,13921,14014,14106,14199,14292,14385,14477,14570,14662,14753,14846,14937,15028,15120,15211,15302,15393,15484,15575,15665,15756,15846,15935,16026,16115,16205,16294,16383,16472,16560,16649,16738,16827,16914,17002,17091,17178,17266,17353,17441,17528,17614,17701,17788,17874,17960,18046,18132,18218,18303,18388,18474,18559,18643,18727,18812,18897,18980,19064,19148,19231,19314,19397,19481,19563,19646,19728,19810,19892,19973,20055,20136,20217,20298,20379,20459,20540,20620,20700,20779,20859,20938,21017,21097,21175,21254,21332,21409,21488,21565,21642,21720,21797,21873,21950,22026,22102,22178,22254,22329,22404,22480,22554,22629,22703,22777,22852,22925,22999,23071,23145,23217,23289,23362,23434,23506,23577,23648,23721,23791,23861,23932,24002,24072,24142,24211,24281,24349,24418,24486,24555,24623,24691,24759,24826,24893,24960,25027,25093,25158,25224,25290,25356,25420,25486,25550,25614,25679,25742,25806,25869,25932,25995,26057,26120,26182,26243,26305,26366,26427,26488,26548,26609,26669,26728,26788,26847,26905,26964,27023,27080,27139,27196,27254,27310,27367,27423,27480,27536,27591,27647,27702,27757,27811,27866,27920,27973,28026,28080,28133,28185,28238,28290,28341,28394,28444,28495,28546,28597,28647,28697,28746,28795,28844,28893,28942,28989,29037,29084,29132,29179,29225,29272,29319,29364,29409,29455,29500,29544,29589,29633,29677,29720,29763,29807,29849,29891,29933,29975,30016,30057,30099,30139,30179,30219,30258,30297,30337,30375,30414,30452,30490,30528,30565,30602,30638,30674,30711,30747,30782,30817,30852,30887,30921,30955,30988,31022,31054,31087,31119,31151,31184,31215,31246,31277,31307,31337,31368,31397,31426,31455,31484,31513,31540,31568,31595,31622,31650,31676,31702,31728,31754,31779,31803,31828,31853,31876,31900,31924,31946,31970,31992,32014,32036,32057,32078,32099,32119,32139,32159,32179,32198,32217,32235,32254,32272,32290,32307,32324,32341,32357,32373,32389,32405,32420,32435,32449,32463,32477,32490,32503,32516,32529,32541,32553,32564,32576,32587,32597,32607,32618,32627,32636,32645,32654,32662,32671,32678,32686,32693,32699,32706,32711,32718,32723,32728,32733,32738,32741,32745,32748,32752,32755,32758,32760,32761,32763,32764,32765,32765,32766,32766,32765,32765,32764,32763,32761,32759,32756,32753,32751,32748,32744,32740,32736,32731,32727,32722,32715,32709,32704,32697,32690,32682,32675,32668,32660,32651,32642,32633,32624,32614,32604,32594,32583,32572,32561,32549,32537,32524,32512,32499,32485,32472,32459,32444,32429,32414,32399,32383,32368,32352,32335,32318,32301,32284,32266,32248,32230,32211,32192,32172,32153,32133,32113,32093,32071,32050,32029,32007,31984,31962,31939,31916,31893,31869,31845,31820,31796,31770,31745,31719,31693,31668,31641,31613,31586,31559,31531,31503,31475,31446,31417,31388,31357,31328,31297,31267,31236,31204,31173,31141,31108,31076,31043,31011,30977,30943,30909,30875,30840,30806,30770,30735,30699,30663,30626,30590,30553,30515,30478,30439,30402,30363,30324,30285,30246,30205,30166,30125,30085,30044,30002,29961,29919,29877,29835,29792,29749,29705,29663,29618,29574,29530,29485,29439,29394,29349,29303,29256,29211,29164,29117,29069,29022,28973,28925,28876,28828,28779,28729,28680,28630,28580,28530,28478,28427};
-
-const short
-bldc_phase2[]={
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,35,137,240,344,446,549,652,754,858,961,1063,1167,1269,1372,1475,1577,1680,1783,1886,1989,2092,2194,2297,2400,2502,2605,2708,2810,2912,3015,3118,3220,3323,3425,3527,3630,3732,3835,3936,4039,4141,4243,4345,4446,4549,4651,4753,4854,4956,5058,5159,5261,5363,5464,5566,5668,5769,5871,5972,6073,6174,6275,6376,6476,6578,6678,6779,6880,6981,7080,7181,7282,7382,7483,7583,7682,7783,7883,7983,8082,8182,8281,8381,8481,8581,8679,8779,8878,8977,9075,9174,9274,9372,9471,9569,9667,9766,9864,9962,10060,10159,10256,10354,10451,10548,10646,10743,10841,10938,11034,11132,11228,11325,11422,11518,11615,11711,11806,11903,11999,12094,12190,12285,12381,12476,12571,12666,12761,12856,12951,13045,13139,13234,13328,13421,13515,13609,13703,13796,13889,13982,14075,14169,14261,14354,14447,14539,14631,14723,14815,14906,14999,15090,15181,15272,15363,15453,15545,15635,15726,15816,15906,15996,16085,16175,16264,16354,16442,16531,16620,16708,16797,16885,16974,17061,17149,17237,17324,17412,17498,17585,17672,17759,17846,17931,18017,18104,18189,18274,18360,18445,18530,18615,18700,18784,18868,18952,19036,19119,19204,19287,19370,19453,19536,19618,19700,19783,19865,19946,20028,20110,20190,20271,20352,20433,20513,20593,20673,20753,20832,20912,20992,21070,21149,21227,21306,21384,21461,21540,21617,21694,21771,21848,21925,22001,22077,22153,22229,22304,22380,22455,22530,22604,22678,22753,22826,22900,22974,23047,23121,23193,23265,23338,23410,23482,23553,23625,23697,23768,23838,23908,23979,24049,24119,24188,24257,24327,24395,24464,24532,24600,24669,24736,24803,24870,24937,25004,25071,25137,25203,25268,25334,25399,25464,25529,25593,25657,25721,25785,25848,25911,25974,26037,26099,26161,26223,26285,26346,26407,26468,26528,26588,26649,26709,26768,26827,26886,26944,27003,27062,27119,27176,27234,27291,27348,27404,27461,27517,27573,27628,27684,27738,27793,27847,27902,27956,28009,28062,28115,28168,28221,28272,28324,28377,28427,28478,28530,28580,28630,28680,28729,28779,28828,28876,28925,28973,29022,29069,29117,29164,29211,29256,29303,29349,29394,29439,29485,29530,29574,29618,29663,29705,29749,29792,29835,29877,29919,29961,30002,30044,30085,30125,30166,30205,30246,30285,30324,30363,30402,30439,30478,30515,30553,30590,30626,30663,30699,30735,30770,30806,30840,30875,30909,30943,30977,31011,31043,31076,31108,31141,31173,31204,31236,31267,31297,31328,31357,31388,31417,31446,31475,31503,31531,31559,31586,31613,31641,31668,31693,31719,31745,31770,31796,31820,31845,31869,31893,31916,31939,31962,31984,32007,32029,32050,32071,32093,32113,32133,32153,32172,32192,32211,32230,32248,32266,32284,32301,32318,32335,32352,32368,32383,32399,32414,32429,32444,32459,32472,32485,32499,32512,32524,32537,32549,32561,32572,32583,32594,32604,32614,32624,32633,32642,32651,32660,32668,32675,32682,32690,32697,32704,32709,32715,32722,32727,32731,32736,32740,32744,32748,32751,32753,32756,32759,32761,32763,32764,32765,32765,32766,32766,32765,32765,32764,32763,32761,32760,32758,32755,32752,32748,32745,32741,32738,32733,32728,32723,32718,32711,32706,32699,32693,32686,32678,32671,32662,32654,32645,32636,32627,32618,32607,32597,32587,32576,32564,32553,32541,32529,32516,32503,32490,32477,32463,32449,32435,32420,32405,32389,32373,32357,32341,32324,32307,32290,32272,32254,32236,32217,32198,32179,32159,32139,32119,32099,32078,32057,32036,32014,31992,31970,31946,31924,31900,31876,31853,31828,31803,31779,31754,31728,31702,31676,31650,31622,31595,31568,31540,31513,31484,31455,31426,31397,31368,31337,31307,31277,31246,31215,31184,31151,31119,31087,31054,31022,30988,30955,30921,30887,30852,30817,30782,30747,30711,30674,30638,30602,30565,30528,30490,30452,30414,30375,30337,30297,30258,30219,30179,30139,30099,30057,30016,29975,29933,29891,29849,29807,29763,29720,29677,29633,29589,29544,29500,29455,29409,29364,29319,29272,29225,29179,29132,29084,29037,28989,28942,28893,28844,28795,28746,28697,28647,28597,28546,28495,28444,28394,28410,28461,28512,28563,28614,28664,28713,28762,28812,28861,28909,28958,29006,29053,29100,29148,29195,29241,29287,29333,29380,29425,29470,29514,29559,29603,29647,29691,29735,29777,29821,29863,29905,29947,29988,30030,30071,30112,30152,30193,30232,30272,30311,30350,30389,30427,30465,30503,30540,30578,30614,30651,30687,30723,30758,30794,30829,30864,30898,30932,30966,31000,31032,31065,31098,31130,31162,31194,31225,31256,31287,31317,31348,31377,31406,31437,31464,31493,31522,31550,31577,31604,31632,31659,31684,31711,31737,31762,31787,31812,31836,31860,31885,31908,31931,31955,31977,31999,32021,32043,32064,32085,32106,32126,32146,32166,32185,32205,32224,32242,32261,32278,32296,32313,32329,32346,32363,32379,32394,32409,32424,32439,32453,32468,32481,32494,32507,32520,32533,32544,32557,32568,32579,32590,32601,32611,32620,32630,32640,32648,32657,32665,32673,32680,32688,32695,32702,32708,32713,32720,32725,32729,32734,32739,32743,32746,32750,32753,32756,32758,32760,32762,32764,32764,32765,32766,32766,32766,32765,32764,32764,32762,32760,32758,32756,32753,32750,32746,32743,32739,32734,32729,32725,32720,32713,32708,32702,32695,32688,32680,32673,32665,32657,32648,32640,32630,32620,32611,32601,32590,32579,32568,32557,32544,32533,32520,32507,32494,32481,32468,32453,32439,32424,32409,32394,32379,32363,32346,32329,32313,32296,32278,32261,32242,32224,32205,32185,32166,32146,32126,32106,32085,32064,32043,32021,31999,31977,31955,31931,31908,31885,31860,31836,31812,31787,31762,31737,31711,31684,31659,31632,31604,31577,31550,31522,31493,31464,31437,31406,31377,31348,31317,31287,31256,31225,31194,31162,31130,31098,31065,31032,31000,30966,30932,30898,30864,30829,30794,30758,30723,30687,30651,30614,30578,30540,30503,30465,30427,30389,30350,30311,30272,30232,30193,30152,30112,30071,30030,29988,29947,29905,29863,29821,29777,29735,29691,29647,29603,29559,29514,29470,29425,29380,29333,29287,29241,29195,29148,29100,29053,29006,28958,28909,28861,28812,28762,28713,28664,28614,28563,28512,28461,28410,28359,28307,28255,28202,28151,28098,28045,27992,27937,27883,27830,27775,27720,27665,27609,27555,27499,27442,27386,27330,27272,27215,27158,27100,27042,26984,26925,26867,26808,26748,26689,26629,26568,26508,26448,26386,26325,26264,26203,26140,26078,26015,25952,25891,25826,25763,25699,25636,25572,25507,25443,25377,25311,25247,25181,25114,25048,24981,24915,24848,24781,24713,24646,24578,24509,24441,24372,24304,24234,24166,24096,24025,23955,23885,23814,23744,23672,23601,23530,23458,23386,23314,23241,23169,23095,23023,22949,22876,22802,22728,22654,22579,22504,22429,22355,22279,22204,22128,22052,21976,21898,21822,21745,21668,21591,21513,21436,21357,21279,21201,21122,21044,20965,20885,20806,20726,20647,20567,20486,20406,20326,20245,20164,20082,20001,19919,19837,19755,19673,19590,19508,19425,19342,19259,19176,19092,19009,18924,18840,18756,18672,18587,18502,18417,18332,18246,18160,18075,17988,17903,17817,17729,17643,17557,17469,17382,17295,17207,17120,17032,16944,16855,16767,16679,16590,16501,16412,16324,16234,16145,16056,15966,15876,15785,15695,15604,15514,15424,15332,15242,15151,15059,14968,14876,14784,14692,14600,14508,14416,14322,14230,14138,14045,13951,13858,13765,13672,13578,13485,13390,13296,13202,13108,13013,12919,12824,12729,12634,12539,12444,12349,12254,12158,12062,11966,11870,11775,11679,11582,11486,11389,11292,11196,11099,11003,10905,10808,10711,10613,10516,10419,10322,10223,10125,10028,9930,9831,9733,9635,9536,9438,9339,9241,9142,9043,8944,8845,8745,8646,8547,8448,8348,8248,8149,8049,7950,7850,7750,7649,7550,7449,7349,7248,7148,7048,6947,6846,6746,6644,6544,6443,6342,6241,6140,6039,5938,5836,5735,5633,5532,5431,5329,5227,5126,5024,4922,4821,4718,4617,4515,4413,4311,4209,4106,4005,3902,3800,3698,3595,3493,3391,3289,3186,3083,2981,2878,2777,2674,2571,2468,2366,2263,2160,2058,1955,1852,1750,1647,1544,1441,1338,1235,1132,1029,926,823,720,618,515,412,309,206,103};
-
-const short
-bldc_phase3[]={
-0,103,206,309,412,515,618,720,823,926,1029,1132,1235,1338,1441,1544,1647,1750,1852,1955,2058,2160,2263,2366,2468,2571,2674,2777,2878,2981,3083,3186,3289,3391,3493,3595,3698,3800,3902,4005,4106,4209,4311,4413,4515,4617,4718,4821,4922,5024,5126,5227,5329,5431,5532,5633,5735,5836,5938,6039,6140,6241,6342,6443,6544,6644,6746,6846,6947,7048,7148,7248,7349,7449,7550,7649,7749,7850,7950,8049,8149,8248,8348,8448,8547,8646,8745,8845,8944,9043,9142,9241,9339,9438,9536,9635,9733,9831,9930,10028,10125,10223,10322,10419,10516,10613,10711,10808,10905,11003,11099,11196,11292,11389,11486,11582,11679,11775,11870,11966,12062,12158,12254,12349,12444,12539,12634,12729,12824,12919,13013,13108,13202,13296,13390,13485,13578,13672,13765,13858,13951,14045,14138,14230,14322,14416,14508,14600,14692,14784,14876,14968,15059,15151,15242,15332,15424,15514,15604,15695,15785,15876,15966,16056,16145,16234,16324,16412,16501,16590,16679,16767,16855,16944,17032,17120,17207,17295,17382,17469,17557,17643,17729,17817,17903,17988,18075,18160,18246,18332,18417,18502,18587,18672,18756,18840,18924,19009,19092,19176,19259,19342,19425,19508,19590,19673,19755,19837,19919,20001,20082,20164,20245,20326,20406,20486,20567,20646,20726,20806,20885,20965,21044,21122,21201,21279,21357,21436,21513,21591,21668,21745,21822,21898,21976,22052,22128,22204,22279,22355,22429,22504,22579,22654,22728,22802,22876,22949,23023,23095,23169,23241,23314,23386,23458,23530,23601,23672,23744,23814,23885,23955,24025,24096,24166,24234,24304,24372,24441,24509,24578,24646,24713,24781,24848,24915,24981,25048,25114,25181,25247,25311,25377,25443,25507,25572,25636,25699,25763,25826,25891,25952,26015,26078,26140,26203,26264,26325,26386,26448,26508,26568,26629,26689,26748,26808,26867,26925,26984,27042,27100,27158,27215,27272,27330,27386,27442,27499,27555,27609,27665,27720,27775,27830,27883,27937,27992,28045,28098,28151,28202,28255,28307,28359,28410,28461,28512,28563,28614,28664,28713,28762,28812,28861,28909,28958,29006,29053,29100,29148,29195,29241,29287,29333,29380,29425,29470,29514,29559,29603,29647,29691,29735,29777,29821,29863,29905,29947,29988,30030,30071,30112,30152,30193,30232,30272,30311,30350,30389,30427,30465,30503,30540,30578,30614,30651,30687,30723,30758,30794,30829,30864,30898,30932,30966,31000,31032,31065,31098,31130,31162,31194,31225,31256,31287,31317,31348,31377,31406,31437,31464,31493,31522,31550,31577,31604,31632,31659,31684,31711,31737,31762,31787,31812,31836,31860,31885,31908,31931,31955,31977,31999,32021,32043,32064,32085,32106,32126,32146,32166,32185,32205,32224,32242,32261,32278,32296,32313,32329,32346,32363,32379,32394,32409,32424,32439,32453,32468,32481,32494,32507,32520,32533,32544,32557,32568,32579,32590,32601,32611,32620,32630,32640,32648,32657,32665,32673,32680,32688,32695,32702,32708,32713,32720,32725,32729,32734,32739,32743,32746,32750,32753,32756,32758,32760,32762,32764,32764,32765,32766,32766,32766,32765,32764,32764,32762,32760,32758,32756,32753,32750,32746,32743,32739,32734,32729,32725,32720,32713,32708,32702,32695,32688,32680,32673,32665,32657,32648,32640,32630,32620,32611,32601,32590,32579,32568,32557,32544,32533,32520,32507,32494,32481,32468,32453,32439,32424,32409,32394,32379,32363,32346,32329,32313,32296,32278,32261,32242,32224,32205,32185,32166,32146,32126,32106,32085,32064,32043,32021,31999,31977,31955,31931,31908,31885,31860,31836,31812,31787,31762,31737,31711,31684,31659,31632,31604,31577,31550,31522,31493,31464,31437,31406,31377,31348,31317,31287,31256,31225,31194,31162,31130,31098,31065,31032,31000,30966,30932,30898,30864,30829,30794,30758,30723,30687,30651,30614,30578,30540,30503,30465,30427,30389,30350,30311,30272,30232,30193,30152,30112,30071,30030,29988,29947,29905,29863,29821,29777,29735,29691,29647,29603,29559,29514,29470,29425,29380,29333,29287,29241,29195,29148,29100,29053,29006,28958,28909,28861,28812,28762,28713,28664,28614,28563,28512,28461,28410,28394,28444,28495,28546,28597,28647,28697,28746,28795,28844,28893,28942,28989,29037,29084,29132,29179,29225,29272,29319,29364,29409,29455,29500,29544,29589,29633,29677,29720,29763,29807,29849,29891,29933,29975,30016,30057,30099,30139,30179,30219,30258,30297,30337,30375,30414,30452,30490,30528,30565,30602,30638,30674,30711,30747,30782,30817,30852,30887,30921,30955,30988,31022,31054,31087,31119,31151,31184,31215,31246,31277,31307,31337,31368,31397,31426,31455,31484,31513,31540,31568,31595,31622,31650,31676,31702,31728,31754,31779,31803,31828,31853,31876,31900,31924,31946,31970,31992,32014,32036,32057,32078,32099,32119,32139,32159,32179,32198,32217,32235,32254,32272,32290,32307,32324,32341,32357,32373,32389,32405,32420,32435,32449,32463,32477,32490,32503,32516,32529,32541,32553,32564,32576,32587,32597,32607,32618,32627,32636,32645,32654,32662,32671,32678,32686,32693,32699,32706,32711,32718,32723,32728,32733,32738,32741,32745,32748,32752,32755,32758,32760,32761,32763,32764,32765,32765,32766,32766,32765,32765,32764,32763,32761,32759,32756,32753,32751,32748,32744,32740,32736,32731,32727,32722,32715,32709,32704,32697,32690,32682,32675,32668,32660,32651,32642,32633,32624,32614,32604,32594,32583,32572,32561,32549,32537,32524,32512,32499,32485,32472,32459,32444,32429,32414,32399,32383,32368,32352,32335,32318,32301,32284,32266,32248,32230,32211,32192,32172,32153,32133,32113,32093,32071,32050,32029,32007,31984,31962,31939,31916,31893,31869,31845,31820,31796,31770,31745,31719,31693,31668,31641,31613,31586,31559,31531,31503,31475,31446,31417,31388,31357,31328,31297,31267,31236,31204,31173,31141,31108,31076,31043,31011,30977,30943,30909,30875,30840,30806,30770,30735,30699,30663,30626,30590,30553,30515,30478,30439,30402,30363,30324,30285,30246,30205,30166,30125,30085,30044,30002,29961,29919,29877,29835,29792,29749,29705,29663,29618,29574,29530,29485,29439,29394,29349,29303,29256,29211,29164,29117,29069,29022,28973,28925,28876,28828,28779,28729,28680,28630,28580,28530,28478,28427,28377,28324,28272,28221,28168,28115,28062,28009,27956,27902,27847,27793,27738,27684,27628,27573,27517,27461,27404,27348,27291,27234,27176,27119,27062,27003,26944,26886,26827,26768,26709,26649,26588,26528,26468,26407,26346,26285,26223,26161,26099,26037,25974,25911,25848,25785,25721,25657,25593,25529,25464,25399,25334,25268,25203,25137,25071,25004,24937,24870,24803,24736,24669,24600,24532,24464,24395,24327,24257,24188,24119,24049,23979,23908,23838,23768,23697,23625,23553,23482,23410,23338,23265,23193,23121,23047,22974,22900,22826,22753,22678,22604,22530,22455,22380,22304,22229,22153,22077,22001,21925,21848,21771,21694,21617,21540,21461,21384,21306,21227,21149,21070,20992,20912,20832,20753,20673,20593,20513,20433,20352,20271,20190,20110,20028,19946,19865,19783,19700,19618,19536,19453,19370,19287,19204,19119,19036,18952,18868,18784,18700,18615,18530,18445,18360,18274,18189,18104,18017,17931,17846,17759,17672,17585,17498,17412,17324,17237,17149,17061,16974,16885,16797,16708,16620,16531,16442,16354,16264,16175,16085,15996,15906,15816,15726,15635,15545,15453,15363,15272,15181,15090,14999,14906,14815,14722,14631,14539,14447,14354,14261,14169,14075,13982,13889,13796,13703,13609,13515,13421,13328,13234,13139,13045,12951,12856,12761,12666,12571,12476,12381,12285,12190,12094,11999,11903,11806,11711,11615,11518,11422,11325,11228,11132,11034,10938,10841,10743,10646,10548,10451,10354,10256,10159,10060,9962,9864,9766,9667,9569,9471,9372,9274,9174,9075,8977,8878,8779,8679,8581,8481,8381,8281,8182,8082,7983,7883,7783,7682,7583,7483,7382,7282,7181,7080,6981,6880,6779,6678,6578,6476,6376,6275,6174,6073,5972,5871,5769,5668,5566,5464,5363,5261,5159,5058,4956,4854,4753,4651,4549,4446,4345,4243,4141,4039,3936,3835,3732,3630,3527,3425,3323,3220,3118,3015,2912,2810,2708,2605,2502,2400,2297,2194,2092,1989,1886,1783,1680,1577,1475,1372,1269,1167,1063,961,858,754,652,549,446,344,240,137,35,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
-
+++ /dev/null
-/* Generated phase table, do not edit */
-
-/* ptirc=4000 ptper=1 ptamp=32767 phasenum=3 combined=0*/
-/* shape="sin3phup" */
-/* ptable_name="bldc" */
-
-#define bldc_ptirc 4000
-#define bldc_ptper 1
-
-const short
-bldc_phase1[]={
-28377,28402,28427,28453,28478,28504,28530,28554,28580,28605,28630,28655,28680,28705,28729,28754,28779,28804,28828,28852,28876,28901,28925,28949,28973,28997,29022,29046,29069,29093,29117,29140,29164,29187,29211,29234,29256,29280,29303,29326,29349,29371,29394,29417,29439,29462,29485,29507,29530,29552,29574,29596,29618,29640,29663,29684,29705,29728,29749,29770,29792,29814,29835,29856,29877,29898,29919,29940,29961,29982,30002,30023,30044,30064,30085,30105,30125,30145,30166,30186,30205,30226,30246,30265,30285,30304,30324,30343,30363,30382,30402,30421,30439,30459,30478,30496,30515,30534,30553,30571,30590,30608,30626,30644,30663,30681,30699,30717,30735,30753,30770,30788,30806,30823,30840,30858,30875,30892,30909,30926,30943,30960,30977,30994,31011,31027,31043,31060,31076,31092,31108,31125,31141,31157,31173,31189,31204,31220,31236,31251,31267,31282,31297,31312,31328,31343,31357,31372,31388,31402,31417,31431,31446,31460,31475,31489,31503,31517,31531,31546,31559,31573,31586,31600,31613,31628,31641,31654,31668,31681,31693,31706,31719,31732,31745,31758,31770,31783,31796,31808,31820,31832,31845,31856,31869,31880,31893,31904,31916,31928,31939,31950,31962,31973,31984,31995,32007,32018,32029,32039,32050,32060,32071,32082,32093,32102,32113,32123,32133,32143,32153,32163,32172,32183,32192,32202,32211,32220,32230,32239,32248,32257,32266,32275,32284,32292,32301,32310,32318,32327,32335,32344,32352,32360,32368,32376,32383,32392,32399,32407,32414,32422,32429,32437,32444,32451,32459,32465,32472,32479,32485,32492,32499,32505,32512,32518,32524,32531,32537,32542,32549,32555,32561,32566,32572,32577,32583,32588,32594,32599,32604,32609,32614,32619,32624,32629,32633,32638,32642,32647,32651,32655,32660,32664,32668,32671,32675,32679,32682,32686,32690,32693,32697,32700,32704,32707,32709,32713,32715,32718,32722,32724,32727,32729,32731,32734,32736,32738,32740,32742,32744,32746,32748,32750,32751,32753,32753,32755,32756,32758,32759,32760,32761,32761,32763,32763,32764,32764,32765,32765,32765,32766,32766,32766,32766,32766,32765,32765,32765,32765,32764,32764,32763,32762,32761,32761,32760,32758,32758,32756,32755,32753,32752,32750,32748,32747,32745,32743,32741,32739,32738,32735,32733,32731,32728,32725,32723,32720,32718,32715,32711,32709,32706,32702,32699,32696,32693,32689,32686,32682,32678,32675,32671,32666,32662,32658,32654,32649,32645,32640,32636,32631,32627,32622,32618,32612,32607,32603,32597,32592,32587,32581,32576,32570,32564,32559,32553,32547,32541,32535,32529,32522,32516,32510,32503,32496,32490,32483,32477,32470,32463,32455,32449,32442,32435,32427,32420,32412,32405,32396,32389,32381,32373,32365,32357,32349,32341,32333,32324,32316,32307,32298,32290,32281,32272,32263,32254,32245,32235,32226,32217,32208,32198,32189,32179,32169,32159,32150,32139,32130,32119,32109,32099,32089,32078,32067,32057,32047,32036,32025,32014,32003,31992,31981,31970,31958,31946,31935,31924,31911,31900,31889,31876,31865,31853,31840,31828,31816,31803,31792,31779,31766,31754,31741,31728,31715,31702,31689,31676,31663,31650,31637,31622,31609,31595,31582,31568,31555,31540,31526,31513,31498,31484,31470,31455,31441,31426,31412,31397,31382,31368,31352,31337,31323,31307,31292,31277,31261,31246,31231,31215,31200,31184,31167,31151,31135,31119,31103,31087,31070,31054,31038,31022,31005,30988,30971,30955,30937,30921,30903,30887,30869,30852,30835,30817,30799,30782,30765,30747,30729,30711,30693,30674,30656,30638,30620,30602,30583,30565,30546,30528,30509,30490,30471,30452,30434,30414,30395,30375,30356,30337,30318,30297,30278,30258,30239,30219,30199,30179,30159,30139,30118,30099,30078,30057,30037,30016,29995,29975,29954,29933,29912,29891,29870,29849,29828,29807,29785,29763,29742,29720,29698,29677,29654,29633,29611,29589,29567,29544,29523,29500,29477,29455,29432,29409,29387,29364,29341,29319,29295,29272,29249,29225,29202,29179,29155,29132,29108,29084,29061,29037,29013,28989,28965,28942,28918,28893,28869,28844,28819,28795,28771,28746,28721,28697,28671,28647,28621,28597,28571,28546,28521,28495,28470,28444,28418,28394,28368,28341,28315,28290,28264,28238,28212,28185,28159,28133,28106,28080,28053,28026,28000,27973,27946,27920,27893,27866,27839,27811,27784,27757,27730,27702,27674,27647,27619,27591,27563,27536,27508,27480,27452,27423,27396,27367,27338,27310,27282,27254,27225,27196,27168,27139,27109,27080,27052,27023,26993,26964,26935,26905,26877,26847,26818,26788,26758,26728,26699,26669,26639,26609,26578,26548,26518,26488,26458,26427,26396,26366,26336,26305,26274,26243,26213,26182,26151,26120,26088,26057,26027,25995,25964,25932,25901,25869,25838,25806,25774,25742,25710,25679,25646,25614,25582,25550,25517,25486,25453,25420,25388,25356,25323,25290,25257,25224,25191,25158,25126,25093,25060,25027,24993,24960,24927,24893,24860,24826,24793,24759,24724,24691,24657,24623,24589,24555,24521,24486,24452,24418,24384,24349,24315,24281,24246,24211,24177,24142,24107,24072,24038,24002,23967,23932,23897,23861,23826,23791,23756,23721,23685,23648,23613,23577,23542,23506,23470,23434,23398,23362,23326,23289,23254,23217,23180,23145,23108,23071,23035,22999,22962,22925,22888,22852,22814,22777,22740,22703,22666,22629,22592,22554,22517,22480,22442,22404,22367,22329,22291,22254,22216,22178,22140,22102,22064,22026,21988,21950,21912,21873,21835,21797,21758,21720,21681,21642,21604,21565,21526,21488,21449,21409,21370,21332,21293,21254,21214,21175,21136,21097,21057,21017,20978,20938,20898,20859,20820,20779,20740,20700,20660,20620,20580,20540,20500,20459,20419,20379,20338,20298,20257,20217,20177,20136,20096,20055,20014,19973,19933,19892,19851,19810,19769,19728,19687,19646,19604,19563,19522,19481,19439,19397,19356,19314,19273,19231,19190,19148,19106,19064,19022,18980,18938,18897,18854,18812,18770,18727,18686,18643,18600,18559,18516,18474,18431,18388,18346,18303,18260,18218,18175,18132,18089,18046,18003,17960,17917,17874,17831,17788,17744,17701,17658,17614,17571,17528,17484,17441,17397,17353,17309,17266,17222,17178,17135,17091,17046,17002,16959,16914,16870,16827,16782,16738,16694,16649,16605,16560,16516,16472,16427,16383,16339,16294,16249,16205,16160,16115,16070,16026,15981,15935,15891,15846,15800,15756,15711,15665,15620,15575,15529,15484,15439,15393,15348,15302,15257,15211,15165,15120,15074,15028,14983,14937,14891,14846,14799,14753,14708,14662,14615,14570,14523,14477,14431,14385,14338,14292,14246,14199,14153,14106,14060,14014,13967,13921,13874,13828,13781,13733,13687,13640,13594,13547,13499,13453,13406,13359,13312,13265,13218,13171,13123,13076,13029,12981,12935,12887,12840,12792,12745,12697,12650,12602,12555,12508,12460,12413,12365,12317,12269,12222,12174,12126,12079,12030,11983,11935,11887,11839,11790,11743,11695,11646,11598,11550,11502,11454,11406,11358,11309,11261,11213,11164,11115,11067,11019,10970,10921,10873,10825,10776,10727,10678,10630,10581,10533,10484,10436,10386,10337,10288,10240,10191,10142,10093,10044,9995,9946,9897,9848,9799,9750,9701,9652,9602,9553,9504,9454,9405,9356,9306,9257,9207,9159,9109,9059,9010,8960,8910,8862,8812,8762,8712,8662,8613,8564,8514,8465,8415,8364,8315,8265,8215,8166,8116,8065,8016,7966,7916,7866,7816,7766,7716,7666,7615,7566,7516,7466,7416,7365,7315,7265,7215,7165,7114,7064,7014,6963,6913,6863,6813,6762,6712,6662,6612,6560,6510,6460,6409,6359,6309,6258,6207,6156,6106,6055,6005,5954,5904,5853,5802,5752,5701,5650,5600,5549,5499,5448,5397,5346,5295,5245,5194,5143,5092,5041,4990,4939,4889,4838,4786,4736,4685,4633,4583,4532,4481,4430,4379,4328,4277,4225,4175,4124,4073,4022,3970,3919,3869,3818,3766,3715,3664,3613,3561,3510,3459,3408,3357,3306,3254,3203,3152,3100,3049,2998,2947,2895,2844,2793,2742,2691,2639,2588,2536,2485,2434,2383,2331,2280,2229,2177,2126,2075,2023,1972,1920,1869,1818,1766,1715,1663,1612,1561,1509,1458,1406,1355,1304,1252,1201,1149,1098,1046,995,943,892,841,789,738,687,635,584,532,481,429,378,326,274,223,171,121,69,17,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,17,69,121,171,223,274,326,378,429,481,532,584,635,687,738,789,841,892,943,995,1046,1098,1149,1201,1252,1304,1355,1406,1458,1509,1561,1612,1663,1715,1766,1818,1869,1920,1972,2023,2075,2126,2177,2229,2280,2331,2383,2434,2485,2536,2588,2639,2691,2742,2793,2844,2895,2947,2998,3049,3100,3152,3203,3254,3306,3357,3408,3459,3510,3561,3613,3664,3715,3766,3818,3869,3919,3970,4022,4073,4124,4175,4225,4277,4328,4379,4430,4481,4532,4583,4633,4685,4736,4786,4838,4889,4939,4990,5041,5092,5143,5194,5245,5295,5346,5397,5448,5499,5549,5600,5650,5701,5752,5802,5853,5904,5954,6005,6055,6106,6156,6207,6258,6309,6359,6409,6460,6510,6560,6612,6662,6712,6762,6813,6863,6913,6963,7014,7064,7114,7165,7215,7265,7315,7365,7416,7466,7516,7566,7615,7666,7716,7766,7816,7866,7916,7966,8016,8065,8116,8166,8215,8265,8315,8364,8415,8465,8514,8564,8613,8662,8712,8762,8812,8862,8910,8960,9010,9059,9109,9159,9207,9257,9306,9356,9405,9454,9504,9553,9602,9652,9701,9750,9799,9848,9897,9946,9995,10044,10093,10142,10191,10240,10288,10337,10386,10436,10484,10533,10581,10630,10678,10727,10776,10825,10873,10921,10970,11019,11067,11115,11164,11213,11261,11309,11358,11406,11454,11502,11550,11598,11646,11695,11743,11790,11839,11887,11935,11983,12030,12079,12126,12174,12222,12269,12317,12365,12413,12460,12508,12555,12602,12650,12697,12745,12792,12840,12887,12935,12981,13029,13076,13123,13171,13218,13265,13312,13359,13406,13453,13499,13547,13594,13640,13687,13733,13781,13828,13874,13921,13967,14014,14060,14106,14153,14199,14246,14292,14338,14385,14431,14477,14523,14570,14615,14662,14708,14753,14799,14846,14891,14937,14983,15028,15074,15120,15165,15211,15257,15302,15348,15393,15439,15484,15529,15575,15620,15665,15711,15756,15800,15846,15891,15935,15981,16026,16070,16115,16160,16205,16249,16294,16339,16383,16427,16472,16516,16560,16605,16649,16694,16738,16782,16827,16870,16914,16959,17002,17046,17091,17135,17178,17222,17266,17309,17353,17397,17441,17484,17528,17571,17614,17658,17701,17744,17788,17831,17874,17917,17960,18003,18046,18089,18132,18175,18218,18260,18303,18346,18388,18431,18474,18516,18559,18600,18643,18686,18727,18770,18812,18854,18897,18938,18980,19022,19064,19106,19148,19190,19231,19273,19314,19356,19397,19439,19481,19522,19563,19604,19646,19687,19728,19769,19810,19851,19892,19933,19973,20014,20055,20096,20136,20177,20217,20257,20298,20338,20379,20419,20459,20500,20540,20580,20620,20660,20700,20740,20779,20820,20859,20898,20938,20978,21017,21057,21097,21136,21175,21214,21254,21293,21332,21370,21409,21449,21488,21526,21565,21604,21642,21681,21720,21758,21797,21835,21873,21912,21950,21988,22026,22064,22102,22140,22178,22216,22254,22291,22329,22367,22404,22442,22480,22517,22554,22592,22629,22666,22703,22740,22777,22814,22852,22888,22925,22962,22999,23035,23071,23108,23145,23180,23217,23254,23289,23326,23362,23398,23434,23470,23506,23542,23577,23613,23648,23685,23721,23756,23791,23826,23861,23897,23932,23967,24002,24038,24072,24107,24142,24177,24211,24246,24281,24315,24349,24384,24418,24452,24486,24521,24555,24589,24623,24657,24691,24724,24759,24793,24826,24860,24893,24927,24960,24993,25027,25060,25093,25126,25158,25191,25224,25257,25290,25323,25356,25388,25420,25453,25486,25517,25550,25582,25614,25646,25679,25710,25742,25774,25806,25838,25869,25901,25932,25964,25995,26027,26057,26088,26120,26151,26182,26213,26243,26274,26305,26336,26366,26396,26427,26458,26488,26518,26548,26578,26609,26639,26669,26699,26728,26758,26788,26818,26847,26877,26905,26935,26964,26993,27023,27052,27080,27109,27139,27168,27196,27225,27254,27282,27310,27338,27367,27396,27423,27452,27480,27508,27536,27563,27591,27619,27647,27674,27702,27730,27757,27784,27811,27839,27866,27893,27920,27946,27973,28000,28026,28053,28080,28106,28133,28159,28185,28212,28238,28264,28290,28315,28341,28368,28394,28418,28444,28470,28495,28521,28546,28571,28597,28621,28647,28671,28697,28721,28746,28771,28795,28819,28844,28869,28893,28918,28942,28965,28989,29013,29037,29061,29084,29108,29132,29155,29179,29202,29225,29249,29272,29295,29319,29341,29364,29387,29409,29432,29455,29477,29500,29523,29544,29567,29589,29611,29633,29654,29677,29698,29720,29742,29763,29785,29807,29828,29849,29870,29891,29912,29933,29954,29975,29995,30016,30037,30057,30078,30099,30118,30139,30159,30179,30199,30219,30239,30258,30278,30297,30318,30337,30356,30375,30395,30414,30434,30452,30471,30490,30509,30528,30546,30565,30583,30602,30620,30638,30656,30674,30693,30711,30729,30747,30765,30782,30799,30817,30835,30852,30869,30887,30903,30921,30937,30955,30971,30988,31005,31022,31038,31054,31071,31087,31103,31119,31135,31151,31167,31184,31200,31215,31231,31246,31261,31277,31292,31307,31323,31337,31352,31368,31382,31397,31412,31426,31441,31455,31470,31484,31498,31513,31526,31540,31555,31568,31582,31595,31609,31622,31637,31650,31663,31676,31689,31702,31715,31728,31741,31754,31766,31779,31792,31803,31816,31828,31840,31853,31865,31876,31889,31900,31911,31924,31935,31946,31958,31970,31981,31992,32003,32014,32025,32036,32047,32057,32067,32078,32089,32099,32109,32119,32130,32139,32150,32159,32169,32179,32189,32198,32208,32217,32226,32235,32245,32254,32263,32272,32281,32290,32298,32307,32316,32324,32333,32341,32349,32357,32365,32373,32381,32389,32396,32405,32412,32420,32427,32435,32442,32449,32455,32463,32470,32477,32483,32490,32496,32503,32509,32516,32522,32529,32535,32541,32547,32553,32559,32564,32570,32576,32581,32587,32592,32597,32603,32607,32612,32618,32622,32627,32631,32636,32640,32645,32649,32654,32658,32662,32666,32671,32675,32678,32682,32686,32689,32693,32696,32699,32702,32706,32709,32711,32715,32718,32720,32723,32725,32728,32731,32733,32735,32738,32739,32741,32743,32745,32747,32748,32750,32752,32753,32755,32756,32758,32758,32760,32761,32761,32762,32763,32764,32764,32765,32765,32765,32765,32766,32766,32766,32766,32766,32765,32765,32765,32764,32764,32763,32763,32761,32761,32760,32759,32758,32756,32755,32753,32753,32751,32750,32748,32746,32744,32742,32740,32738,32736,32734,32731,32729,32727,32724,32722,32718,32715,32713,32709,32707,32704,32700,32697,32693,32690,32686,32682,32679,32675,32671,32668,32664,32660,32655,32651,32647,32642,32638,32633,32629,32624,32619,32614,32609,32604,32599,32594,32588,32583,32577,32572,32566,32561,32555,32549,32542,32537,32531,32524,32518,32512,32505,32499,32492,32485,32479,32472,32465,32459,32451,32444,32437,32429,32422,32414,32407,32399,32392,32383,32376,32368,32360,32352,32344,32335,32327,32318,32310,32301,32292,32284,32275,32266,32257,32248,32239,32230,32220,32211,32202,32192,32183,32172,32163,32153,32143,32133,32123,32113,32102,32093,32082,32071,32060,32050,32039,32029,32018,32007,31995,31984,31973,31962,31950,31939,31928,31916,31904,31893,31880,31869,31856,31845,31832,31820,31808,31796,31783,31770,31758,31745,31732,31719,31706,31693,31681,31668,31654,31641,31628,31613,31600,31586,31573,31559,31546,31531,31517,31503,31489,31475,31460,31446,31431,31417,31402,31388,31372,31357,31343,31328,31312,31297,31282,31267,31251,31236,31220,31204,31189,31173,31157,31141,31125,31108,31092,31076,31060,31043,31027,31011,30994,30977,30960,30943,30926,30909,30892,30875,30858,30840,30823,30806,30788,30770,30753,30735,30717,30699,30681,30663,30644,30626,30608,30590,30571,30553,30534,30515,30496,30478,30459,30439,30421,30402,30382,30363,30343,30324,30304,30285,30265,30246,30226,30205,30186,30166,30145,30125,30105,30085,30064,30044,30023,30002,29982,29961,29940,29919,29898,29877,29856,29835,29814,29792,29770,29749,29728,29705,29684,29663,29640,29618,29596,29574,29552,29530,29507,29485,29462,29439,29417,29394,29371,29349,29326,29303,29280,29256,29234,29211,29187,29164,29140,29117,29093,29069,29046,29022,28997,28973,28949,28925,28901,28876,28852,28828,28804,28779,28754,28729,28705,28680,28655,28630,28605,28580,28554,28530,28504,28478,28453,28427,28402};
-
-const short
-bldc_phase2[]={
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,35,85,137,189,240,292,344,394,446,497,549,600,652,703,754,806,858,909,961,1012,1063,1115,1167,1218,1269,1321,1372,1423,1475,1526,1577,1629,1680,1732,1783,1835,1886,1938,1989,2040,2092,2143,2194,2246,2297,2348,2400,2452,2502,2554,2605,2657,2708,2759,2810,2861,2912,2964,3015,3066,3118,3169,3220,3271,3323,3374,3425,3476,3527,3578,3630,3681,3732,3783,3835,3885,3936,3988,4039,4089,4141,4192,4243,4294,4345,4396,4446,4498,4549,4600,4651,4702,4753,4804,4854,4906,4956,5007,5058,5109,5159,5210,5261,5312,5363,5413,5464,5515,5566,5617,5668,5718,5769,5820,5871,5921,5972,6022,6073,6123,6174,6224,6275,6325,6376,6426,6476,6527,6578,6628,6678,6728,6779,6829,6880,6930,6981,7031,7080,7131,7181,7232,7282,7332,7382,7432,7483,7532,7583,7633,7682,7733,7783,7833,7883,7932,7983,8032,8082,8133,8182,8232,8281,8331,8381,8431,8481,8531,8581,8629,8679,8729,8779,8828,8878,8927,8977,9027,9075,9125,9174,9224,9274,9323,9372,9422,9471,9519,9569,9618,9667,9716,9766,9815,9864,9913,9962,10011,10060,10110,10159,10207,10256,10305,10354,10402,10451,10500,10548,10598,10646,10695,10743,10792,10841,10890,10938,10986,11034,11084,11132,11180,11228,11277,11325,11373,11422,11470,11518,11566,11615,11662,11711,11759,11806,11855,11903,11950,11999,12046,12094,12142,12190,12238,12285,12333,12381,12428,12476,12523,12571,12618,12666,12713,12761,12808,12856,12903,12951,12997,13045,13092,13139,13186,13234,13280,13328,13375,13421,13469,13515,13562,13609,13656,13703,13749,13796,13842,13889,13936,13982,14029,14075,14122,14169,14215,14261,14308,14354,14400,14447,14492,14539,14585,14631,14677,14723,14769,14815,14861,14906,14952,14999,15044,15090,15136,15181,15226,15272,15317,15363,15409,15453,15499,15545,15590,15635,15680,15726,15770,15816,15861,15906,15950,15996,16041,16085,16130,16175,16220,16264,16309,16354,16397,16442,16487,16531,16575,16620,16664,16708,16752,16797,16841,16885,16929,16974,17017,17061,17105,17149,17193,17237,17281,17324,17368,17412,17454,17498,17542,17585,17629,17672,17716,17759,17802,17846,17888,17931,17975,18017,18061,18104,18146,18189,18232,18274,18317,18360,18402,18445,18487,18530,18573,18615,18657,18700,18742,18784,18826,18868,18911,18952,18995,19036,19078,19119,19162,19204,19245,19287,19328,19370,19411,19453,19494,19536,19576,19618,19659,19700,19741,19783,19823,19865,19905,19946,19987,20028,20068,20110,20150,20190,20231,20271,20312,20352,20392,20433,20473,20513,20553,20593,20634,20673,20713,20753,20793,20832,20873,20912,20951,20992,21031,21070,21109,21149,21188,21227,21266,21306,21345,21384,21423,21461,21500,21540,21578,21617,21655,21694,21732,21771,21810,21848,21886,21925,21963,22001,22039,22077,22115,22153,22191,22229,22266,22304,22342,22380,22417,22455,22492,22530,22566,22604,22641,22678,22716,22753,22790,22826,22863,22900,22937,22974,23010,23047,23084,23121,23156,23193,23230,23265,23302,23338,23374,23410,23447,23482,23518,23553,23590,23625,23661,23697,23732,23768,23803,23838,23873,23908,23944,23979,24014,24049,24083,24119,24153,24188,24223,24257,24292,24327,24361,24395,24429,24464,24498,24532,24566,24600,24635,24669,24703,24736,24770,24803,24837,24870,24904,24937,24971,25004,25037,25071,25104,25137,25170,25203,25236,25268,25301,25334,25366,25399,25431,25464,25496,25529,25560,25593,25625,25657,25689,25721,25753,25785,25816,25848,25879,25911,25942,25974,26005,26037,26068,26099,26130,26161,26192,26223,26253,26285,26315,26346,26376,26407,26438,26468,26498,26528,26558,26588,26619,26649,26679,26709,26738,26768,26798,26827,26857,26886,26915,26944,26974,27003,27033,27062,27090,27119,27148,27176,27205,27234,27262,27291,27320,27348,27377,27404,27433,27461,27489,27517,27545,27573,27601,27628,27656,27684,27711,27738,27766,27793,27820,27847,27874,27902,27929,27956,27982,28009,28036,28062,28089,28115,28142,28168,28194,28221,28247,28272,28298,28324,28351,28377,28402,28427,28453,28478,28504,28530,28554,28580,28605,28630,28655,28680,28705,28729,28754,28779,28804,28828,28852,28876,28901,28925,28949,28973,28997,29022,29046,29069,29093,29117,29140,29164,29187,29211,29234,29256,29280,29303,29326,29349,29371,29394,29417,29439,29462,29485,29507,29530,29552,29574,29596,29618,29640,29663,29684,29705,29728,29749,29770,29792,29814,29835,29856,29877,29898,29919,29940,29961,29982,30002,30023,30044,30064,30085,30105,30125,30145,30166,30186,30205,30226,30246,30265,30285,30304,30324,30343,30363,30382,30402,30421,30439,30459,30478,30496,30515,30534,30553,30571,30590,30608,30626,30644,30663,30681,30699,30717,30735,30753,30770,30788,30806,30823,30840,30858,30875,30892,30909,30926,30943,30960,30977,30994,31011,31027,31043,31060,31076,31092,31108,31125,31141,31157,31173,31189,31204,31220,31236,31251,31267,31282,31297,31312,31328,31343,31357,31372,31388,31402,31417,31431,31446,31460,31475,31489,31503,31517,31531,31546,31559,31573,31586,31600,31613,31628,31641,31654,31668,31681,31693,31706,31719,31732,31745,31758,31770,31783,31796,31808,31820,31832,31845,31856,31869,31880,31893,31904,31916,31928,31939,31950,31962,31973,31984,31995,32007,32018,32029,32039,32050,32060,32071,32082,32093,32102,32113,32123,32133,32143,32153,32163,32172,32183,32192,32202,32211,32220,32230,32239,32248,32257,32266,32275,32284,32292,32301,32310,32318,32327,32335,32344,32352,32360,32368,32376,32383,32392,32399,32407,32414,32422,32429,32437,32444,32451,32459,32465,32472,32479,32485,32492,32499,32505,32512,32518,32524,32531,32537,32542,32549,32555,32561,32566,32572,32577,32583,32588,32594,32599,32604,32609,32614,32619,32624,32629,32633,32638,32642,32647,32651,32655,32660,32664,32668,32671,32675,32679,32682,32686,32690,32693,32697,32700,32704,32707,32709,32713,32715,32718,32722,32724,32727,32729,32731,32734,32736,32738,32740,32742,32744,32746,32748,32750,32751,32753,32753,32755,32756,32758,32759,32760,32761,32761,32763,32763,32764,32764,32765,32765,32765,32766,32766,32766,32766,32766,32765,32765,32765,32765,32764,32764,32763,32762,32761,32761,32760,32758,32758,32756,32755,32753,32752,32750,32748,32747,32745,32743,32741,32739,32738,32735,32733,32731,32728,32725,32723,32720,32718,32715,32711,32709,32706,32702,32699,32696,32693,32689,32686,32682,32678,32675,32671,32666,32662,32658,32654,32649,32645,32640,32636,32631,32627,32622,32618,32612,32607,32603,32597,32592,32587,32581,32576,32570,32564,32559,32553,32547,32541,32535,32529,32522,32516,32510,32503,32496,32490,32483,32477,32470,32463,32455,32449,32442,32435,32427,32420,32412,32405,32396,32389,32381,32373,32365,32357,32349,32341,32333,32324,32316,32307,32298,32290,32281,32272,32263,32254,32245,32236,32226,32217,32208,32198,32189,32179,32169,32159,32150,32139,32130,32119,32109,32099,32089,32078,32067,32057,32047,32036,32025,32014,32003,31992,31981,31970,31958,31946,31935,31924,31911,31900,31889,31876,31865,31853,31840,31828,31816,31803,31792,31779,31766,31754,31741,31728,31715,31702,31689,31676,31663,31650,31637,31622,31609,31595,31582,31568,31555,31540,31526,31513,31498,31484,31470,31455,31441,31426,31412,31397,31382,31368,31352,31337,31323,31307,31292,31277,31261,31246,31231,31215,31200,31184,31167,31151,31135,31119,31103,31087,31070,31054,31038,31022,31005,30988,30971,30955,30937,30921,30903,30887,30869,30852,30835,30817,30799,30782,30765,30747,30729,30711,30693,30674,30656,30638,30620,30602,30583,30565,30546,30528,30509,30490,30471,30452,30434,30414,30395,30375,30356,30337,30318,30297,30278,30258,30239,30219,30199,30179,30159,30139,30118,30099,30078,30057,30037,30016,29995,29975,29954,29933,29912,29891,29870,29849,29828,29807,29785,29763,29742,29720,29698,29677,29654,29633,29611,29589,29567,29544,29523,29500,29477,29455,29432,29409,29387,29364,29341,29319,29295,29272,29249,29225,29202,29179,29155,29132,29108,29084,29061,29037,29013,28989,28965,28942,28918,28893,28869,28844,28819,28795,28771,28746,28721,28697,28671,28647,28621,28597,28571,28546,28521,28495,28470,28444,28418,28394,28385,28410,28436,28461,28487,28512,28538,28563,28588,28614,28638,28664,28688,28713,28738,28762,28787,28812,28836,28861,28885,28909,28933,28958,28982,29006,29029,29053,29077,29100,29124,29148,29171,29195,29218,29241,29265,29287,29310,29333,29357,29380,29402,29425,29448,29470,29492,29514,29537,29559,29581,29603,29626,29647,29670,29691,29713,29735,29756,29777,29799,29821,29842,29863,29884,29905,29926,29947,29968,29988,30009,30030,30051,30071,30092,30112,30132,30152,30173,30193,30212,30232,30252,30272,30292,30311,30331,30350,30370,30389,30407,30427,30446,30465,30484,30503,30521,30540,30558,30578,30596,30614,30633,30651,30669,30687,30705,30723,30740,30758,30776,30794,30812,30829,30846,30864,30881,30898,30915,30932,30949,30966,30982,31000,31016,31032,31049,31065,31081,31098,31114,31130,31146,31162,31178,31194,31209,31225,31241,31256,31272,31287,31303,31317,31332,31348,31363,31377,31392,31406,31422,31437,31451,31464,31479,31493,31508,31522,31535,31550,31564,31577,31591,31604,31618,31632,31645,31659,31672,31684,31697,31711,31724,31737,31750,31762,31774,31787,31799,31812,31825,31836,31849,31860,31873,31885,31896,31908,31920,31931,31943,31955,31966,31977,31988,31999,32010,32021,32032,32043,32054,32064,32075,32085,32095,32106,32117,32126,32137,32146,32156,32166,32176,32185,32195,32205,32214,32224,32233,32242,32251,32261,32270,32278,32287,32296,32304,32313,32322,32329,32338,32346,32355,32363,32370,32379,32387,32394,32402,32409,32417,32424,32431,32439,32446,32453,32461,32468,32474,32481,32488,32494,32501,32507,32514,32520,32527,32533,32539,32544,32551,32557,32562,32568,32574,32579,32585,32590,32596,32601,32605,32611,32616,32620,32625,32630,32635,32640,32644,32648,32653,32657,32660,32665,32669,32673,32677,32680,32684,32688,32691,32695,32698,32702,32704,32708,32711,32713,32716,32720,32722,32725,32727,32729,32732,32734,32736,32739,32741,32743,32745,32746,32748,32750,32751,32753,32755,32756,32757,32758,32759,32760,32761,32762,32763,32764,32764,32764,32765,32765,32765,32766,32766,32766,32766,32766,32765,32765,32765,32764,32764,32764,32763,32762,32761,32760,32759,32758,32757,32756,32755,32753,32751,32750,32748,32746,32745,32743,32741,32739,32736,32734,32732,32729,32727,32725,32722,32720,32716,32713,32711,32708,32704,32702,32698,32695,32691,32688,32684,32680,32677,32673,32669,32665,32660,32657,32653,32648,32644,32640,32635,32630,32625,32620,32616,32611,32605,32601,32596,32590,32585,32579,32574,32568,32562,32557,32551,32544,32539,32533,32527,32520,32514,32507,32501,32494,32488,32481,32474,32468,32461,32453,32446,32439,32431,32424,32417,32409,32402,32394,32387,32379,32370,32363,32355,32346,32338,32329,32322,32313,32304,32296,32287,32278,32270,32261,32251,32242,32233,32224,32214,32205,32195,32185,32176,32166,32156,32146,32137,32126,32117,32106,32095,32085,32075,32064,32054,32043,32032,32021,32010,31999,31988,31977,31966,31955,31943,31931,31920,31908,31896,31885,31873,31860,31849,31836,31825,31812,31799,31787,31774,31762,31750,31737,31724,31711,31697,31684,31672,31659,31645,31632,31618,31604,31591,31577,31564,31550,31535,31522,31508,31493,31479,31464,31451,31437,31422,31406,31392,31377,31363,31348,31332,31317,31303,31287,31272,31256,31241,31225,31209,31194,31178,31162,31146,31130,31114,31098,31081,31065,31049,31032,31016,31000,30982,30966,30949,30932,30915,30898,30881,30864,30846,30829,30812,30794,30776,30758,30740,30723,30705,30687,30669,30651,30633,30614,30596,30578,30558,30540,30521,30503,30484,30465,30446,30427,30407,30389,30370,30350,30331,30311,30292,30272,30252,30232,30212,30193,30173,30152,30132,30112,30092,30071,30051,30030,30009,29988,29968,29947,29926,29905,29884,29863,29842,29821,29799,29777,29756,29735,29713,29691,29670,29647,29626,29603,29581,29559,29537,29514,29492,29470,29448,29425,29402,29380,29357,29333,29310,29287,29265,29241,29218,29195,29171,29148,29124,29100,29077,29053,29029,29006,28982,28958,28933,28909,28885,28861,28836,28812,28787,28762,28738,28713,28688,28664,28638,28614,28588,28563,28538,28512,28487,28461,28436,28410,28385,28359,28333,28307,28281,28255,28229,28202,28177,28151,28124,28098,28071,28045,28018,27992,27965,27937,27910,27883,27857,27830,27803,27775,27747,27720,27692,27665,27638,27609,27582,27555,27526,27499,27470,27442,27414,27386,27357,27330,27301,27272,27244,27215,27186,27158,27129,27100,27071,27042,27013,26984,26954,26925,26895,26867,26837,26808,26778,26748,26719,26689,26659,26629,26599,26568,26538,26508,26478,26448,26417,26386,26356,26325,26295,26264,26233,26203,26171,26140,26110,26078,26047,26015,25984,25952,25922,25891,25858,25826,25795,25763,25732,25699,25667,25636,25603,25572,25539,25507,25474,25443,25410,25377,25344,25311,25280,25247,25214,25181,25148,25114,25081,25048,25015,24981,24948,24915,24881,24848,24814,24781,24747,24713,24680,24646,24612,24578,24543,24509,24475,24441,24407,24372,24338,24304,24270,24234,24200,24166,24130,24096,24060,24025,23991,23955,23920,23885,23850,23814,23779,23744,23708,23672,23637,23601,23566,23530,23494,23458,23422,23386,23350,23314,23278,23241,23206,23169,23132,23095,23060,23023,22986,22949,22913,22876,22839,22802,22765,22728,22691,22654,22616,22579,22542,22504,22467,22429,22392,22355,22317,22279,22242,22204,22166,22128,22090,22052,22014,21976,21937,21898,21860,21822,21784,21745,21707,21668,21630,21591,21552,21513,21474,21436,21397,21357,21318,21279,21240,21201,21161,21122,21083,21044,21004,20965,20925,20885,20846,20806,20767,20726,20687,20647,20606,20567,20526,20486,20446,20406,20366,20326,20285,20245,20204,20164,20122,20082,20042,20001,19960,19919,19878,19837,19796,19755,19714,19673,19632,19590,19550,19508,19467,19425,19384,19342,19301,19259,19217,19176,19134,19092,19050,19009,18966,18924,18882,18840,18798,18756,18713,18672,18629,18587,18544,18502,18460,18417,18374,18332,18289,18246,18204,18160,18117,18075,18032,17988,17946,17903,17859,17817,17773,17729,17687,17643,17599,17557,17513,17469,17426,17382,17338,17295,17251,17207,17163,17120,17076,17032,16988,16944,16900,16855,16812,16767,16723,16679,16635,16590,16545,16501,16457,16412,16369,16324,16279,16234,16190,16145,16100,16056,16011,15966,15920,15876,15831,15785,15741,15695,15650,15604,15560,15514,15469,15424,15378,15332,15288,15242,15196,15151,15105,15059,15013,14968,14922,14876,14830,14784,14738,14692,14646,14600,14554,14508,14461,14416,14369,14322,14277,14230,14184,14138,14091,14045,13998,13951,13905,13858,13812,13765,13718,13672,13624,13578,13531,13485,13437,13390,13343,13296,13249,13202,13155,13108,13061,13013,12966,12919,12872,12824,12777,12729,12682,12634,12587,12539,12492,12444,12397,12349,12301,12254,12206,12158,12110,12062,12015,11966,11919,11870,11823,11775,11726,11679,11631,11582,11534,11486,11437,11389,11341,11292,11244,11196,11148,11099,11051,11003,10954,10905,10857,10808,10760,10711,10663,10613,10565,10516,10468,10419,10370,10322,10272,10223,10174,10125,10076,10028,9979,9930,9880,9831,9782,9733,9684,9635,9586,9536,9487,9438,9388,9339,9290,9241,9191,9142,9092,9043,8993,8944,8895,8845,8795,8745,8696,8646,8597,8547,8498,8448,8398,8348,8298,8248,8199,8149,8099,8049,7999,7950,7899,7850,7799,7750,7699,7649,7599,7550,7499,7449,7399,7349,7298,7248,7199,7148,7098,7048,6997,6947,6897,6846,6796,6746,6696,6644,6594,6544,6494,6443,6392,6342,6291,6241,6190,6140,6089,6039,5988,5938,5887,5836,5786,5735,5684,5633,5583,5532,5481,5431,5380,5329,5278,5227,5177,5126,5075,5024,4974,4922,4871,4821,4770,4718,4668,4617,4566,4515,4464,4413,4362,4311,4260,4209,4158,4106,4056,4005,3953,3902,3852,3800,3749,3698,3647,3595,3544,3493,3442,3391,3340,3289,3237,3186,3135,3083,3032,2981,2930,2878,2827,2777,2725,2674,2622,2571,2519,2468,2417,2366,2314,2263,2212,2160,2109,2058,2006,1955,1903,1852,1801,1750,1698,1647,1595,1544,1492,1441,1390,1338,1286,1235,1183,1132,1081,1029,978,926,875,823,772,720,669,618,566,515,463,412,360,309,257,206,155,103,51};
-
-const short
-bldc_phase3[]={
-0,51,103,155,206,257,309,360,412,463,515,566,618,669,720,772,823,875,926,978,1029,1081,1132,1183,1235,1286,1338,1390,1441,1492,1544,1595,1647,1698,1750,1801,1852,1903,1955,2006,2058,2109,2160,2212,2263,2314,2366,2417,2468,2519,2571,2622,2674,2725,2777,2827,2878,2930,2981,3032,3083,3135,3186,3237,3289,3340,3391,3442,3493,3544,3595,3647,3698,3749,3800,3852,3902,3953,4005,4056,4106,4158,4209,4260,4311,4362,4413,4464,4515,4566,4617,4668,4718,4770,4821,4871,4922,4974,5024,5075,5126,5177,5227,5278,5329,5380,5431,5481,5532,5583,5633,5684,5735,5786,5836,5887,5938,5988,6039,6089,6140,6190,6241,6291,6342,6392,6443,6494,6544,6594,6644,6696,6746,6796,6846,6897,6947,6997,7048,7098,7148,7199,7248,7298,7349,7399,7449,7499,7550,7599,7649,7699,7749,7799,7850,7899,7950,7999,8049,8099,8149,8199,8248,8298,8348,8398,8448,8498,8547,8597,8646,8696,8745,8795,8845,8895,8944,8993,9043,9092,9142,9191,9241,9290,9339,9388,9438,9487,9536,9586,9635,9684,9733,9782,9831,9880,9930,9979,10028,10076,10125,10174,10223,10272,10322,10370,10419,10468,10516,10565,10613,10663,10711,10760,10808,10857,10905,10954,11003,11051,11099,11148,11196,11244,11292,11341,11389,11437,11486,11534,11582,11631,11679,11726,11775,11823,11870,11919,11966,12015,12062,12110,12158,12206,12254,12301,12349,12397,12444,12492,12539,12587,12634,12682,12729,12777,12824,12872,12919,12966,13013,13061,13108,13155,13202,13249,13296,13343,13390,13437,13485,13531,13578,13624,13672,13718,13765,13812,13858,13905,13951,13998,14045,14091,14138,14184,14230,14277,14322,14369,14416,14461,14508,14554,14600,14646,14692,14738,14784,14830,14876,14922,14968,15013,15059,15105,15151,15196,15242,15288,15332,15378,15424,15469,15514,15560,15604,15650,15695,15741,15785,15831,15876,15920,15966,16011,16056,16100,16145,16190,16234,16279,16324,16369,16412,16457,16501,16545,16590,16635,16679,16723,16767,16812,16855,16900,16944,16988,17032,17076,17120,17163,17207,17251,17295,17338,17382,17426,17469,17513,17557,17599,17643,17687,17729,17773,17817,17859,17903,17946,17988,18032,18075,18117,18160,18204,18246,18289,18332,18374,18417,18460,18502,18544,18587,18629,18672,18713,18756,18798,18840,18882,18924,18966,19009,19050,19092,19134,19176,19217,19259,19301,19342,19384,19425,19467,19508,19550,19590,19632,19673,19714,19755,19797,19837,19878,19919,19960,20001,20042,20082,20122,20164,20204,20245,20285,20326,20366,20406,20446,20486,20526,20567,20606,20646,20687,20726,20767,20806,20846,20885,20925,20965,21004,21044,21083,21122,21161,21201,21240,21279,21318,21357,21397,21436,21474,21513,21552,21591,21630,21668,21707,21745,21784,21822,21860,21898,21937,21976,22014,22052,22090,22128,22166,22204,22242,22279,22317,22355,22392,22429,22467,22504,22542,22579,22616,22654,22691,22728,22765,22802,22839,22876,22913,22949,22986,23023,23060,23095,23132,23169,23206,23241,23278,23314,23350,23386,23422,23458,23494,23530,23566,23601,23637,23672,23708,23744,23779,23814,23850,23885,23920,23955,23991,24025,24060,24096,24130,24166,24200,24234,24270,24304,24338,24372,24407,24441,24475,24509,24543,24578,24612,24646,24680,24713,24747,24781,24814,24848,24881,24915,24948,24981,25015,25048,25081,25114,25148,25181,25214,25247,25280,25311,25344,25377,25410,25443,25474,25507,25539,25572,25603,25636,25667,25699,25732,25763,25795,25826,25858,25891,25922,25952,25984,26015,26047,26078,26110,26140,26171,26203,26233,26264,26295,26325,26356,26386,26417,26448,26478,26508,26538,26568,26599,26629,26659,26689,26719,26748,26778,26808,26837,26867,26895,26925,26954,26984,27013,27042,27071,27100,27129,27158,27186,27215,27244,27272,27301,27330,27357,27386,27414,27442,27470,27499,27526,27555,27582,27609,27638,27665,27692,27720,27747,27775,27803,27830,27857,27883,27910,27937,27965,27992,28018,28045,28071,28098,28124,28151,28177,28202,28229,28255,28281,28307,28333,28359,28385,28410,28436,28461,28487,28512,28538,28563,28588,28614,28638,28664,28688,28713,28738,28762,28787,28812,28836,28861,28885,28909,28933,28958,28982,29006,29029,29053,29077,29100,29124,29148,29171,29195,29218,29241,29265,29287,29310,29333,29357,29380,29402,29425,29448,29470,29492,29514,29537,29559,29581,29603,29626,29647,29670,29691,29713,29735,29756,29777,29799,29821,29842,29863,29884,29905,29926,29947,29968,29988,30009,30030,30051,30071,30092,30112,30132,30152,30173,30193,30212,30232,30252,30272,30292,30311,30331,30350,30370,30389,30407,30427,30446,30465,30484,30503,30521,30540,30558,30578,30596,30614,30633,30651,30669,30687,30705,30723,30740,30758,30776,30794,30812,30829,30846,30864,30881,30898,30915,30932,30949,30966,30982,31000,31016,31032,31049,31065,31081,31098,31114,31130,31146,31162,31178,31194,31209,31225,31241,31256,31272,31287,31303,31317,31332,31348,31363,31377,31392,31406,31422,31437,31451,31464,31479,31493,31508,31522,31535,31550,31564,31577,31591,31604,31618,31632,31645,31659,31672,31684,31697,31711,31724,31737,31750,31762,31774,31787,31799,31812,31825,31836,31849,31860,31873,31885,31896,31908,31920,31931,31943,31955,31966,31977,31988,31999,32010,32021,32032,32043,32054,32064,32075,32085,32095,32106,32117,32126,32137,32146,32156,32166,32176,32185,32195,32205,32214,32224,32233,32242,32251,32261,32270,32278,32287,32296,32304,32313,32322,32329,32338,32346,32355,32363,32370,32379,32387,32394,32402,32409,32417,32424,32431,32439,32446,32453,32461,32468,32474,32481,32488,32494,32501,32507,32514,32520,32527,32533,32539,32544,32551,32557,32562,32568,32574,32579,32585,32590,32596,32601,32605,32611,32616,32620,32625,32630,32635,32640,32644,32648,32653,32657,32660,32665,32669,32673,32677,32680,32684,32688,32691,32695,32698,32702,32704,32708,32711,32713,32716,32720,32722,32725,32727,32729,32732,32734,32736,32739,32741,32743,32745,32746,32748,32750,32751,32753,32755,32756,32757,32758,32759,32760,32761,32762,32763,32764,32764,32764,32765,32765,32765,32766,32766,32766,32766,32766,32765,32765,32765,32764,32764,32764,32763,32762,32761,32760,32759,32758,32757,32756,32755,32753,32751,32750,32748,32746,32745,32743,32741,32739,32736,32734,32732,32729,32727,32725,32722,32720,32716,32713,32711,32708,32704,32702,32698,32695,32691,32688,32684,32680,32677,32673,32669,32665,32660,32657,32653,32648,32644,32640,32635,32630,32625,32620,32616,32611,32605,32601,32596,32590,32585,32579,32574,32568,32562,32557,32551,32544,32539,32533,32527,32520,32514,32507,32501,32494,32488,32481,32474,32468,32461,32453,32446,32439,32431,32424,32417,32409,32402,32394,32387,32379,32370,32363,32355,32346,32338,32329,32322,32313,32304,32296,32287,32278,32270,32261,32251,32242,32233,32224,32214,32205,32195,32185,32176,32166,32156,32146,32137,32126,32117,32106,32095,32085,32075,32064,32054,32043,32032,32021,32010,31999,31988,31977,31966,31955,31943,31931,31920,31908,31896,31885,31873,31860,31849,31836,31825,31812,31799,31787,31774,31762,31750,31737,31724,31711,31697,31684,31672,31659,31645,31632,31618,31604,31591,31577,31564,31550,31535,31522,31508,31493,31479,31464,31451,31437,31422,31406,31392,31377,31363,31348,31332,31317,31303,31287,31272,31256,31241,31225,31209,31194,31178,31162,31146,31130,31114,31098,31081,31065,31049,31032,31016,31000,30982,30966,30949,30932,30915,30898,30881,30864,30846,30829,30812,30794,30776,30758,30740,30723,30705,30687,30669,30651,30633,30614,30596,30578,30558,30540,30521,30503,30484,30465,30446,30427,30407,30389,30370,30350,30331,30311,30292,30272,30252,30232,30212,30193,30173,30152,30132,30112,30092,30071,30051,30030,30009,29988,29968,29947,29926,29905,29884,29863,29842,29821,29799,29777,29756,29735,29713,29691,29670,29647,29626,29603,29581,29559,29537,29514,29492,29470,29448,29425,29402,29380,29357,29333,29310,29287,29265,29241,29218,29195,29171,29148,29124,29100,29077,29053,29029,29006,28982,28958,28933,28909,28885,28861,28836,28812,28787,28762,28738,28713,28688,28664,28638,28614,28588,28563,28538,28512,28487,28461,28436,28410,28385,28394,28418,28444,28470,28495,28521,28546,28571,28597,28621,28647,28671,28697,28721,28746,28771,28795,28819,28844,28869,28893,28918,28942,28965,28989,29013,29037,29061,29084,29108,29132,29155,29179,29202,29225,29249,29272,29295,29319,29341,29364,29387,29409,29432,29455,29477,29500,29523,29544,29567,29589,29611,29633,29654,29677,29698,29720,29742,29763,29785,29807,29828,29849,29870,29891,29912,29933,29954,29975,29995,30016,30037,30057,30078,30099,30118,30139,30159,30179,30199,30219,30239,30258,30278,30297,30318,30337,30356,30375,30395,30414,30434,30452,30471,30490,30509,30528,30546,30565,30583,30602,30620,30638,30656,30674,30693,30711,30729,30747,30765,30782,30799,30817,30835,30852,30869,30887,30903,30921,30937,30955,30971,30988,31005,31022,31038,31054,31070,31087,31103,31119,31135,31151,31167,31184,31200,31215,31231,31246,31261,31277,31292,31307,31323,31337,31352,31368,31382,31397,31412,31426,31441,31455,31470,31484,31498,31513,31526,31540,31555,31568,31582,31595,31609,31622,31637,31650,31663,31676,31689,31702,31715,31728,31741,31754,31766,31779,31792,31803,31816,31828,31840,31853,31865,31876,31889,31900,31911,31924,31935,31946,31958,31970,31981,31992,32003,32014,32025,32036,32047,32057,32067,32078,32089,32099,32109,32119,32130,32139,32150,32159,32169,32179,32189,32198,32208,32217,32226,32235,32245,32254,32263,32272,32281,32290,32298,32307,32316,32324,32333,32341,32349,32357,32365,32373,32381,32389,32396,32405,32412,32420,32427,32435,32442,32449,32455,32463,32470,32477,32483,32490,32496,32503,32509,32516,32522,32529,32535,32541,32547,32553,32559,32564,32570,32576,32581,32587,32592,32597,32603,32607,32612,32618,32622,32627,32631,32636,32640,32645,32649,32654,32658,32662,32666,32671,32675,32678,32682,32686,32689,32693,32696,32699,32702,32706,32709,32711,32715,32718,32720,32723,32725,32728,32731,32733,32735,32738,32739,32741,32743,32745,32747,32748,32750,32752,32753,32755,32756,32758,32758,32760,32761,32761,32762,32763,32764,32764,32765,32765,32765,32765,32766,32766,32766,32766,32766,32765,32765,32765,32764,32764,32763,32763,32761,32761,32760,32759,32758,32756,32755,32753,32753,32751,32750,32748,32746,32744,32742,32740,32738,32736,32734,32731,32729,32727,32724,32722,32718,32715,32713,32709,32707,32704,32700,32697,32693,32690,32686,32682,32679,32675,32671,32668,32664,32660,32655,32651,32647,32642,32638,32633,32629,32624,32619,32614,32609,32604,32599,32594,32588,32583,32577,32572,32566,32561,32555,32549,32542,32537,32531,32524,32518,32512,32505,32499,32492,32485,32479,32472,32465,32459,32451,32444,32437,32429,32422,32414,32407,32399,32392,32383,32376,32368,32360,32352,32344,32335,32327,32318,32310,32301,32292,32284,32275,32266,32257,32248,32239,32230,32220,32211,32202,32192,32183,32172,32163,32153,32143,32133,32123,32113,32102,32093,32082,32071,32060,32050,32039,32029,32018,32007,31995,31984,31973,31962,31950,31939,31928,31916,31904,31893,31880,31869,31856,31845,31832,31820,31808,31796,31783,31770,31758,31745,31732,31719,31706,31693,31681,31668,31654,31641,31628,31613,31600,31586,31573,31559,31546,31531,31517,31503,31489,31475,31460,31446,31431,31417,31402,31388,31372,31357,31343,31328,31312,31297,31282,31267,31251,31236,31220,31204,31189,31173,31157,31141,31125,31108,31092,31076,31060,31043,31027,31011,30994,30977,30960,30943,30926,30909,30892,30875,30858,30840,30823,30806,30788,30770,30753,30735,30717,30699,30681,30663,30644,30626,30608,30590,30571,30553,30534,30515,30496,30478,30459,30439,30421,30402,30382,30363,30343,30324,30304,30285,30265,30246,30226,30205,30186,30166,30145,30125,30105,30085,30064,30044,30023,30002,29982,29961,29940,29919,29898,29877,29856,29835,29814,29792,29770,29749,29728,29705,29684,29663,29640,29618,29596,29574,29552,29530,29507,29485,29462,29439,29417,29394,29371,29349,29326,29303,29280,29256,29234,29211,29187,29164,29140,29117,29093,29069,29046,29022,28997,28973,28949,28925,28901,28876,28852,28828,28804,28779,28754,28729,28705,28680,28655,28630,28605,28580,28554,28530,28504,28478,28453,28427,28402,28377,28351,28324,28298,28272,28247,28221,28194,28168,28142,28115,28089,28062,28036,28009,27982,27956,27929,27902,27874,27847,27820,27793,27766,27738,27711,27684,27656,27628,27601,27573,27545,27517,27489,27461,27433,27404,27377,27348,27320,27291,27262,27234,27205,27176,27148,27119,27090,27062,27033,27003,26974,26944,26915,26886,26857,26827,26798,26768,26738,26709,26679,26649,26619,26588,26558,26528,26498,26468,26438,26407,26376,26346,26315,26285,26253,26223,26192,26161,26130,26099,26068,26037,26005,25974,25942,25911,25879,25848,25816,25785,25753,25721,25689,25657,25625,25593,25560,25529,25496,25464,25431,25399,25366,25334,25301,25268,25236,25203,25170,25137,25104,25071,25037,25004,24971,24937,24904,24870,24837,24803,24770,24736,24703,24669,24635,24600,24566,24532,24498,24464,24429,24395,24361,24327,24292,24257,24223,24188,24153,24119,24083,24049,24014,23979,23944,23908,23873,23838,23803,23768,23732,23697,23661,23625,23590,23553,23518,23482,23447,23410,23374,23338,23302,23265,23230,23193,23156,23121,23084,23047,23010,22974,22937,22900,22863,22826,22790,22753,22716,22678,22641,22604,22566,22530,22492,22455,22417,22380,22342,22304,22266,22229,22191,22153,22115,22077,22039,22001,21963,21925,21886,21848,21810,21771,21732,21694,21655,21617,21578,21540,21500,21461,21423,21384,21345,21306,21266,21227,21188,21149,21109,21070,21031,20992,20951,20912,20873,20832,20793,20753,20713,20673,20634,20593,20553,20513,20473,20433,20392,20352,20312,20271,20231,20190,20150,20110,20068,20028,19987,19946,19905,19865,19823,19783,19741,19700,19659,19618,19576,19536,19494,19453,19411,19370,19328,19287,19245,19204,19162,19119,19078,19036,18995,18952,18911,18868,18826,18784,18742,18700,18657,18615,18573,18530,18487,18445,18402,18360,18317,18274,18232,18189,18146,18104,18061,18017,17975,17931,17888,17846,17802,17759,17716,17672,17629,17585,17542,17498,17454,17412,17368,17324,17281,17237,17193,17149,17105,17061,17017,16974,16929,16885,16841,16797,16752,16708,16664,16620,16575,16531,16487,16442,16397,16354,16309,16264,16220,16175,16130,16085,16041,15996,15950,15906,15861,15816,15770,15726,15680,15635,15590,15545,15499,15453,15409,15363,15317,15272,15226,15181,15136,15090,15044,14999,14952,14906,14861,14815,14769,14722,14677,14631,14585,14539,14492,14447,14400,14354,14308,14261,14215,14169,14122,14075,14029,13982,13936,13889,13842,13796,13749,13703,13656,13609,13562,13515,13469,13421,13375,13328,13280,13234,13186,13139,13092,13045,12997,12951,12903,12856,12808,12761,12713,12666,12618,12571,12523,12476,12428,12381,12333,12285,12238,12190,12142,12094,12046,11999,11950,11903,11855,11806,11759,11711,11662,11615,11566,11518,11470,11422,11373,11325,11277,11228,11180,11132,11084,11034,10986,10938,10890,10841,10792,10743,10695,10646,10598,10548,10500,10451,10402,10354,10305,10256,10207,10159,10110,10060,10011,9962,9913,9864,9815,9766,9716,9667,9618,9569,9519,9471,9422,9372,9323,9274,9224,9174,9125,9075,9027,8977,8927,8878,8828,8779,8729,8679,8629,8581,8531,8481,8431,8381,8331,8281,8232,8182,8133,8082,8032,7983,7932,7883,7833,7783,7733,7682,7633,7583,7532,7483,7432,7382,7332,7282,7232,7181,7131,7080,7031,6981,6930,6880,6829,6779,6728,6678,6628,6578,6527,6476,6426,6376,6325,6275,6224,6174,6123,6073,6022,5972,5921,5871,5820,5769,5718,5668,5617,5566,5515,5464,5413,5363,5312,5261,5210,5159,5109,5058,5007,4956,4906,4854,4804,4753,4702,4651,4600,4549,4498,4446,4396,4345,4294,4243,4192,4141,4089,4039,3988,3936,3885,3835,3783,3732,3681,3630,3578,3527,3476,3425,3374,3323,3271,3220,3169,3118,3066,3015,2964,2912,2861,2810,2759,2708,2657,2605,2554,2502,2452,2400,2348,2297,2246,2194,2143,2092,2040,1989,1938,1886,1835,1783,1732,1680,1629,1577,1526,1475,1423,1372,1321,1269,1218,1167,1115,1063,1012,961,909,858,806,754,703,652,600,549,497,446,394,344,292,240,189,137,85,35,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
-
+++ /dev/null
-/* increment subdivission is fixed, else selectable by pxms_subdiv */
-#define PXMC_WITH_FIXED_SUBDIV
-/* multiphase motor control */
-#define PXMC_WITH_PHASE_TABLE
-/* controller with finegrained speed generation */
-#define PXMC_WITH_FINE_GRAINED
-/* special calling convention for pxms_??? state functions */
-//#define PXMC_WITH_FAST_CALL
-/* possibility to select generator variant per axis */
-//#define PXMC_WITH_GEN_SELECTION
-/* whether to compile in debugger support */
-#define PXMC_WITH_DBG_HIST
-/* controller with current feedback */
-//#define PXMC_WITH_CURRENTFB
-/* The pxmc_set/clear_flags not supported, use one by one pxmc_set/clear_flag */
-#define PXMC_WITH_FLAGS_BYBITS_ONLY
-
-#ifndef PXMC_SUBDIV
-#ifdef PXMC_WITH_FIXED_SUBDIV
-/* number of fraction bits in integer part of possition */
-#define PXMC_SUBDIV(mcs) 8
-#else /*PXMC_WITH_FIXED_SUBDIV*/
-#define PXMC_SUBDIV(mcs) (mcs->pxms_subdiv)
-#endif /*PXMC_WITH_FIXED_SUBDIV*/
-#endif /*PXMC_SUBDIV*/
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc_h2638.c - generic multi axis motion controller
- h8s2638 hrdware specific functions
-//
- (C) 2001-2005 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (C) 2002-2005 by PiKRON Ltd. http://www.pikron.com
-
- (C) 2005 Petr Kovacik <kovacp1@Guillaume>
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
-
- *******************************************************************/
-
-/* [extern API] characteristic means that the function is declared
-in the header file pxmc.h so it is part of the external API */
-
-#include <types.h>
-#include <cpu_def.h>
-#include <h8s2638h.h>
-#include <system_def.h>
-#include <string.h>
-#include "pxmc.h"
-#include "pxmc_internal.h"
-#include "pxmc_inp_common.h"
-#include "pxmc_h2638.h"
-
-#ifndef PXMC_SFI_TPU_CHN
-#define PXMC_SFI_TPU_CHN 4 /* TPU channel used for timing */
-#endif
-
-// #define PXMC_USE_VANG /* Use pxms_ptvang for mag.fld. offs */
-//
-// #define PXMC_WITH_TPUCUR /* Measurement of phase current by TPU */
-
-/* Phase counting clock sources */
-/* TPCNT1, TPCNT5 .. TCLKA, TCLKB */
-/* TPCNT2, TPCNT4 .. TCLKC, TCLKD */
-
-// #if HW_VER_CODE<VER_CODE(0,2,0)
-// #define PXMC_IRC_TPUN 2 /* TPU channel used for IRC */
-// #else /* >=VER_CODE(0,2,0) */
-// #define PXMC_IRC_TPUN 5 /* TPU channel used for IRC */
-// #endif /* >=VER_CODE(0,2,0) */
-
-//pxmc_state_t mcs0;
-
-int pxmc_sfikhz=2;
-int pxmc_sfi2msec;
-#define pxmc_cyclekhz 20
-#define pwm_per (((CPU_SYS_HZ/16)/1000)/pxmc_cyclekhz)
-
-
-#define TEMP_PXMC_TPU_SFI_INIT(n)\
- do{ \
- *TPU_TSTR&=~TSTR_CST##n##m; /*counter n is stoped*/\
- /*CNTN cleared by by TGRA, rising edge; source TGRA compare match/input capture */\
- *TPU_TCR##n=(TPCR_TPSC_F1 | TPCR_CKEG_RIS | TPCR_CCLR_TGRA); \
- /*TGRA is output compare registr - . 0 output at compare match*/\
- *TPU_TIOR##n|=TIOR##n##_IOA1m; \
- /*MDn = 0x000 normal operation */\
- *TPU_TMDR##n=TPMDR_MD_NORMAL; \
- /*seting interupt vector - from TGRA at registr TSRn*/\
- excptvec_set(EXCPTVEC_TGI##n##A,pxmc_sfi_isr); \
- /*TimerInteruptEnableReg - enable IRQ generation from TGRA*/\
- *TPU_TIER##n |=TIER##n##_TGIEAm; \
- /*setup TGRA - sampling period*/\
- *TPU_TGR##n##A=(CPU_SYS_HZ/1000)/pxmc_sfikhz; \
- /*TimerStatusRegistr - clear match flag */\
- *TPU_TSR##n &=~TSR##n##_TGFAm; \
- /*Start TCNTn*/\
- *TPU_TSTR|=(1<<n);\
- }while (0)
-
-// TRICK: See cpp info pages, node Argument Prescan,
-// parahraph "Macros that call other macros that stringify or concatenate."
-#define PXMC_TPU_SFI_INIT(n) TEMP_PXMC_TPU_SFI_INIT(n)
-
-/*---------------------------------------------------------------------*/
-
-#define TEMP_PXMC_TPU_PHASE_CNT_INIT(n) \
- do{ \
- /*TimerControlRegistr - TCNT clearing disabled; count rasing adge; internal clock fi/1 */\
- *TPU_TCR##n=0; \
- /*phase counting mode 1 - how will be TCNT incrementd */\
- *TPU_TMDR##n=TPMDR_MD_PHACN1; \
- /*AD conversion disabled; interrupts from TCFU(TCFV,TGIA,TGIB) all interupt disabled.; */\
- *TPU_TIER##n=0; \
- /*start TCNT*/\
- *TPU_TSTR|=(1<<n); \
- }while (0)
-
-// TRICK: See cpp info pages, node Argument Prescan,
-// parahraph "Macros that call other macros that stringify or concatenate."
-#define PXMC_TPU_PHASE_CNT_INIT(n) TEMP_PXMC_TPU_PHASE_CNT_INIT(n)
-
-
-extern void deb_led_out(char val);
-
-#if 0 //FOR DEBUGGING
-
-#define error_led(led) \
- if (mcs->pxms_flg & PXMS_ERR_m) { \
- deb_led_out(led); \
- while(1);\
- }\
-
-#else
-#define error_led(led)
-#endif
-
-
-
-inline
-void pxmc_add_pservice_and_mode(short mod)
-{
- int var;
- for (var = 0; var < pxmc_main_list.pxml_cnt; var++) {
- /* Set default motor behaviour */
- pxmc_main_list.pxml_arr[var]->pxms_do_inp=pxmc_nofb_inp;
- pxmc_main_list.pxml_arr[var]->pxms_do_con=pxmc_nofb_con;
- pxmc_main_list.pxml_arr[var]->pxms_do_out=pxmc_nofb_out;
-
- pxmc_axis_mode(pxmc_main_list.pxml_arr[var], mod);
- }
-}
-
-
-/* direct, one mcs processing */
-inline void pxmc_tpu_interupt_eni()
-{
- int var=0;
- for (var = 0; var < pxmc_main_list.pxml_cnt; var++) {
- /* direct, one mcs processing */
- if(pxmc_main_list.pxml_arr[var]->pxms_flg&PXMS_ENI_m)
- {
- /* pxmc_nofb_inp(mcs); */
- pxmc_call(pxmc_main_list.pxml_arr[var],pxmc_main_list.pxml_arr[var]->pxms_do_inp);
- error_led(~1);
- }
- }
-}
-
-inline void pxmc_tpu_interupt_eng()
-{
- int var=0;
- for (var = 0; var < pxmc_main_list.pxml_cnt; var++) {
- if(pxmc_main_list.pxml_arr[var]->pxms_flg&PXMS_ENG_m)
- {
- /* pxmc_spdfg_gnr(pxmc_main_list.pxml_arr[var]); */
- pxmc_call(pxmc_main_list.pxml_arr[var],pxmc_main_list.pxml_arr[var]->pxms_do_gen);
- error_led(~2);
- }
- }
-}
-
-inline void pxmc_tpu_interupt_enr()
-{
- int var=0;
- for (var = 0; var < pxmc_main_list.pxml_cnt; var++) {
- if(pxmc_main_list.pxml_arr[var]->pxms_flg&PXMS_ENR_m)
- {
- /* pxmc_nofb_con(mcs); */
- pxmc_call(pxmc_main_list.pxml_arr[var],pxmc_main_list.pxml_arr[var]->pxms_do_con);
- error_led(~4);
- if(pxmc_main_list.pxml_arr[var]->pxms_flg&PXMS_ERR_m)
- pxmc_main_list.pxml_arr[var]->pxms_ene=0;
- /* pxmc_nofb_out(mcs); */
- pxmc_call(pxmc_main_list.pxml_arr[var],pxmc_main_list.pxml_arr[var]->pxms_do_out);
- error_led(1);
- }
- }
-}
-
-inline void pxmc_tpu_interupt_dbg()
-{
- int var=0;
- for (var = 0; var < pxmc_main_list.pxml_cnt; var++) {
- if(pxmc_main_list.pxml_arr[var]->pxms_flg&PXMS_DBG_m) {
- pxmc_call(pxmc_main_list.pxml_arr[var],pxmc_main_list.pxml_arr[var]->pxms_do_deb);
- error_led(2);
- }
- if(--pxmc_sfi2msec<=0) {
- msec_time++;
- pxmc_sfi2msec=pxmc_sfikhz;
- /* Hook for external finite state machine */
- }else {
- /* Run ADC measurements */
- }
- }
-}
-
-/********************************************************************/
-/* Stepper controller hardware dependant part */
-
-#define USE_BUF_PWM
-
-/*------------------------------------------------------------------*/
-/* Stepper motor with IRC feedback */
-
-short pxmc_tpupwm2f_mult;
-//int pxmc_sfikhz;
-
-/**
- * pxmc_tpuirc_inp - IRC encoder connected to H8S 2638 TPU
- * @mcs: Motion controller state information
- *
- * This version of input routine updates @pxms_ap from TPU counter.
- * Acquired value is then used for phase commutation..
- */
-int
-pxmc_tpuirc_inp(struct pxmc_state *mcs)
-{
- short irc;
- irc=*(volatile short*)mcs->pxms_inp_info;
- pxmc_irc_16bit_update(mcs,irc);
-
- /* Running of the motor commutator */
- if(mcs->pxms_flg&PXMS_PTI_m)
- pxmc_irc_16bit_commindx(mcs,irc);
-
- return 0;
-}
-
-/**
- * pxmc_tpuirc_nofb_inp - TPU IRC input without feedback commutation control
- * @mcs: Motion controller state information
- *
- * This version of input routine updates @pxms_ap from TPU counter,
- * but commutation is controlled by @pxms_rp.
- */
-inline int
-pxmc_tpuirc_nofb_inp(struct pxmc_state *mcs)
-{
- short irc;
- irc=*(volatile short*)mcs->pxms_inp_info;
- pxmc_irc_16bit_update(mcs,irc);
-
- /* Running of the motor commutator */
- irc=mcs->pxms_rp>>PXMC_SUBDIV(mcs);
- if(mcs->pxms_flg&PXMS_PTI_m)
- pxmc_irc_16bit_commindx(mcs,irc);
-
- return 0;
-}
-
-#if HW_VER_CODE<VER_CODE(0,2,0)
-
-static inline void
-pxmc_tpupwm2f_tpuwr(struct pxmc_state *mcs, short pwm1, short pwm2)
-{
- char mask=0;
-
- if(pwm1>0) mask|=1;
- else if(pwm1<0) {
- mask|=2;
- pwm1=-pwm1;
- }
- if(pwm2>0) mask|=4;
- else if(pwm2<0) {
- mask|=8;
- pwm2=-pwm2;
- }
- *STPM_OUT=mask;
-
- pwm1=((long)pwm1*pxmc_tpupwm2f_mult)>>16;
- pwm2=((long)pwm2*pxmc_tpupwm2f_mult)>>16;
- /* Output PWM requests into TPU registers */
- /* TPU logic becomes mad if width and period is same */
- /* and it cannot output zero width */
- /* full fill is possible by width longer than period */
-#ifndef USE_BUF_PWM
- *TPU_TPGR0A=pwm1;
- *TPU_TPGR0B=pwm2;
-#else /* USE_BUF_PWM */
- *TPU_TPGR0C=pwm1;
- *TPU_TPGR0D=pwm2;
-
-#endif /* USE_BUF_PWM */
-}
-
-#else /* >=VER_CODE(0,2,0) */
-
-static inline void
-pxmc_tpupwm2f_tpuwr(struct pxmc_state *mcs, short pwm1, short pwm2)
-{
-
-
- if(pwm1>0){
- pwm1+=mcs->pxms_pwm1cor;
-// atomic_set_mask_b1(PFDR_PF0DRm,DIO_PFDR);
- }else if(pwm1<0) {
- pwm1=-pwm1+mcs->pxms_pwm1cor;
-// atomic_clear_mask_b1(PFDR_PF0DRm,DIO_PFDR);
- }
- if(pwm2>0){
- pwm2+=mcs->pxms_pwm2cor;
-// atomic_clear_mask_b1(PFDR_PF1DRm,DIO_PFDR);
- }else if(pwm2<0) {
- pwm2=-pwm2+mcs->pxms_pwm2cor;
-// atomic_set_mask_b1(PFDR_PF1DRm,DIO_PFDR);
- }
-
- pwm1=((long)pwm1*pxmc_tpupwm2f_mult)>>16;
- pwm2=((long)pwm2*pxmc_tpupwm2f_mult)>>16;
- /* Output PWM requests into TPU registers */
- /* TPU logic becomes mad if width and period is same */
- /* and it cannot output zero width */
- /* full fill is possible by width longer than period */
-
-
-#ifndef USE_BUF_PWM
- *TPU_TGR0A=pwm1;
- *TPU_TGR0B=pwm2;
-#else /* USE_BUF_PWM */
- *TPU_TGR0C=pwm1;
- *TPU_TGR0D=pwm2;
-#endif /* USE_BUF_PWM */
-}
-
-#endif /* >=VER_CODE(0,2,0) */
-
-/**
- * pxmc_tpupwm2f_out - Two phase stepper motor phase PWM output
- * @mcs: Motion controller state information
- *
- * calls pxmc_tpupwm2f_tpuwr which, ifndef USE_BUF_PWM,
- * uses (TPU_TGR0A, TPU_TGR0B) timers for PWM registers
- * else (TPU_TGR0C, TPU_TGR0D)
- */
-int
-pxmc_tpupwm2f_out(struct pxmc_state *mcs)
-{
- short pwm1;
- short pwm2;
- short indx;
- short ene;
-
- indx=mcs->pxms_ptindx;
- ene=mcs->pxms_ene;
- if(ene==0){
- pwm1=pwm2=0;
- }else{
-#ifdef PXMC_USE_VANG
- if(ene<0){
- /* Generating direction of stator mag. field for backward torque */
- ene=-ene;
- if((indx-=mcs->pxms_ptvang)<0)
- indx+=mcs->pxms_ptirc;
- }else{
- /* Generating direction of stator mag. field for forward torque */
- if((indx+=mcs->pxms_ptvang)>=mcs->pxms_ptirc)
- indx-=mcs->pxms_ptirc;
- }
-#endif /* PXMC_USE_VANG */
- pwm1=mcs->pxms_ptptr1[indx];
- pwm2=mcs->pxms_ptptr2[indx];
-
- pwm1=((long)pwm1*ene)>>15;
- pwm2=((long)pwm2*ene)>>15;
-
- }
-
- pxmc_tpupwm2f_tpuwr(mcs,pwm1,pwm2);
- return 0;
-}
-
-int
-pxmc_dbg_pwm12(pxmc_state_t *mcs)
-{
- long *ptr;
- if(!(mcs->pxms_flg&PXMS_ENG_m))
- return 0;
- if(pxmc_dbg_hist){
- ptr=pxmc_dbg_hist->ptr;
- if(ptr&&(ptr<pxmc_dbg_hist->end-1)){
- *(ptr++)=*TPU_TGR0A;
- *(ptr++)=*TPU_TGR0B;
- pxmc_dbg_hist->ptr=ptr;
- }
- }
- return 0;
-}
-
-inline int
-pxmc_cpme_con(pxmc_state_t *mcs)
-{
- mcs->pxms_ene=mcs->pxms_me;
- return 0;
-}
-
-/*------------------------------------------------------------------*/
-/* Stepper motor without feedback */
-
-/**
- * pxmc_nofb_out - Phase output for open loop direct stepper motor control
- * @mcs: Motion controller state information
- */
-int
-pxmc_nofb_out(pxmc_state_t *mcs)
-{
- short pwm1;
- short pwm2;
- short indx;
- short ene;
-
- ene=mcs->pxms_ene;
- if(ene){
- pxmc_irc_16bit_commindx(mcs, mcs->pxms_ap>>PXMC_SUBDIV(mcs));
- indx=mcs->pxms_ptindx;
- pwm1=mcs->pxms_ptptr1[indx];
- pwm2=mcs->pxms_ptptr2[indx];
-
- pwm1=((long)pwm1*ene)>>15;
- pwm2=((long)pwm2*ene)>>15;
-
- pxmc_tpupwm2f_tpuwr(mcs,pwm1,pwm2);
- return 0;
- }else{
- pxmc_tpupwm2f_tpuwr(mcs,0,0);
- return 0;
- }
-}
-
-
-
-/**
- * pxmc_nofb_inp - Dummy input for direct stepper motor control
- * @mcs: Motion controller state information
- */
-int
-pxmc_nofb_inp(pxmc_state_t *mcs)
-{
- return 0;
-}
-
-/**
- * pxmc_nofb_con - Empty controller for direct stepper motor control
- * @mcs: Motion controller state information
- */
-int
-pxmc_nofb_con(pxmc_state_t *mcs)
-{
- mcs->pxms_ap=mcs->pxms_rp;
- mcs->pxms_as=mcs->pxms_rs;
- mcs->pxms_ene=mcs->pxms_me;
- return 0;
-}
-
-
-/*------------------------------------------------------------------*/
-
-/* sampling frequency interrupt handler */
-/* int intno, void *dev_id, struct pt_regs *regs */
-void pxmc_sfi_isr(void) __attribute__ ((interrupt_handler));
-
-/**
- * pxmc_sfi_isr - Stepper motor interrupt service routine
- *
- *(I do not understand the following comment, maybe these are features that will be written later??)
- *
- * This is basic routine calling motion controller functions.
- */
-
-
-void pxmc_sfi_isr(void)
-{
- short local_var=0xff;
-
- local_var=*TPU_TSR4;
- if(local_var & TSR4_TGFAm)
- {
- *TPU_TSR4&=~TSR4_TGFAm; //FIXME:zakazat nejak rozumne preruseni pro vsechny motory a ne jenom preruseni 4 tj.Motor4
- };
- local_var=*TPU_TSR5;
- if(local_var & TSR5_TGFAm)
- {
- *TPU_TSR5&=~TSR5_TGFAm; //FIXME:zakazat nejak rozumne preruseni pro vsechny motory a ne jenom preruseni 4 tj.Motor4
- };
-
- /* Clear interrupt source */
-
- /*PXMS_ENG=enable input (IRC) update */
- pxmc_tpu_interupt_eni(); /*macro for service Interupt for all defined motors*/
-
- /*PXMS_ENG - enable requested value (position) generator*/
- pxmc_tpu_interupt_eng(); /*macro for service Interupt for all defined motors*/
-
- /*enable controller*/
- pxmc_tpu_interupt_enr(); /*macro for service Interupt for all defined motors*/
-
- /* enable debugging */
- pxmc_tpu_interupt_dbg(); /*macro for service Interupt for all defined motors*/
-}
-
-/********************************************************************/
-/* Stepper controller tests */
-
-#include <math.h>
-
-/********************************************************************/
-/* DC motor controller hardware dependant part */
-
-
-/*------------------------------------------------------------------*/
-/* DC motor with IRC feedback */
-
-/**
- * pxmc_pwm1f_out - two phase PWM output for DC motor
- * @mcs: Motion controller state information
- *
- * This function sets directly duty cycle register in
- * PWM_PWBFR1n n=(A,C,E,G) buffer register.
- */
-int pxmc_pwm1f_out(struct pxmc_state *mcs)
-{
- short ene=0;
- ene=mcs->pxms_ene;
- if(ene>=0)
- {
-/* #ifdef BOARD_MIROSOT
- *(volatile __u16 *)(mcs->pxms_out_info) = ((ene) >> 7)|0x1000;
- #else */
- *(volatile __u16 *)(mcs->pxms_out_info) = ((ene) >> 7)&0xefff;
-// #endif
- }
-
- else
- {
-/* #ifdef BOARD_MIROSOT
- *(volatile __u16 *)(mcs->pxms_out_info) = (((-ene) >> 7))&0xefff;
- #else */
- *(volatile __u16 *)(mcs->pxms_out_info) = (((-ene) >> 7))|0x1000;
-// #endif
-
- }
- return 0;
-}
-
-/**
- * pxmc_axis_rdmode - Reads actual axis mode.[extern API]
- * @mcs: Motion controller state information
- */
-int
-pxmc_axis_rdmode(pxmc_state_t *mcs)
-{
- if(mcs->pxms_do_con==pxmc_nofb_con)
- return 1;
- if(mcs->pxms_do_con==pxmc_pid_con)
- return 2;
- if(mcs->pxms_do_con==pxmc_cpme_con)
- return 3;
- return 0;
-}
-
-/**************************area change ****************/
-/*pxmc.c - line 1586 and px,c%h2638.c line 494*/
-
- //fce pxmc_axis_mode - is not pxmc.c and here
-/**************************area change ****************/
-
-
-int tpu_irc_ap2hw(struct pxmc_state *mcs) {
- short old_pos;
- if(mcs->pxms_inp_info) {
-#ifdef PXMC_WITH_PHASE_TABLE
- pxmc_clear_flag(mcs,PXMS_PHA_b);
- old_pos = *(short *)mcs->pxms_inp_info;
- mcs->pxms_ptofs+=mcs->pxms_ap-old_pos;
-#endif
- *(short *)mcs->pxms_inp_info=mcs->pxms_ap;
- }
- return 0;
-}
-
-
-/**
- * pxmc_get_sfi_hz - Reads sampling frequency of axis
- * @mcs: Motion controller state information
- */
-long pxmc_get_sfi_hz(pxmc_state_t *mcs)
-{
- return (long)pxmc_sfikhz*1000;
-}
-
-#ifdef WITH_SFI_SEL
-/**
- * pxmc_sfi_sel - Setting sampling frequency of PXMC subsystem (TPU_TGR4A)
- * @sfi_hz: Requested sampling frequency in Hz
- *
- * Function returns newly selected sampling frequency
- * after rounding or -1 if there is problem to set
- * requested frequency.
- */
-long pxmc_sfi_sel(long sfi_hz)
-{
- int sfikhz=(sfi_hz+500)/1000;
- if((sfikhz<2)||(sfikhz>12))
- return -1;
- *TPU_TSTR&=~(1<<4); /* Stop channel 4 */
- pxmc_sfikhz=sfikhz;
- *TPU_TCNT4=0; //FIXME: Use macro PCMC_SFI_SEL
- *TPU_TGR4A=((CPU_SYS_HZ+500)/1000)/pxmc_sfikhz;
- *TPU_TSTR|=(1<<4); /* Start channel 4 */
- return (long)pxmc_sfikhz*1000l;
-}
-#endif /* WITH_SFI_SEL */
-
-/**
- * pxmc_axis_mode - Sets axis mode.[extern API]
- * @mcs: Motion controller state information
- * @mode: 0 .. previous mode, 1 .. stepper motor mode,
- * 2 .. stepper motor with IRC feedback and PWM ,
- * 3 .. stepper motor with PWM control
- * 4 .. DC motor with IRC feedback and PWM
- *
- */
-int
-pxmc_axis_mode(pxmc_state_t *mcs, int mode)
-{
-// int ret;
- pxmc_set_const_out(mcs,0);
- pxmc_clear_flag(mcs,PXMS_ENI_b);
- pxmc_clear_flag(mcs,PXMS_PHA_b);
-
- if(!mode){
- mode=pxmc_axis_rdmode(mcs);
- if(!mode) mode=1;
- }
-
- switch(mode){
-// case 1 : /* Stepper motor mode */
-// #if HW_VER_CODE>=VER_CODE(0,2,0)
-// if((ret=pxmc_init_ptable(mcs,0))<0)
-// return ret;
-// /* necessary to initialize TPU for phase current control */
-// pxmc_stm_init_fbmode(mcs);
-// #endif /*HW_VER_CODE>=VER_CODE(0,2,0)*/
-// mcs->pxms_do_inp=pxmc_nofb_inp;
-// mcs->pxms_do_out=pxmc_nofb_out;
-// mcs->pxms_do_con=pxmc_nofb_con;
-// break;
-//
-// case 2 : /* Stepper motor with IRC feedback and PWM */
-// if((ret=pxmc_init_ptable(mcs,0))<0)
-// return ret;
-// pxmc_stm_init_fbmode(mcs);
-// mcs->pxms_do_con=pxmc_pid_con;
-//
-// break;
-//
-// case 3 : /* Stepper motor with PWM control */
-// if((ret=pxmc_init_ptable(mcs,0))<0)
-// return ret;
-// pxmc_stm_init_fbmode(mcs);
-// mcs->pxms_ptshift=0;
-// mcs->pxms_ptvang=0;
-// mcs->pxms_do_inp=pxmc_tpuirc_nofb_inp;
-// mcs->pxms_do_con=pxmc_cpme_con;
-// break;
-
- case 4 : /* DC motor with IRC feedback and PWM */
- pxmc_dcm_init_fbmode(mcs);
- break;
- default : return -1;
- }
- pxmc_set_flag(mcs,PXMS_ENI_b);
- return 0;
-}
-
-
-
-/**
- * pxmc_dcm_init_fbmode - Initializes feedback mode control
- * @mcs: Motion controller state information
- *
- * Function initializes TPU for IRC input and one phase
- * PWM output, generated by PWM unit, not TPU.
- *
- * -> IRC inputs are TCLKC and TCLKD,
- * Timer 2 is in phase counting mode 1.
- * Timer 4 is used to count sampling period (set in TPU_TGR4A)
- * and activate pxmc_sfi_isr interrupt handler.
- * -> PWM outputs are A and B, the frequency of the signal is set in PWM_PWCYR1.
- */
-
-void pxmc_set_pwm_tpu(void)
-{
- int num_motors = pxmc_main_list.pxml_cnt;
-
- *SYS_MSTPCRD &= ~MSTPCRD_PWMm; /*set up Gate as PWM gate (not I/O)*/
- *SYS_MSTPCRA &= ~MSTPCRA_TPUm; /*set up Gate as TPU (not I/O) */
-
- *PWM_PWCR1 =0xC4;//interrupt disabled, counter stopped, frequency phi/16
-
- if(num_motors>0) /*motor 1 - output PWM 1A and 1B*/
- {
- /*PWM seting*/
-// #ifdef BOARD_MIROSOT
-// *PWM_PWOCR1 |= (PWOCR1_OE1Am | PWOCR1_OE1Bm );
-// *PWM_PWPR1 &= ~((PWPR1_OPS1Am)|(PWPR1_OPS1Bm));
-// #else
- *PWM_PWOCR1 |= (PWOCR1_OE1Am | PWOCR1_OE1Bm );
- *PWM_PWPR1 &= ~((PWPR1_OPS1Am)|(PWPR1_OPS1Bm));
-// #endif
-// *PWM_PWPR1 &= ~((PWPR1_OPS1Am)|(PWPR1_OPS1Bm));
- /*TPU seting*/
-#if 1
- *DIO_P1DDR &= ~(P1DDR_P12DDRm | P1DDR_P13DDRm); /*TCLKA and TCLKB are inputs*/
- PXMC_TPU_SFI_INIT(PXMC_SFI_TPU_CHN);
- PXMC_TPU_PHASE_CNT_INIT(1); /*macro for inicialized TPU - mod 1*/
-
-#endif
- };
- if(num_motors>1) /*motor 2 - output PWM 1C and 1D*/
- {
-// #ifdef BOARD_MIROSOT
-// *PWM_PWOCR1 |= (PWOCR1_OE1Em | PWOCR1_OE1Fm );
-// *PWM_PWPR1 &= ~((PWPR1_OPS1Em)|(PWPR1_OPS1Fm));
-// #else
- *PWM_PWOCR1 |= (PWOCR1_OE1Cm | PWOCR1_OE1Dm );
- *PWM_PWPR1 &= ~((PWPR1_OPS1Cm)|(PWPR1_OPS1Dm));
-// #endif
- /*TPU seting*/
- *DIO_P1DDR &= ~(P1DDR_P15DDRm | P1DDR_P17DDRm); /*TCLKC and TCLKD are inputs*/
-// PXMC_TPU1(4); /*macro for inicialized TPU*/
- PXMC_TPU_PHASE_CNT_INIT(2); /*macro for inicialized TPU - mod 1*/
- };
- if(num_motors>2) /*motor 3 - output PWM 1E and 1F*/
- {
- /*PWM seting*/
- *PWM_PWOCR1 |= (PWOCR1_OE1Em | PWOCR1_OE1Fm );
- *PWM_PWPR1 &= ~((PWPR1_OPS1Em)|(PWPR1_OPS1Fm));
- };
- if(num_motors>3) /*motor 4 - output PWM 1G and 1H*/
- {
- *PWM_PWOCR1 = (PWOCR1_OE1Gm | PWOCR1_OE1Hm );
- *PWM_PWPR1 &= ~((PWPR1_OPS1Gm)|(PWPR1_OPS1Hm));
- };
- if(num_motors>0) /*motor 1 - output PWM 1A and 1B*/
- {
- *PWM_PWCYR1 = pwm_per; //constant set up period of PWM counter
- *PWM_PWCR1 |=PWCR1_CSTm; /*start PWM counter*/
- };
-}
-
-void
-pxmc_dcm_init_fbmode(pxmc_state_t *mcs)
-{
- mcs->pxms_do_inp=pxmc_tpuirc_inp; /*set up pointer on function measure IRC*/
- mcs->pxms_do_out=pxmc_pwm1f_out; //pointer on funct. control energi for outpu (PWM)
- mcs->pxms_do_ap2hw=tpu_irc_ap2hw;
- int HelVar=pwm_per;
- mcs->pxms_me = HelVar << 7; //max energy enter in a motor
-
- /* debug PWM generation */
- if(!pxmc_dbg_hist)
- pxmc_dbg_hist=0; /* FIXME: Are here correct braces??? */
- pxmc_dbg_hist=pxmc_dbg_histalloc(4096);
- pxmc_dbgset(mcs, pxmc_dbg_pwm12, 1);
- pxmc_set_flag(mcs,PXMS_DBG_b);
- mcs->pxms_do_con=pxmc_pid_con; /*set up pointer on PID con. function (implicit
- (PXMC.c) or user*/
-}
-
-
-#if 0
-/**
- * pxmc_stm_init - Initializes stepper or brush-less motor subsystem
- *
- * Configuration of IO registers, TPU for phase counting mode.
- * Returns 0 if the axis mode is set correctly.
- *
- * Timers 2 or 5 are in phase counting mode 1.
- * Choose which one defining PXMC_IRC_TPUN.
- * Timer 4 is used to count sampling period (set in TPU_TGR4A)
- * and activate pxmc_sfi_isr interrupt handler.
- *
- */
-int
-pxmc_stm_init(void)
-{
- #if HW_VER_CODE<VER_CODE(0,2,0)
- ID_STM_ENABLE(0);
- /*SHADDOW_REG_SET(DIO_P1DDR,0x0f);*/
- #else /* >=VER_CODE(0,2,0) */
- /* P10, P11 .. PWM */
- /* P15 .. synchronization for PWM */
- /* P12, P13 .. IRC */
- /* P16 .. THUMB inp, P17 .. THUMB output */
- SHADDOW_REG_CLR(DIO_P1DDR,0x5c);
- SHADDOW_REG_SET(DIO_P1DDR,0xa3);
- /* PF0, PF1 .. phase direction registers */
- SHADDOW_REG_SET(DIO_PFDDR,0x03);
- #endif /* >=VER_CODE(0,2,0) */
- pxmc_main_list.pxml_arr[0]->pxms_ap+=0x100; /*!!!*/
- if(pxmc_main_list.pxml_arr[0]->pxms_ene<0x1800)
- pxmc_main_list.pxml_arr[0]->pxms_ene+=0x0400;
-
- pxmc_sfikhz=1; /* 5 kHz fast sampling interval for stepper motors; 1kHz for normal CD brash motors*/
-
- pxmc_clear_flag(pxmc_main_list.pxml_arr[0],PXMS_ENG_b);
- pxmc_clear_flag(pxmc_main_list.pxml_arr[0],PXMS_ENR_b);
-
- #if HW_VER_CODE<VER_CODE(0,2,0)
- *SYS_MSTPCRC &= ~MSTPCRC_DA23m;
- *DA_DACR23|= (DACR23_DAEm*0) | DACR23_DAOE0m;
-
- pxmc_nofb_out(&mcs0);
-
- ID_STM_ENABLE(1);
- #endif /* <VER_CODE(0,2,0) */
-
- /* TPU initialization */
- *SYS_MSTPCRA&=~MSTPCRA_TPUm;
- *TPU_TSTR&=~(1<<4); /* Stop channel 4 */
- /* system clock/1 ,rising edge, clearing source TGRA */
- *TPU_TCR4=(TPCR_TPSC_F1 | TPCR_CKEG_RIS | TPCR_CCLR_TGRA);
- /* normal mode */
- *TPU_TMDR4=TPMDR_MD_NORMAL;
- /* TGRA initiates interrupt */
- excptvec_set(EXCPTVEC_TGI4A,pxmc_sfi_isr);
- *TPU_TIER4=TIER4_TGIEAm;
-
- *TPU_TGR4A=((CPU_SYS_HZ+500)/1000)/pxmc_sfikhz;
-
- *TPU_TSTR|=(1<<4); /* Start channel 4 */
-
- #if PXMC_IRC_TPUN==2
- /* IRC input initialization for channel 2 */
- *TPU_TPSTR&=~(1<<2);/* Stop channel 2 */
- /* system clock/1 ,rising edge */
- *TPU_TPCR2=0;
- /* phase counting mode 1 */
- *TPU_TPMDR2=TPMDR_MD_PHACN1;
- /* no irq */
- *TPU_TPIER2=0;
- *TPU_TPSTR|=(1<<2); /* Start channel 2 */
- #elif PXMC_IRC_TPUN==5
- /* IRC input initialization for channel 5 */
- *TPU_TSTR&=~(1<<5);/* Stop channel 5 */
- /* system clock/1 ,rising edge */
- *TPU_TCR5=0;
- /* phase counting mode 1 */
- *TPU_TMDR5=TPMDR_MD_PHACN1;
- /* no irq */
- *TPU_TIER5=0;
- *TPU_TSTR|=(1<<5); /* Start channel 5 */
- #endif /* PXMC_IRC_TPUN */
-
- /* set working mode for axis */
- return pxmc_axis_mode(pxmc_main_list.pxml_arr[0], 1);
-}
-#endif
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc_h2638.h - generic multi axis motion controller
- hitachi H8S 2638 specific functions
-
- (C) 2001-2005 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (C) 2002-2005 by PiKRON Ltd. http://www.pikron.com
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
-
- *******************************************************************/
-
-#ifndef _PXMC_H2638_H
-#define _PXMC_H2638_H
-
-int pxmc_pwm1f_out(struct pxmc_state *mcs);
-
-int pxmc_tpuirc_inp(struct pxmc_state *mcs);
-
-void pxmc_sfi_isr(void);
-
-int pxmc_dbg_pwm12(pxmc_state_t *mcs);
-
-int pxmc_pid_con(pxmc_state_t *mcs);
-
-int pxmc_nofb_inp(pxmc_state_t *mcs);
-
-int pxmc_nofb_con(pxmc_state_t *mcs);
-
-int pxmc_nofb_out(pxmc_state_t *mcs);
-
-int tpu_irc_ap2hw(struct pxmc_state *mcs);
-
-/* select operating mode of axis */
-int pxmc_axis_mode(pxmc_state_t *mcs, int mode);
-
-void pxmc_dcm_init_fbmode(pxmc_state_t *mcs);
-
-void pxmc_set_pwm_tpu(void);
-
-void pxmc_add_pservice_and_mode(short mod);
-#endif
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc_h2638.c - generic multi axis motion controller
- h8s2638 hrdware specific functions
-//
- (C) 2001-2005 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (C) 2002-2005 by PiKRON Ltd. http://www.pikron.com
- (C) 2005 Petr Kovacik <kovacp1@Guillaume>
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
-
- *******************************************************************/
-
-/* [extern API] characteristic means that the function is declared
-in the header file pxmc.h so it is part of the external API */
-
-#include <types.h>
-#include <cpu_def.h>
-#include <h8s2638h.h>
-#include <system_def.h>
-#include <string.h>
-#include "pxmc.h"
-#include "pxmc_internal.h"
-#include "pxmc_inp_common.h"
-#include "pxmc_h2638.h"
-
-#ifndef PXMC_SFI_TPU_CHN
-#define PXMC_SFI_TPU_CHN 4 /* TPU channel used for timing */
-#endif
-
-// #define PXMC_USE_VANG /* Use pxms_ptvang for mag.fld. offs */
-//
-// #define PXMC_WITH_TPUCUR /* Measurement of phase current by TPU */
-
-/* Phase counting clock sources */
-/* TPCNT1, TPCNT5 .. TCLKA, TCLKB */
-/* TPCNT2, TPCNT4 .. TCLKC, TCLKD */
-
-// #if HW_VER_CODE<VER_CODE(0,2,0)
-// #define PXMC_IRC_TPUN 2 /* TPU channel used for IRC */
-// #else /* >=VER_CODE(0,2,0) */
-// #define PXMC_IRC_TPUN 5 /* TPU channel used for IRC */
-// #endif /* >=VER_CODE(0,2,0) */
-
-//pxmc_state_t mcs0;
-
-int pxmc_sfikhz=2;
-int pxmc_sfi2msec;
-#define pxmc_cyclekhz 20
-#define pwm_per (((CPU_SYS_HZ/16)/1000)/pxmc_cyclekhz)
-
-
-#define TEMP_PXMC_TPU_SFI_INIT(n)\
- do{ \
- *TPU_TSTR&=~TSTR_CST##n##m; /*counter n is stoped*/\
- /*CNTN cleared by by TGRA, rising edge; source TGRA compare match/input capture */\
- *TPU_TCR##n=(TPCR_TPSC_F1 | TPCR_CKEG_RIS | TPCR_CCLR_TGRA); \
- /*TGRA is output compare registr - . 0 output at compare match*/\
- *TPU_TIOR##n|=TIOR##n##_IOA1m; \
- /*MDn = 0x000 normal operation */\
- *TPU_TMDR##n=TPMDR_MD_NORMAL; \
- /*seting interupt vector - from TGRA at registr TSRn*/\
- excptvec_set(EXCPTVEC_TGI##n##A,pxmc_sfi_isr); \
- /*TimerInteruptEnableReg - enable IRQ generation from TGRA*/\
- *TPU_TIER##n |=TIER##n##_TGIEAm; \
- /*setup TGRA - sampling period*/\
- *TPU_TGR##n##A=(CPU_SYS_HZ/1000)/pxmc_sfikhz; \
- /*TimerStatusRegistr - clear match flag */\
- *TPU_TSR##n &=~TSR##n##_TGFAm; \
- /*Start TCNTn*/\
- *TPU_TSTR|=(1<<n);\
- }while (0)
-
-// TRICK: See cpp info pages, node Argument Prescan,
-// parahraph "Macros that call other macros that stringify or concatenate."
-#define PXMC_TPU_SFI_INIT(n) TEMP_PXMC_TPU_SFI_INIT(n)
-
-/*---------------------------------------------------------------------*/
-
-#define TEMP_PXMC_TPU_PHASE_CNT_INIT(n) \
- do{ \
- /*TimerControlRegistr - TCNT clearing disabled; count rasing adge; internal clock fi/1 */\
- *TPU_TCR##n=0; \
- /*phase counting mode 1 - how will be TCNT incrementd */\
- *TPU_TMDR##n=TPMDR_MD_PHACN1; \
- /*AD conversion disabled; interrupts from TCFU(TCFV,TGIA,TGIB) all interupt disabled.; */\
- *TPU_TIER##n=0; \
- /*start TCNT*/\
- *TPU_TSTR|=(1<<n); \
- }while (0)
-
-// TRICK: See cpp info pages, node Argument Prescan,
-// parahraph "Macros that call other macros that stringify or concatenate."
-#define PXMC_TPU_PHASE_CNT_INIT(n) TEMP_PXMC_TPU_PHASE_CNT_INIT(n)
-
-
-extern void deb_led_out(char val);
-
-#if 0 //FOR DEBUGGING
-
-#define error_led(led) \
- if (mcs->pxms_flg & PXMS_ERR_m) { \
- deb_led_out(led); \
- while(1);\
- }\
-
-#else
-#define error_led(led)
-#endif
-//-----------------------------
-// pxmc_axis_rdmode() - they setting up mode=4 but it's impos. to read it
-// _MOTOR_MODE_ - maybe it should be motor type (?)
-// what is here exactly mode?
-// what is 1 - near #if 1
-// pxmc_set_pwm_tpu_new - wow that was stayed is einfach great :o)
-// PXMC_IRC_TPUN - what is it?
-// HW_VER_CODE<VER_CODE(0,2,0) - what (for) is this
-//-----------------------------
-enum _BOOL_ {false=0,true=1};
-typedef enum _BOOL_ bool;
-
-/**
- * 0 .. previous mode,
- * 1 .. stepper motor mode,
- * 2 .. stepper motor with IRC feedback and PWM ,
- * 3 .. stepper motor with PWM control
- * 4 .. DC motor with IRC feedback and PWM
-*/
-enum _MOTOR_MODE_ {m_stepper=1,m_stepper_irc_pwm=2,m_stepper_pwn=3,m_dc_irc_pwm=4};
-typedef enum _MOTOR_MODE_ motor_mode_e;
-
-inline bool pxmc_set_default_services(pxmc_state_t *mcs,motor_mode_e mode)
-{
- switch(mode)
- {
- case m_stepper:
- /*mcs->pxms_do_inp=pxmc_nofb_inp;
- mcs->pxms_do_con=pxmc_nofb_con;
- mcs->pxms_do_out=pxmc_nofb_out;*/
- break;
- case m_stepper_irc_pwm:
- //mcs->pxms_do_inp=pxmc_tpuirc_inp; /*set up pointer on function measure IRC*/
- //mcs->pxms_do_out=pxmc_pwm1f_out; //pointer on funct. control energi for outpu (PWM)
- //pxmc_tpupwm2f_tpuwr
- //pxmc_tpuirc_nofb_inp
- //pxmc_tpupwm2f_out
- //pxmc_cpme_con
- break;
- case m_stepper_pwn:
- break;
- case m_dc_irc_pwm:
- /*mcs->pxms_do_inp=pxmc_nofb_inp;
- mcs->pxms_do_con=pxmc_nofb_con;
- mcs->pxms_do_out=pxmc_nofb_out;
- */
- mcs->pxms_do_inp=pxmc_tpuirc_inp; /*set up pointer on function measure IRC*/
- mcs->pxms_do_out=pxmc_pwm1f_out; //pointer on funct. control energi for outpu (PWM)
- mcs->pxms_do_con=pxmc_pid_con; /*set up pointer on PID con. function (implicit (PXMC.c) or user*/
- break;
- default:
- return false;
- }
-
- return true;
-}
-
-inline short pxmc_set_max_energy(pxmc_state_t *mcs)
-{
- int HelVar=pwm_per;
- return (mcs->pxms_me = HelVar << 7); //max energy enter in a motor
-}
-
-inline bool pxmc_set_default_debug(pxmc_state_t *mcs,motor_mode_e mode)
-{
- switch(mode)
- {
- case m_stepper:
- break;
- case m_stepper_irc_pwm:
- break;
- case m_stepper_pwn:
- break;
- case m_dc_irc_pwm:
- if(!pxmc_dbg_hist) pxmc_dbg_hist=0; /* FIXME: Are here correct braces??? */
- pxmc_dbg_hist=pxmc_dbg_histalloc(4096);
- pxmc_dbgset(mcs, pxmc_dbg_pwm12, 1);
- pxmc_set_flag(mcs,PXMS_DBG_b);
- break;
- default:
- return false;
- }
-
- return true;
-}
-
-
-void pxmc_set_pwm_tpu_new(void)
-{
- int num_motors = pxmc_main_list.pxml_cnt;
-
- *SYS_MSTPCRD &= ~MSTPCRD_PWMm; /*set up Gate as PWM gate (not I/O)*/
- *SYS_MSTPCRA &= ~MSTPCRA_TPUm; /*set up Gate as TPU (not I/O) */
-
- *PWM_PWCR1 =0xC4;//interrupt disabled, counter stopped, frequency phi/16
-
- unsigned char pwm_pwocr1[]={PWOCR1_OE1Am,PWOCR1_OE1Bm,PWOCR1_OE1Cm,PWOCR1_OE1Dm,\
- PWOCR1_OE1Em,PWOCR1_OE1Fm,PWOCR1_OE1Gm,PWOCR1_OE1Hm};
-
- unsigned char pwm_pwpr1[]={PWPR1_OPS1Am,PWPR1_OPS1Bm,PWPR1_OPS1Cm,PWPR1_OPS1Dm,\
- PWPR1_OPS1Em,PWPR1_OPS1Fm,PWPR1_OPS1Gm,PWPR1_OPS1Hm};
-
- int r=0;
- for(r=0;r<num_motors;r++)
- { // motors - output PWM 1A and 1B
- *PWM_PWOCR1 |= ( pwm_pwocr1[2*r] | pwm_pwocr1[2*r+1] );
- *PWM_PWPR1 &= ~((pwm_pwpr1[2*r]) | (pwm_pwpr1[2*r+1]));
- }
-
- if(num_motors>0) /*motor 1 - output PWM 1A and 1B*/
- {
- /*TPU seting*/
-#if 1
- *DIO_P1DDR &= ~(P1DDR_P12DDRm | P1DDR_P13DDRm); /*TCLKA and TCLKB are inputs*/
- PXMC_TPU_SFI_INIT(PXMC_SFI_TPU_CHN);
- PXMC_TPU_PHASE_CNT_INIT(1); /*macro for inicialized TPU - mod 1*/
-#endif
- };
- if(num_motors>1) /*motor 2 - output PWM 1C and 1D*/
- {
- /*TPU seting*/
- *DIO_P1DDR &= ~(P1DDR_P15DDRm | P1DDR_P17DDRm); /*TCLKC and TCLKD are inputs*/
-// PXMC_TPU1(4); /*macro for inicialized TPU*/
- PXMC_TPU_PHASE_CNT_INIT(2); /*macro for inicialized TPU - mod 1*/
- };
- //????????????????????
- // WLACZENIE PWM jesli jest choc jeden silnik ???????????
-//????????????????????
- if(num_motors>0) /*motor 1 - output PWM 1A and 1B*/
- {
- *PWM_PWCYR1 = pwm_per; //constant set up period of PWM counter
- *PWM_PWCR1 |=PWCR1_CSTm; /*start PWM counter*/
- };
-}
-
-
-//-----------------------------
-__attribute__ ((deprecated)) inline void pxmc_add_pservice_and_mode(short mod)
-{
- int var;
- for (var = 0; var < pxmc_main_list.pxml_cnt; var++) {
- /* Set default motor behaviour */
- /*pxmc_main_list.pxml_arr[var]->pxms_do_inp=pxmc_nofb_inp;
- pxmc_main_list.pxml_arr[var]->pxms_do_con=pxmc_nofb_con;
- pxmc_main_list.pxml_arr[var]->pxms_do_out=pxmc_nofb_out;
- */
- pxmc_set_default_services(pxmc_main_list.pxml_arr[var],mod);
- pxmc_axis_mode(pxmc_main_list.pxml_arr[var], mod);
- }
-}
-
-
-/* direct, one mcs processing */
-inline void pxmc_tpu_interupt_eni()
-{
- int var=0;
- for (var = 0; var < pxmc_main_list.pxml_cnt; var++) {
- /* direct, one mcs processing */
- if(pxmc_main_list.pxml_arr[var]->pxms_flg&PXMS_ENI_m)
- {
- /* pxmc_nofb_inp(mcs); */
- pxmc_call(pxmc_main_list.pxml_arr[var],pxmc_main_list.pxml_arr[var]->pxms_do_inp);
- error_led(~1);
- }
- }
-}
-
-
-inline void pxmc_tpu_interupt_eng()
-{
- int var=0;
- for (var = 0; var < pxmc_main_list.pxml_cnt; var++) {
- if(pxmc_main_list.pxml_arr[var]->pxms_flg&PXMS_ENG_m)
- {
- /* pxmc_spdfg_gnr(pxmc_main_list.pxml_arr[var]); */
- pxmc_call(pxmc_main_list.pxml_arr[var],pxmc_main_list.pxml_arr[var]->pxms_do_gen);
- error_led(~2);
- }
- }
-}
-
-inline void pxmc_tpu_interupt_enr()
-{
- int var=0;
- for (var = 0; var < pxmc_main_list.pxml_cnt; var++) {
- if(pxmc_main_list.pxml_arr[var]->pxms_flg&PXMS_ENR_m)
- {
- /* pxmc_nofb_con(mcs); */
- pxmc_call(pxmc_main_list.pxml_arr[var],pxmc_main_list.pxml_arr[var]->pxms_do_con);
- error_led(~4);
- if(pxmc_main_list.pxml_arr[var]->pxms_flg&PXMS_ERR_m)
- pxmc_main_list.pxml_arr[var]->pxms_ene=0;
- /* pxmc_nofb_out(mcs); */
- pxmc_call(pxmc_main_list.pxml_arr[var],pxmc_main_list.pxml_arr[var]->pxms_do_out);
- error_led(1);
- }
- }
-}
-
-
-inline void pxmc_tpu_interupt_dbg()
-{
- int var=0;
- for (var = 0; var < pxmc_main_list.pxml_cnt; var++) {
- if(pxmc_main_list.pxml_arr[var]->pxms_flg&PXMS_DBG_m) {
- pxmc_call(pxmc_main_list.pxml_arr[var],pxmc_main_list.pxml_arr[var]->pxms_do_deb);
- error_led(2);
- }
- if(--pxmc_sfi2msec<=0) {
- msec_time++;
- pxmc_sfi2msec=pxmc_sfikhz;
- /* Hook for external finite state machine */
- }else {
- /* Run ADC measurements */
- }
- }
-}
-
-/********************************************************************/
-/* Stepper controller hardware dependant part */
-
-#define USE_BUF_PWM
-
-/*------------------------------------------------------------------*/
-/* Stepper motor with IRC feedback */
-
-short pxmc_tpupwm2f_mult;
-//int pxmc_sfikhz;
-
-/**
- * pxmc_tpuirc_inp - IRC encoder connected to H8S 2638 TPU
- * @mcs: Motion controller state information
- *
- * This version of input routine updates @pxms_ap from TPU counter.
- * Acquired value is then used for phase commutation..
- */
-int pxmc_tpuirc_inp(struct pxmc_state *mcs)
-{
- short irc;
- irc=*(volatile short*)mcs->pxms_inp_info;
- pxmc_irc_16bit_update(mcs,irc);
-
- /* Running of the motor commutator */
- if(mcs->pxms_flg&PXMS_PTI_m)
- pxmc_irc_16bit_commindx(mcs,irc);
-
- return 0;
-}
-
-/**
- * pxmc_tpuirc_nofb_inp - TPU IRC input without feedback commutation control
- * @mcs: Motion controller state information
- *
- * This version of input routine updates @pxms_ap from TPU counter,
- * but commutation is controlled by @pxms_rp.
- */
-inline int pxmc_tpuirc_nofb_inp(struct pxmc_state *mcs)
-{
- short irc;
- irc=*(volatile short*)mcs->pxms_inp_info;
- pxmc_irc_16bit_update(mcs,irc);
-
- /* Running of the motor commutator */
- irc=mcs->pxms_rp>>PXMC_SUBDIV(mcs);
- if(mcs->pxms_flg&PXMS_PTI_m)
- pxmc_irc_16bit_commindx(mcs,irc);
-
- return 0;
-}
-
-#if HW_VER_CODE<VER_CODE(0,2,0)
-
-static inline void pxmc_tpupwm2f_tpuwr(struct pxmc_state *mcs, short pwm1, short pwm2)
-{
- char mask=0;
-
- if(pwm1>0) mask|=1;
- else if(pwm1<0) {
- mask|=2;
- pwm1=-pwm1;
- }
- if(pwm2>0) mask|=4;
- else if(pwm2<0) {
- mask|=8;
- pwm2=-pwm2;
- }
- *STPM_OUT=mask;
-
- pwm1=((long)pwm1*pxmc_tpupwm2f_mult)>>16;
- pwm2=((long)pwm2*pxmc_tpupwm2f_mult)>>16;
- /* Output PWM requests into TPU registers */
- /* TPU logic becomes mad if width and period is same */
- /* and it cannot output zero width */
- /* full fill is possible by width longer than period */
-#ifndef USE_BUF_PWM
- *TPU_TPGR0A=pwm1;
- *TPU_TPGR0B=pwm2;
-#else /* USE_BUF_PWM */
- *TPU_TPGR0C=pwm1;
- *TPU_TPGR0D=pwm2;
-
-#endif /* USE_BUF_PWM */
-}
-
-#else /* >=VER_CODE(0,2,0) */
-
-static inline void pxmc_tpupwm2f_tpuwr(struct pxmc_state *mcs, short pwm1, short pwm2)
-{
- if(pwm1>0){
- pwm1+=mcs->pxms_pwm1cor;
-// atomic_set_mask_b1(PFDR_PF0DRm,DIO_PFDR);
- }else if(pwm1<0) {
- pwm1=-pwm1+mcs->pxms_pwm1cor;
-// atomic_clear_mask_b1(PFDR_PF0DRm,DIO_PFDR);
- }
- if(pwm2>0){
- pwm2+=mcs->pxms_pwm2cor;
-// atomic_clear_mask_b1(PFDR_PF1DRm,DIO_PFDR);
- }else if(pwm2<0) {
- pwm2=-pwm2+mcs->pxms_pwm2cor;
-// atomic_set_mask_b1(PFDR_PF1DRm,DIO_PFDR);
- }
-
- pwm1=((long)pwm1*pxmc_tpupwm2f_mult)>>16;
- pwm2=((long)pwm2*pxmc_tpupwm2f_mult)>>16;
- /* Output PWM requests into TPU registers */
- /* TPU logic becomes mad if width and period is same */
- /* and it cannot output zero width */
- /* full fill is possible by width longer than period */
-
-
-#ifndef USE_BUF_PWM
- *TPU_TGR0A=pwm1;
- *TPU_TGR0B=pwm2;
-#else /* USE_BUF_PWM */
- *TPU_TGR0C=pwm1;
- *TPU_TGR0D=pwm2;
-#endif /* USE_BUF_PWM */
-}
-
-#endif /* >=VER_CODE(0,2,0) */
-
-/**
- * pxmc_tpupwm2f_out - Two phase stepper motor phase PWM output
- * @mcs: Motion controller state information
- *
- * calls pxmc_tpupwm2f_tpuwr which, ifndef USE_BUF_PWM,
- * uses (TPU_TGR0A, TPU_TGR0B) timers for PWM registers
- * else (TPU_TGR0C, TPU_TGR0D)
- */
-int pxmc_tpupwm2f_out(struct pxmc_state *mcs)
-{
- short pwm1;
- short pwm2;
- short indx;
- short ene;
-
- indx=mcs->pxms_ptindx;
- ene=mcs->pxms_ene;
- if(ene==0){
- pwm1=pwm2=0;
- }else{
-#ifdef PXMC_USE_VANG
- if(ene<0){
- /* Generating direction of stator mag. field for backward torque */
- ene=-ene;
- if((indx-=mcs->pxms_ptvang)<0)
- indx+=mcs->pxms_ptirc;
- }else{
- /* Generating direction of stator mag. field for forward torque */
- if((indx+=mcs->pxms_ptvang)>=mcs->pxms_ptirc)
- indx-=mcs->pxms_ptirc;
- }
-#endif /* PXMC_USE_VANG */
- pwm1=mcs->pxms_ptptr1[indx];
- pwm2=mcs->pxms_ptptr2[indx];
-
- pwm1=((long)pwm1*ene)>>15;
- pwm2=((long)pwm2*ene)>>15;
-
- }
-
- pxmc_tpupwm2f_tpuwr(mcs,pwm1,pwm2);
- return 0;
-}
-
-int pxmc_dbg_pwm12(pxmc_state_t *mcs)
-{
- long *ptr;
- if(!(mcs->pxms_flg&PXMS_ENG_m))
- return 0;
- if(pxmc_dbg_hist){
- ptr=pxmc_dbg_hist->ptr;
- if(ptr&&(ptr<pxmc_dbg_hist->end-1)){
- *(ptr++)=*TPU_TGR0A;
- *(ptr++)=*TPU_TGR0B;
- pxmc_dbg_hist->ptr=ptr;
- }
- }
- return 0;
-}
-
-inline int pxmc_cpme_con(pxmc_state_t *mcs)
-{
- mcs->pxms_ene=mcs->pxms_me;
- return 0;
-}
-
-/*------------------------------------------------------------------*/
-/* Stepper motor without feedback */
-
-/**
- * pxmc_nofb_out - Phase output for open loop direct stepper motor control
- * @mcs: Motion controller state information
- */
-int pxmc_nofb_out(pxmc_state_t *mcs)
-{
- short pwm1;
- short pwm2;
- short indx;
- short ene;
-
- ene=mcs->pxms_ene;
- if(ene){
- pxmc_irc_16bit_commindx(mcs, mcs->pxms_ap>>PXMC_SUBDIV(mcs));
- indx=mcs->pxms_ptindx;
- pwm1=mcs->pxms_ptptr1[indx];
- pwm2=mcs->pxms_ptptr2[indx];
-
- pwm1=((long)pwm1*ene)>>15;
- pwm2=((long)pwm2*ene)>>15;
-
- pxmc_tpupwm2f_tpuwr(mcs,pwm1,pwm2);
- return 0;
- }else{
- pxmc_tpupwm2f_tpuwr(mcs,0,0);
- return 0;
- }
-}
-
-
-
-/**
- * pxmc_nofb_inp - Dummy input for direct stepper motor control
- * @mcs: Motion controller state information
- */
-int pxmc_nofb_inp(pxmc_state_t *mcs)
-{
- return 0;
-}
-
-/**
- * pxmc_nofb_con - Empty controller for direct stepper motor control
- * @mcs: Motion controller state information
- */
-int
-pxmc_nofb_con(pxmc_state_t *mcs)
-{
- mcs->pxms_ap=mcs->pxms_rp;
- mcs->pxms_as=mcs->pxms_rs;
- mcs->pxms_ene=mcs->pxms_me;
- return 0;
-}
-
-
-/*------------------------------------------------------------------*/
-
-/* sampling frequency interrupt handler */
-/* int intno, void *dev_id, struct pt_regs *regs */
-void pxmc_sfi_isr(void) __attribute__ ((interrupt_handler));
-
-/**
- * pxmc_sfi_isr - Stepper motor interrupt service routine
- *
- *(I do not understand the following comment, maybe these are features that will be written later??)
- *
- * This is basic routine calling motion controller functions.
- */
-
-
-void pxmc_sfi_isr(void)
-{
- short local_var=0xff;
-
- local_var=*TPU_TSR4;
- if(local_var & TSR4_TGFAm)
- {
- *TPU_TSR4&=~TSR4_TGFAm; //FIXME:zakazat nejak rozumne preruseni pro vsechny motory a ne jenom preruseni 4 tj.Motor4
- };
- local_var=*TPU_TSR5;
- if(local_var & TSR5_TGFAm)
- {
- *TPU_TSR5&=~TSR5_TGFAm; //FIXME:zakazat nejak rozumne preruseni pro vsechny motory a ne jenom preruseni 4 tj.Motor4
- };
-
- /* Clear interrupt source */
-
- /*PXMS_ENG=enable input (IRC) update */
- pxmc_tpu_interupt_eni(); /*macro for service Interupt for all defined motors*/
-
- /*PXMS_ENG - enable requested value (position) generator*/
- pxmc_tpu_interupt_eng(); /*macro for service Interupt for all defined motors*/
-
- /*enable controller*/
- pxmc_tpu_interupt_enr(); /*macro for service Interupt for all defined motors*/
-
- /* enable debugging */
- pxmc_tpu_interupt_dbg(); /*macro for service Interupt for all defined motors*/
-}
-
-/********************************************************************/
-/* Stepper controller tests */
-
-#include <math.h>
-#include <cmd_proc.h>
-
-/********************************************************************/
-/* DC motor controller hardware dependant part */
-
-
-/*------------------------------------------------------------------*/
-/* DC motor with IRC feedback */
-
-/**
- * pxmc_pwm1f_out - two phase PWM output for DC motor
- * @mcs: Motion controller state information
- *
- * This function sets directly duty cycle register in
- * PWM_PWBFR1n n=(A,C,E,G) buffer register.
- */
-int pxmc_pwm1f_out(struct pxmc_state *mcs)
-{
- short ene=0;
- ene=mcs->pxms_ene;
- if(ene>=0)
- {
-/* #ifdef BOARD_MIROSOT
- *(volatile __u16 *)(mcs->pxms_out_info) = ((ene) >> 7)|0x1000;
- #else */
- *(volatile __u16 *)(mcs->pxms_out_info) = ((ene) >> 7)&0xefff;
-// #endif
- }
-
- else
- {
-/* #ifdef BOARD_MIROSOT
- *(volatile __u16 *)(mcs->pxms_out_info) = (((-ene) >> 7))&0xefff;
- #else */
- *(volatile __u16 *)(mcs->pxms_out_info) = (((-ene) >> 7))|0x1000;
-// #endif
-
- }
- return 0;
-}
-
-/**
- * pxmc_axis_rdmode - Reads actual axis mode.[extern API]
- * @mcs: Motion controller state information
- */
-int pxmc_axis_rdmode(pxmc_state_t *mcs)
-{
- if(mcs->pxms_do_con==pxmc_nofb_con)
- return 1;
- if(mcs->pxms_do_con==pxmc_pid_con)
- return 2;
- if(mcs->pxms_do_con==pxmc_cpme_con)
- return 3;
- return 0;
-}
-
-/**************************area change ****************/
-/*pxmc.c - line 1586 and px,c%h2638.c line 494*/
-
- //fce pxmc_axis_mode - is not pxmc.c and here
-/**************************area change ****************/
-
-
-int tpu_irc_ap2hw(struct pxmc_state *mcs) {
- short old_pos;
- if(mcs->pxms_inp_info) {
-#ifdef PXMC_WITH_PHASE_TABLE
- pxmc_clear_flag(mcs,PXMS_PHA_b);
- old_pos = *(short *)mcs->pxms_inp_info;
- mcs->pxms_ptofs+=mcs->pxms_ap-old_pos;
-#endif
- *(short *)mcs->pxms_inp_info=mcs->pxms_ap;
- }
- return 0;
-}
-
-
-/**
- * pxmc_get_sfi_hz - Reads sampling frequency of axis
- * @mcs: Motion controller state information
- */
-long pxmc_get_sfi_hz(pxmc_state_t *mcs)
-{
- return (long)pxmc_sfikhz*1000;
-}
-
-#ifdef WITH_SFI_SEL
-/**
- * pxmc_sfi_sel - Setting sampling frequency of PXMC subsystem (TPU_TGR4A)
- * @sfi_hz: Requested sampling frequency in Hz
- *
- * Function returns newly selected sampling frequency
- * after rounding or -1 if there is problem to set
- * requested frequency.
- */
-long pxmc_sfi_sel(long sfi_hz)
-{
- int sfikhz=(sfi_hz+500)/1000;
- if((sfikhz<2)||(sfikhz>12))
- return -1;
- *TPU_TSTR&=~(1<<4); /* Stop channel 4 */
- pxmc_sfikhz=sfikhz;
- *TPU_TCNT4=0; //FIXME: Use macro PCMC_SFI_SEL
- *TPU_TGR4A=((CPU_SYS_HZ+500)/1000)/pxmc_sfikhz;
- *TPU_TSTR|=(1<<4); /* Start channel 4 */
- return (long)pxmc_sfikhz*1000l;
-}
-#endif /* WITH_SFI_SEL */
-
-/**
- * pxmc_axis_mode - Sets axis mode.[extern API]
- * @mcs: Motion controller state information
- * @mode: 0 .. previous mode,
- 1 .. stepper motor mode,
- * 2 .. stepper motor with IRC feedback and PWM ,
- * 3 .. stepper motor with PWM control
- * 4 .. DC motor with IRC feedback and PWM
- *
- */
-__attribute__ ((deprecated)) int pxmc_axis_mode(pxmc_state_t *mcs, int mode)
-{
-// int ret;
- pxmc_set_const_out(mcs,0);
- pxmc_clear_flag(mcs,PXMS_ENI_b);
- pxmc_clear_flag(mcs,PXMS_PHA_b);
-
- if(!mode){
- mode=pxmc_axis_rdmode(mcs);
- if(!mode) mode=1;
- }
-
- switch(mode){
-// case 1 : /* Stepper motor mode */
-// #if HW_VER_CODE>=VER_CODE(0,2,0)
-// if((ret=pxmc_init_ptable(mcs,0))<0)
-// return ret;
-// /* necessary to initialize TPU for phase current control */
-// pxmc_stm_init_fbmode(mcs);
-// #endif /*HW_VER_CODE>=VER_CODE(0,2,0)*/
-// mcs->pxms_do_inp=pxmc_nofb_inp;
-// mcs->pxms_do_out=pxmc_nofb_out;
-// mcs->pxms_do_con=pxmc_nofb_con;
-// break;
-//
-// case 2 : /* Stepper motor with IRC feedback and PWM */
-// if((ret=pxmc_init_ptable(mcs,0))<0)
-// return ret;
-// pxmc_stm_init_fbmode(mcs);
-// mcs->pxms_do_con=pxmc_pid_con;
-//
-// break;
-//
-// case 3 : /* Stepper motor with PWM control */
-// if((ret=pxmc_init_ptable(mcs,0))<0)
-// return ret;
-// pxmc_stm_init_fbmode(mcs);
-// mcs->pxms_ptshift=0;
-// mcs->pxms_ptvang=0;
-// mcs->pxms_do_inp=pxmc_tpuirc_nofb_inp;
-// mcs->pxms_do_con=pxmc_cpme_con;
-// break;
-
- case 4 : /* DC motor with IRC feedback and PWM */
- pxmc_dcm_init_fbmode(mcs);
- break;
- default :
- return -1;
- }
- pxmc_set_flag(mcs,PXMS_ENI_b);
- return 0;
-}
-
-
-
-/**
- * pxmc_dcm_init_fbmode - Initializes feedback mode control
- * @mcs: Motion controller state information
- *
- * Function initializes TPU for IRC input and one phase
- * PWM output, generated by PWM unit, not TPU.
- *
- * -> IRC inputs are TCLKC and TCLKD,
- * Timer 2 is in phase counting mode 1.
- * Timer 4 is used to count sampling period (set in TPU_TGR4A)
- * and activate pxmc_sfi_isr interrupt handler.
- * -> PWM outputs are A and B, the frequency of the signal is set in PWM_PWCYR1.
- */
-
-void pxmc_set_pwm_tpu(void)
-{
- int num_motors = pxmc_main_list.pxml_cnt;
-
- *SYS_MSTPCRD &= ~MSTPCRD_PWMm; /*set up Gate as PWM gate (not I/O)*/
- *SYS_MSTPCRA &= ~MSTPCRA_TPUm; /*set up Gate as TPU (not I/O) */
-
- *PWM_PWCR1 =0xC4;//interrupt disabled, counter stopped, frequency phi/16
-
- if(num_motors>0) /*motor 1 - output PWM 1A and 1B*/
- {
- /*PWM seting*/
- *PWM_PWOCR1 |= (PWOCR1_OE1Am | PWOCR1_OE1Bm );
- *PWM_PWPR1 &= ~((PWPR1_OPS1Am)|(PWPR1_OPS1Bm));
- /*TPU seting*/
-#if 1
- *DIO_P1DDR &= ~(P1DDR_P12DDRm | P1DDR_P13DDRm); /*TCLKA and TCLKB are inputs*/
- PXMC_TPU_SFI_INIT(PXMC_SFI_TPU_CHN);
- PXMC_TPU_PHASE_CNT_INIT(1); /*macro for inicialized TPU - mod 1*/
-
-#endif
- };
- if(num_motors>1) /*motor 2 - output PWM 1C and 1D*/
- {
- *PWM_PWOCR1 |= (PWOCR1_OE1Cm | PWOCR1_OE1Dm );
- *PWM_PWPR1 &= ~((PWPR1_OPS1Cm)|(PWPR1_OPS1Dm));
- /*TPU seting*/
- *DIO_P1DDR &= ~(P1DDR_P15DDRm | P1DDR_P17DDRm); /*TCLKC and TCLKD are inputs*/
-// PXMC_TPU1(4); /*macro for inicialized TPU*/
- PXMC_TPU_PHASE_CNT_INIT(2); /*macro for inicialized TPU - mod 1*/
- };
- if(num_motors>2) /*motor 3 - output PWM 1E and 1F*/
- {
- /*PWM seting*/
- *PWM_PWOCR1 |= (PWOCR1_OE1Em | PWOCR1_OE1Fm );
- *PWM_PWPR1 &= ~((PWPR1_OPS1Em)|(PWPR1_OPS1Fm));
- };
- if(num_motors>3) /*motor 4 - output PWM 1G and 1H*/
- {
- *PWM_PWOCR1 = (PWOCR1_OE1Gm | PWOCR1_OE1Hm );
- *PWM_PWPR1 &= ~((PWPR1_OPS1Gm)|(PWPR1_OPS1Hm));
- };
- if(num_motors>0) /*motor 1 - output PWM 1A and 1B*/
- {
- *PWM_PWCYR1 = pwm_per; //constant set up period of PWM counter
- *PWM_PWCR1 |=PWCR1_CSTm; /*start PWM counter*/
- };
-}
-
-__attribute__ ((deprecated)) void pxmc_dcm_init_fbmode(pxmc_state_t *mcs)
-{
-// mcs->pxms_do_inp=pxmc_tpuirc_inp; /*set up pointer on function measure IRC*/
-// mcs->pxms_do_out=pxmc_pwm1f_out; //pointer on funct. control energi for outpu (PWM)
-// mcs->pxms_do_con=pxmc_pid_con; /*set up pointer on PID con. function (implicit (PXMC.c) or user*/
- pxmc_set_default_services(mcs,m_dc_irc_pwm); // !!! For compability only
-
- //int HelVar=pwm_per;
- //mcs->pxms_me = HelVar << 7; //max energy enter in a motor
- pxmc_set_max_energy(mcs); // !!! For compability only
-
- /* debug PWM generation */
-/* if(!pxmc_dbg_hist)
- pxmc_dbg_hist=0;*/ /* FIXME: Are here correct braces??? */
-/* pxmc_dbg_hist=pxmc_dbg_histalloc(4096);
- pxmc_dbgset(mcs, pxmc_dbg_pwm12, 1);
- pxmc_set_flag(mcs,PXMS_DBG_b);*/
- pxmc_set_default_debug(mcs,m_dc_irc_pwm); // !!! For compability only
-}
-
-
-#if 0
-/**
- * pxmc_stm_init - Initializes stepper or brush-less motor subsystem
- *
- * Configuration of IO registers, TPU for phase counting mode.
- * Returns 0 if the axis mode is set correctly.
- *
- * Timers 2 or 5 are in phase counting mode 1.
- * Choose which one defining PXMC_IRC_TPUN.
- * Timer 4 is used to count sampling period (set in TPU_TGR4A)
- * and activate pxmc_sfi_isr interrupt handler.
- *
- */
-int pxmc_stm_init(void)
-{
- #if HW_VER_CODE<VER_CODE(0,2,0)
- ID_STM_ENABLE(0);
- /*SHADDOW_REG_SET(DIO_P1DDR,0x0f);*/
- #else /* >=VER_CODE(0,2,0) */
- /* P10, P11 .. PWM */
- /* P15 .. synchronization for PWM */
- /* P12, P13 .. IRC */
- /* P16 .. THUMB inp, P17 .. THUMB output */
- SHADDOW_REG_CLR(DIO_P1DDR,0x5c);
- SHADDOW_REG_SET(DIO_P1DDR,0xa3);
- /* PF0, PF1 .. phase direction registers */
- SHADDOW_REG_SET(DIO_PFDDR,0x03);
- #endif /* >=VER_CODE(0,2,0) */
- pxmc_main_list.pxml_arr[0]->pxms_ap+=0x100; /*!!!*/
- if(pxmc_main_list.pxml_arr[0]->pxms_ene<0x1800)
- pxmc_main_list.pxml_arr[0]->pxms_ene+=0x0400;
-
- pxmc_sfikhz=1; /* 5 kHz fast sampling interval for stepper motors; 1kHz for normal CD brash motors*/
-
- pxmc_clear_flag(pxmc_main_list.pxml_arr[0],PXMS_ENG_b);
- pxmc_clear_flag(pxmc_main_list.pxml_arr[0],PXMS_ENR_b);
-
- #if HW_VER_CODE<VER_CODE(0,2,0)
- *SYS_MSTPCRC &= ~MSTPCRC_DA23m;
- *DA_DACR23|= (DACR23_DAEm*0) | DACR23_DAOE0m;
-
- pxmc_nofb_out(&mcs0);
-
- ID_STM_ENABLE(1);
- #endif /* <VER_CODE(0,2,0) */
-
- /* TPU initialization */
- *SYS_MSTPCRA&=~MSTPCRA_TPUm;
- *TPU_TSTR&=~(1<<4); /* Stop channel 4 */
- /* system clock/1 ,rising edge, clearing source TGRA */
- *TPU_TCR4=(TPCR_TPSC_F1 | TPCR_CKEG_RIS | TPCR_CCLR_TGRA);
- /* normal mode */
- *TPU_TMDR4=TPMDR_MD_NORMAL;
- /* TGRA initiates interrupt */
- excptvec_set(EXCPTVEC_TGI4A,pxmc_sfi_isr);
- *TPU_TIER4=TIER4_TGIEAm;
-
- *TPU_TGR4A=((CPU_SYS_HZ+500)/1000)/pxmc_sfikhz;
-
- *TPU_TSTR|=(1<<4); /* Start channel 4 */
-
- #if PXMC_IRC_TPUN==2
- /* IRC input initialization for channel 2 */
- *TPU_TPSTR&=~(1<<2);/* Stop channel 2 */
- /* system clock/1 ,rising edge */
- *TPU_TPCR2=0;
- /* phase counting mode 1 */
- *TPU_TPMDR2=TPMDR_MD_PHACN1;
- /* no irq */
- *TPU_TPIER2=0;
- *TPU_TPSTR|=(1<<2); /* Start channel 2 */
- #elif PXMC_IRC_TPUN==5
- /* IRC input initialization for channel 5 */
- *TPU_TSTR&=~(1<<5);/* Stop channel 5 */
- /* system clock/1 ,rising edge */
- *TPU_TCR5=0;
- /* phase counting mode 1 */
- *TPU_TMDR5=TPMDR_MD_PHACN1;
- /* no irq */
- *TPU_TIER5=0;
- *TPU_TSTR|=(1<<5); /* Start channel 5 */
- #endif /* PXMC_IRC_TPUN */
-
- /* set working mode for axis */
- return pxmc_axis_mode(pxmc_main_list.pxml_arr[0], 1);
-}
-#endif
-
+++ /dev/null
-# Generic directory or leaf node makefile for OCERA make framework
-
-ifndef MAKERULES_DIR
-MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
-endif
-
-ifeq ($(MAKERULES_DIR),)
-all : default
-.DEFAULT::
- @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
-else
-include $(MAKERULES_DIR)/Makefile.rules
-endif
-
+++ /dev/null
-
-default_CONFIG = CONFIG_PXMC_VARIANT=bldctest
-
-lib_LIBRARIES = pxmcbsp
-
-pxmcbsp_SOURCES = bldctest.c
-renamed_include_HEADERS += bldctest.h->pxmcbsp.h
-renamed_include_HEADERS += pxmc_config_h8mirosot.h->pxmc_config.h
-#include_HEADERS=bldctest.h
-
-
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc_eurobot.c - generic multi axis motion controller
- for eurobot competition
-
- Copyright (c)2001 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (c)2006-2007 by Konrad Skup ahblitz@yahoo.de
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
- *******************************************************************/
-
- /* [extern API] characteristic means that the function is declared
-in the header file pxmc.h so it is part of the external API */
-
-#include <types.h>
-#include <cpu_def.h>
-#include <h8s2638h.h>
-#include <system_def.h>
-#include <string.h>
-//#include <math.h>
-#include <pxmc.h>
-#include "../../pxmc_inp_common.h"
-#include "../../pxmc_internal.h"
-
-#include "pxmc_config.h"
-#include "pt2048.h"
-
-#include "bldctest.h"
-
-//=================================
-/* Prototypes */
-//=================================
-static void init_pwm(void);
-static void init_irc(void);
-static void init_hal(void);
-static void init_sampling(void);
-
-void index_mark_isr(void) __attribute__ ((interrupt_handler)); // zero crossing detectio interrput handler
-//=================================
-/* Definitions of some basic but
- usefull variables. */
-//=================================
-int pxmc_sfikhz=2; // Sampling freq. interrupt - how often we want to collect data
-int pxmc_sfi2msec; // No idea what for it is... - used in: pxmc_tpu_interupt_dbg
-volatile unsigned pxmc_msec_counter; // miliseconds counter
-extern int index_mark;
-#define PWM_HZ 20000
-#define PWM_MAX (((CPU_SYS_HZ / PWM_HZ) <= PWBFR1A_DTxm) ? (CPU_SYS_HZ / PWM_HZ) : PWBFR1A_DTxm)
-/*struktury charakterizujici motor 0*/
-pxmc_state_t mcs_left={
- pxms_flg:PXMS_ENI_m,
- pxms_do_inp:motor_do_input,
- pxms_do_out:motor_do_output,
- pxms_do_con:pxmc_pid_con,
- pxms_do_ap2hw: tpu_irc_ap2hw,
- pxms_md:20000l<<8, pxms_ms:200, pxms_ma:5,
- pxms_inp_info:(long)TPU_TCNT1,//TPU_TCNT1 /*chanel TPU A,B*/
- pxms_out_info:0,//(long)PWM_PWBFR1A, /*chanel PWM A,B*/
- pxms_ene:0,
- pxms_p: 50, pxms_i: 10, pxms_d: 50, pxms_s1: 0, pxms_s2: 0,
- pxms_me:PWM_MAX,//PXMC_PWM_PER<<8, //6144
- pxms_ptirc:bldc_ptirc, // IRC count per phase table
- pxms_ptper:bldc_ptper, // Number of periods per table
-
- //* @pxms_ptofs: Offset between table and IRC counter
- pxms_ptshift:0, // Shift of generated phase curves
- pxms_ptvang:bldc_ptirc/4, // Angle (in irc) between rotor and stator mag. fld.
-
- pxms_ptptr1:bldc_phase1,
- pxms_ptptr2:bldc_phase2,
- pxms_ptptr3:bldc_phase3,
- pxms_ptamp:0x7fff,
- pxms_hal: 0x40,
-
- pxms_cfg:PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m |
- PXMS_CFG_HPS_m | PXMS_CFG_HDIR_m| PXMS_CFG_SMTH_m| 0x1
-};
-
-pxmc_state_t *pxmc_main_arr[] = {&mcs_left};
-
-pxmc_state_list_t pxmc_main_list = {
- pxml_arr:pxmc_main_arr,
- pxml_cnt:sizeof(pxmc_main_arr) / sizeof(pxmc_main_arr[0])
-};
-
-// Table which keeps info for pwm order in hal sensor case
-const unsigned char eurobot_bdc_hal_pos_table[8]=
-{
- // h3.h2.h1 // order in case of one byte
- //0xff,2,4,3,0,1,5,0xff
- [0]=0xff,
- [5]=0, //1 0 1 = 5
- [1]=1, //0 0 1 = 1
- [3]=2, //0 1 1 = 3
- [2]=3, //0 1 0 = 2
- [6]=4, //1 1 0 = 6
- [4]=5, //1 0 0 = 4
- [7]=0xff,
-};
-
-//=========================================
-/* Here are hardware independent functions
- like for motor control and so one... */
-//=========================================
-/**
- * pxmc_initialize - Initializes all subsystems.
- * @return: Always 0.
- *
- * Description: This function initializes all subsystems necessary for
- * proper work of pxmc. There are switched on: pwm, hall sensors read,
- * irc updates, sampling period is set up and the irq for index mark
- * detection.
-*/
-int pxmc_initialize(void)
-{
- //pxmc_set_default_functions_for_all_motors(pxmc_m_brushless);
- //pxmc_set_irq();
-
- init_pwm(); //nastaveni HW (TPU, PWM)
- init_hal();
- init_irc();
- init_sampling();
-
- init_irq();
-
- return 0;
-};
-
-/**
- * pxmc_set_default_functions_for_all_motors - sets default control functions for all motors
- * @motor: Kind of motor which we are going to use -> see header for
- * full list of supported motors
- * @return: True if no errors were detected, false in a case if there
- * were some problems
- *
- * Description: This function sets up defaults pointers to control functions
- * for all motors which are presented in the system. The default configuration
- * depends on the motor kind.
-*/
-bool pxmc_set_default_functions_for_all_motors(pxmc_motor_kind_e motor)
-{
- int var;
- pxmc_state_t *mcs;
- pxmc_for_all_axes(var, mcs)
- {
- pxmc_set_default_functions(mcs, motor);
- }
- return true;
-}
-
-/**
- * pxmc_set_default_functions - sets default control functions for given motor
- * @mcs: Motion controller state information
- * @motor: Kind of motor which we are going to use -> see header for
- * full list of supported motors
- * @return: True if no errors were detected, false in a case if there
- * were some problems
- *
- * Description: This function sets up defaults pointers to control functions
- * for a given motor. The default configuration depends on the motor kind.
-*/
-inline bool pxmc_set_default_functions(pxmc_state_t *mcs,pxmc_motor_kind_e motor)
-{
- switch(motor)
- {
- case pxmc_m_stepper:
- /*mcs->pxms_do_inp=pxmc_nofb_inp;
- mcs->pxms_do_con=pxmc_nofb_con;
- mcs->pxms_do_out=pxmc_nofb_out;*/
- break;
- case pxmc_m_brushless:
- mcs->pxms_do_inp=motor_do_input; // set pointer to funct. for measuring IRC
- mcs->pxms_do_out=motor_do_output; // set pointer to funct. control energi for outpu (PWM)
- mcs->pxms_do_con=pxmc_pid_con; // set pointer to PID con. function (implicit (PXMC.c) or user
- //pxmc_set_default_debug(mcs,m_dc_irc_pwm); // !!! For compability only
-
- break;
- case pxmc_m_test:
- mcs->pxms_do_inp=motor_do_input_test; // set pointer to funct. for measuring IRC
- mcs->pxms_do_out=motor_do_output_test; // set pointer to funct. control energi for outpu (PWM)
- mcs->pxms_do_con=pxmc_pid_con; // set pointer to PID con. function (implicit (PXMC.c) or user
- //pxmc_set_default_debug(mcs,m_dc_irc_pwm); // !!! For compability only
-
- pxmc_set_const_out(mcs,0); // stop the motor
-
- pxmc_set_flag(mcs,PXMS_ENI_b); // switch on pxms_do_inp
-
- break;
- default:
- return false;
- }
-
- return true;
-}
-
-/**
- * pxmc_sfi_isr - Interrupt Sampling Routine
- *
- * This interrupt service routine is used to call routines for different services, like pxms_do_inp,
- * pxms_do_out and so one. It also updates the pxmc_msec_counter. It works according to interrupts
- * and is called with freq. equal to pxmc_sfikhz. Function is set in interrupt vector table by: init_sampling() function.
-*/
-/* int intno, void *dev_id, struct pt_regs *regs */
-void pxmc_sfi_isr(void)
-{
- /* PXMS_ENI_m = enable input (IRC) update */
- interupt_input();
-
- /* PXMS_ENG_m - enable requested value (position) generator*/
- interupt_generator();
-
- /*PXMS_ENR_m - enable controller*/
- interupt_controller_and_output();
-
- /* PXMS_DBG_m - enable debugging */
- interupt_dbg();
-
- static int submsec = 0;
- submsec++;
- if (submsec==pxmc_sfikhz) {
- submsec = 0;
- pxmc_msec_counter++;
- }
-
- // Allow next interrupt...
- *TPU_TSR4&=~TSR4_TGFAm; //Acknowledge an interrupt
-}
-
-/**
- * interupt_input - calls pxms_do_inp function
- *
- * The function is used to call pxms_do_inp functions for all motors present on
- * the board. It's called by pxmc_sfi_isr() during every sampling period.
-*/
-inline void interupt_input()
-{
- int var;
- pxmc_state_t *mcs;
- pxmc_for_all_axes(var, mcs)
- {
- // PXMS_ENI_m - check if input (IRC) update is enabled
- if(mcs->pxms_flg&PXMS_ENI_m)
- {
- pxmc_call(mcs,mcs->pxms_do_inp);
-// error_led(~1);
- }
- }
-}
-
-/**
- * interupt_controller_and_output - calls pxms_do_con and pxms_do_out functions
- *
- * The function is used to call pxms_do_con and pxms_do_out functions for all
- * motors present on the board. It's called by pxmc_sfi_isr() during every
- * sampling period.
-*/
-inline void interupt_controller_and_output()
-{
- int var;
- pxmc_state_t *mcs;
- pxmc_for_all_axes(var, mcs)
- {
- // PXMS_ENR_m - check if controller is enabled
- if(mcs->pxms_flg&PXMS_ENR_m || mcs->pxms_flg&PXMS_ENO_m)
- {
-
- /* If output only is enabled, we skip the controller */
- if(mcs->pxms_flg&PXMS_ENR_m)
- {
- pxmc_call(mcs,mcs->pxms_do_con);
- // PXMS_ERR_m - if axis in error state
- if(mcs->pxms_flg&PXMS_ERR_m) mcs->pxms_ene=0;
- }
-
- // FIXME: For bushless motors, it is necessary to call do_out
- // even if the controller is not enabled.
- pxmc_call(mcs,mcs->pxms_do_out);
- // error_led(1);
- }
- }
-}
-
-/**
- * interupt_generator - calls pxms_do_gen function
- *
- * The function is used to call pxms_do_gen functions for
- * all motors present on the board. It's called by
- * pxmc_sfi_isr() during every sampling period.
-*/
-inline void interupt_generator()
-{
- int var;
- pxmc_state_t *mcs;
- pxmc_for_all_axes(var, mcs)
- {
- // PXMS_ENG_m - check if requested value (position) generator is enabled
- if(mcs->pxms_flg&PXMS_ENG_m)
- {
- pxmc_call(mcs,mcs->pxms_do_gen);
- // error_led(~2);
- }
- }
-}
-
-/**
- * interupt_dbg - calls pxms_do_deb function
- *
- * The function used to call pxms_do_deb functions for all
- * motors present on the board. It's called by
- * pxmc_sfi_isr() during every sampling period.
-*/
-inline void interupt_dbg()
-{
- int var;
- pxmc_state_t *mcs;
- pxmc_for_all_axes(var, mcs)
- {
- if(mcs->pxms_flg&PXMS_DBG_m)
- {
- pxmc_call(mcs,mcs->pxms_do_deb);
- // error_led(2);
- }
- if(--pxmc_sfi2msec<=0)
- {
- msec_time++;
- pxmc_sfi2msec=pxmc_sfikhz;
- /* Hook for external finite state machine */
- }
- else
- {
- /* Run ADC measurements */
- }
- }
-}
-
-/**
- * index_mark_isr_1 - finds index mark for 1st motor
- *
- * This interrupt routine is used to set up %PXMS_PHA_b flag
- * for the first motor on index mark detection. Function uses
- * TPU for index mark detection. In general the function should
- * work in such manner, that it switch off itself when the second
- * index mark occurs.
-*/
-void index_mark_isr_1(void)
-{
- // static int led = 0;
- static int count = 2;
- pxmc_state_t *mcs = &mcs_left;
-
- // I think we can detect index mark after detection of hal edge.
-
- if (mcs->pxms_flg&PXMS_PTI_m)
- {
- count--;
- if(count==0) { //FIXME constant is different for different motors
- mcs->pxms_ptofs = (*TPU_TGR1A - 5l/*degrees*/*mcs->pxms_ptirc/360);
- //pxmc_set_flag(mcs, PXMS_PTI_b); // see the 1st comment
- pxmc_set_flag(mcs, PXMS_PHA_b);
- count = 2; //If it is necessary to find index mark later
- //*TPU_TIER1=0; //Disable future index mark interrupts
- }
- }
-// static int i=0;
- // i=(i+1)%200;
-// if (i==0)
- {
- // *DIO_PJDR ^= 2;
- //*DIO_PJDR ^= 4;
- *DIO_PJDR = 4;
- }
-
- *TPU_TSR1 &=(~TSR1_TGFAm);
-}
-
-/**
- * index_mark_isr_2 - finds index mark for 2nd motor
- *
- * This interrupt routine works in exactly the same way as
- * index_mark_isr_1(). The only one difference is that it
- * works for second motor.
-*/
-void index_mark_isr_2(void)
-{
- *DIO_PJDR ^= 2;
- //*DIO_PJDR ^= 4;
-
- *TPU_TSR2 &=(~TSR2_TGFAm);
-}
-
-/**
- * index_mark_isr - finds index mark for 1st motor
- *
- * This interrupt routine is used to detect index mark for the first motor.
- * Function uses IRQ as a source information about index mark crossing.
- * In general the function was created for HalDetector program and do one
- * thing. It increases value of global variable: index_marking.
-*/
-/* static */ void index_mark_isr(void)
-{
-// static int count = 2;
- pxmc_state_t *mcs = &mcs_left;
-
- if (mcs->pxms_flg&PXMS_PTI_m)
- {
- //count--;
- //if(count==0) { //FIXME constant is different for different motors
- //mcs->pxms_ptofs = (*TPU_TGR1A - 5l/*degrees*/*mcs->pxms_ptirc/360);
-
- //pxmc_set_flag(mcs, PXMS_PHA_b);
- //index_marking++;
- //count = 2; //If it is necessary to find index mark later
- //}
- }
- index_marking++;
- *DIO_PJDR ^= 2;
- *DIO_PJDR ^= 4; // for positive speed, index marking is at 1024, for negative it is 1365
- // index_mark=1; // using test f. 3 I detected that the index marking is in the range: 1340-1343
-
- *INT_ISR&=(~ISR_IRQ2Fm);
-}
-//=========================================
-/* Here are hardware dependent functions
- like for motor control and so one... */
-//=========================================
-/* General functions... */
-//=========================================
-/**
- * init_pwm - sets up PWM
- *
- * This function sets up PWM for proper work with motor.
-*/
-static void init_pwm(void)
-{
- // /* setup pwm */
- *SYS_MSTPCRD &= ~MSTPCRD_PWMm;
-
- *PWM_PWCR1 =0xc4; //interrupt disabled, counter stopped, frequency phi/16
-
- *DIO_PJDDR &= ~0xf8; // hal(d,e,f and g,h) is in as i/o
- *DIO_PJDDR |= (PJDDR_PJ0DDRm | PJDDR_PJ1DDRm | PJDDR_PJ2DDRm); // led-s are out as i/o
-
- *PWM_PWOCR1 = PWOCR1_OE1Am |PWOCR1_OE1Cm|PWOCR1_OE1Em; // enable output as pwm
-
- *PWM_PWCYR1 = PXMC_PWM_PER; //constant set up period of PWM counter
- *PWM_PWCR1 |= 0xc8; // 8-start clock, ph/1 //PWCR2_CSTm
-}
-
-/**
- * init_irc - sets up IRC
- *
- * This function sets up TPU for index mark detection for 1st and 2nd motor.
-*/
-static void init_irc(void)
-{
- /* setup tpu */
- *SYS_MSTPCRA &= ~MSTPCRA_TPUm; /* Switch on TPU module */
- TPU_PHASE_CNT_INIT(1);
- TPU_PHASE_CNT_INIT(2);
-}
-
-/**
- * init_hal - set up hall
- *
- * This function sets up registers and I/O ports in such a way, that the read
- * of hall sensor is possible.
-*/
-static void init_hal(void)
-{
- *PWM_PWCR2 =0xC4; //interrupt disabled, counter stopped, frequency phi/16
-
- *PWM_PWOCR2 &= ~0x07; // switch off: A,B,C for PWM - leds - now as i/o
- *PWM_PWOCR2 &= ~0xf8; // switch off: D,E,F(and g,h) for PWM - hal - now as i/o
-
- *PWM_PWCR2 |= 0xc8; //PWCR2_CSTm
-}
-
-/**
- * init_sampling - sets up TPU
- *
- * This function sets up TPU main unit to generate interrupts with sampling
- * frequency: %pxmc_sfikhz. It also puts pxmc_sfi_isr() to the interrupt
- * vector table.
-*/
-static void init_sampling(void)
-{
- *SYS_MSTPCRA &= ~MSTPCRA_TPUm; /* Switch on TPU module */
-
- *TPU_TSTR &= ~TSTR_CST4m; /*counter n is stoped*/
- /*CNTN cleared by by TGRA, rising edge; source TGRA compare match/input capture */
- *TPU_TCR4 = (TPCR_TPSC_F1 | TPCR_CKEG_RIS | TPCR_CCLR_TGRA);
- /*TGRA is output compare registr - . 0 output at compare match*/
- *TPU_TIOR4 |= TIOR4_IOA1m;
- /*MDn = 0x000 normal operation */
- *TPU_TMDR4 = TPMDR_MD_NORMAL;
- /*seting interupt vector - from TGRA at registr TSRn*/
- excptvec_set(EXCPTVEC_TGI4A, pxmc_sfi_isr);
- /*TimerInteruptEnableReg - enable IRQ generation from TGRA*/
- *TPU_TIER4 |= TIER4_TGIEAm;
- /*setup TGRA - sampling period*/
- *TPU_TGR4A = (CPU_SYS_HZ / 1000) / pxmc_sfikhz;
- /*TimerStatusRegistr - clear match flag */
- *TPU_TSR4 &= ~TSR4_TGFAm;
- /*Start TCNTn*/
- *TPU_TSTR |= TSTR_CST4m;
-}
-
-/**
- * init_irq - sets up IRQ
- *
- * This function set up IRQ interrupt, by putting index_mark_isr() to the
- * interrupt vector table.
-*/
-void init_irq(void)
-{
- // set up highest priotity for IRQ2
- *INT_IPRB|=(IPRB_IPR4m|IPRB_IPR5m|IPRB_IPR6m); // irq2
- // set up detection of rasing egde
- *INT_ISCRL&=(~ISCRL_IRQ2SCAm);
- *INT_ISCRL|=(ISCRL_IRQ2SCBm);
- // connect some function to the interrupts
- excptvec_set(EXCPTVEC_IRQ2,index_mark_isr); // irq2
- // switch on IRQ2
- *INT_IER|=(IER_IRQ2Em);
-}
-
-//=========================================
-/* Control functions for brushless motor...*/
-//=========================================
-/**
- * motor_do_input - IRC encoder connected to H8S 2638 TPU
- * @mcs: Motion controller state information
- * @return: Always 0.
- *
- * Description:
- * The function checks weather %PXMS_PTI_b flag is enabled and if it is,
- * than it calls pxmc_irc_16bit_commindx() function to update pxms_ap
- * according to IRC.
-*/
-inline int motor_do_input(struct pxmc_state *mcs)
-{
- short irc;
- irc=*(volatile short*)mcs->pxms_inp_info;
- pxmc_irc_16bit_update(mcs,irc);
-
- /* Running of the motor commutator */
- //irc=mcs->pxms_rp>>PXMC_SUBDIV(mcs); // when enabled means without feedback
- if(mcs->pxms_flg&PXMS_PTI_m) pxmc_irc_16bit_commindx(mcs,irc); //pxmc_irc_16bit_commindx(mcs,irc);
-
- return 0;
-}
-
-unsigned test_index = 0, test_counter=0;
-
-/**
- * motor_do_output - sends/sets up proper values to PWM outputs.
- * @mcs: Motion controller state information
- * @return: Always 0.
- *
- * Description:
- * This function has two major tasks. Firstly if flag: %PXMS_PTI_b is not
- * enabled, the motor_do_output tries to estimate actual position of the motor
- * according to readings from hall sensor. Secondly, it read the values from
- * pahse table(s) according to the value of pxms_indx. Then it
- * multiplies is by pxms_ene, and then it sends this result to PWM outputs.
-*/
-int motor_do_output(struct pxmc_state *mcs)
-{
- unsigned short pwm1, pwm2, pwm3;
- short ene = 0;
- int indx = 0;
-
- if (!(mcs->pxms_flg&PXMS_PTI_m))
- {
- short ptindx;
- short ptirc = mcs->pxms_ptirc;
- short divisor = mcs->pxms_ptper * 6;
- unsigned char hal_pos;
-
- hal_pos = eurobot_bdc_hal_pos_table[hal_read(mcs)];
-
- if (hal_pos == 0xff) {
- pxmc_set_errno(mcs, PXMS_E_HAL);
- mcs->pxms_ene = 0; //FIXME is it correct to stop the motor this way?
- } else {
- ptindx = (hal_pos * ptirc + divisor / 2) / divisor;
-
- if (!(mcs->pxms_flg&PXMS_PTI_m)) {
- if( ((hal_pos!=mcs->pxms_hal)&&(mcs->pxms_hal!=0x40))){
- if ((ptindx > mcs->pxms_ptindx+ptirc/2) /* underrun */||
- (mcs->pxms_ptindx>ptindx+ptirc/2) /* overrun */){
- ptindx=(mcs->pxms_ptindx+ptindx-ptirc)/2;
- if (ptindx<0)
- ptindx+=ptirc;
- }else{
- ptindx=(mcs->pxms_ptindx+ptindx)/2;
- }
- mcs->pxms_ptindx=ptindx;
-
- mcs->pxms_ptofs=(mcs->pxms_ap>>PXMC_SUBDIV(mcs))+mcs->pxms_ptshift-ptindx;
-
- pxmc_set_flag(mcs,PXMS_PTI_b);
- }else{
- mcs->pxms_ptindx=ptindx;
- }
- mcs->pxms_hal = hal_pos;
- }
- }
- }
-
- ene = mcs->pxms_ene;
-
- if (ene)
- {
- indx = mcs->pxms_ptindx;
- if (ene < 0) {
- // Generating direction of stator mag. field for backward torque
- ene = -ene;
- indx -= mcs->pxms_ptvang;
- if (indx < 0) indx += mcs->pxms_ptirc;
- } else {
- // Generating direction of stator mag. field for forward torque
- indx += mcs->pxms_ptvang;
- if (indx >= mcs->pxms_ptirc) indx -= mcs->pxms_ptirc;
- }
- }
-
- //indx %= mcs->pxms_ptirc;
- pwm1 = mcs->pxms_ptptr1[indx];
- pwm2 = mcs->pxms_ptptr2[indx];
- pwm3 = mcs->pxms_ptptr3[indx];
-
- /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
- /* Initialized CTM4 PWM period is 0x200 => divide by value about 2097024 */
- pwm1 = (((unsigned long)pwm1*(unsigned int)ene) >> (15+5));
- pwm2 = (((unsigned long)pwm2*(unsigned int)ene) >> (15+5));
- pwm3 = (((unsigned long)pwm3*(unsigned int)ene) >> (15+5));
-
- *PWM_PWBFR1A = pwm1;
- *PWM_PWBFR1C = pwm2;
- *PWM_PWBFR1E = pwm3;
-
- return 0;
-
-}
-
-/**
- * tpu_irc_ap2hw - makes updates in pxms_ptofs and irc
- * @mcs: Motion controller state information
- * @return: Always 0.
- *
- * Description:
- * The function updates pxms_ptofs and reads IRC. It need to be called
- * if pxms_ap is changed by the application.
-*/
-inline int tpu_irc_ap2hw(struct pxmc_state *mcs)
-{//FIXME - move to processor dependent file
- short old_pos;
- if (mcs->pxms_inp_info)
- {
-#ifdef PXMC_WITH_PHASE_TABLE
- pxmc_clear_flag(mcs, PXMS_PHA_b);
- old_pos = *(short *)mcs->pxms_inp_info;
- mcs->pxms_ptofs += (mcs->pxms_ap>>PXMC_SUBDIV(mcs)) - old_pos;
-#endif
- *(short *)mcs->pxms_inp_info = (mcs->pxms_ap>>PXMC_SUBDIV(mcs));
- }
- return 0;
-}
-/****************************************
-* Debuging part... *
-****************************************/
-//=========================================
-/* Control functions for testing motor...
- All functions in this section are
- deprecated. */
-//=========================================
-/**
- * motor_do_input_test - IRC encoder connected to H8S 2638 TPU.
- * @mcs: Motion controller state information
- * @return: Always 0.
- *
- * Description:
- * Function works in exactly the same manner as motor_do_input(). The only puropse
- * for it, is that it should be used whenever we make some tests with new
- * functionality of the input function.
-*/
-inline int motor_do_input_test(struct pxmc_state *mcs)
-{
- short irc;
- irc=*(volatile short*)mcs->pxms_inp_info;
- //irc=-mcs->pxms_ap>>PXMC_SUBDIV(mcs);
- pxmc_irc_16bit_update(mcs,irc);
-
- /* Running of the motor commutator */
- if(mcs->pxms_flg&PXMS_PTI_m) pxmc_irc_16bit_commindx(mcs,irc);
-
- return 0;
-}
-
-/**
- * motor_do_output_test - sends/sets up proper values to PWM outputs.
- * @mcs: Motion controller state information
- * @return: Always 0.
- *
- * Description:
- * Function works in exactly the same manner as motor_do_output(). The only puropse
- * for it, is that it should be used whenever we make some tests with new
- * functionality of the output function.
-*/
-inline int motor_do_output_test(struct pxmc_state *mcs)
-{
- /* short ene=0;
- ene=mcs->pxms_ene;
- if(ene>=0)
- {
- *(volatile __u16 *)(mcs->pxms_out_info) = ((ene) >> 7)&0xefff;
- }
- else
- {
- *(volatile __u16 *)(mcs->pxms_out_info) = (((-ene) >> 7))|0x1000;
- }
-
- return 0; */
-
- static unsigned idx=0; // out index in phase table
- unsigned long pwm1,pwm2,pwm3;
- short ene=mcs->pxms_ene;
-
- idx++;
- idx %= mcs->pxms_ptirc;
-
- pwm1 = mcs->pxms_ptptr1[idx];
- pwm2 = mcs->pxms_ptptr2[idx];
- pwm3 = mcs->pxms_ptptr3[idx];
-
- /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
- /* Initialized CTM4 PWM period is 0x200 => divide by value about 2097024 */
- pwm1 = (((unsigned long)pwm1*(unsigned int)ene) >> (15+5));
- pwm2 = (((unsigned long)pwm2*(unsigned int)ene) >> (15+5));
- pwm3 = (((unsigned long)pwm3*(unsigned int)ene) >> (15+5));
-
- *PWM_PWBFR1A = pwm1;
- *PWM_PWBFR1C = pwm2;
- *PWM_PWBFR1E = pwm3;
-
- return 0;
-}
-
-//=========================================
-/* Additional functions. */
-//=========================================
-
-/* Local Variables: */
-/* c-basic-offset: 2 */
-/* End */
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc_eurobot.h - generic multi axis motion controller
- for eurobot competition
-
- Copyright (c)2001 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (c)2006-2007 by Konrad Skup ahblitz@yahoo.de
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
- *******************************************************************/
-
-#ifndef _PXMC_EUROBOT_H
-#define _PXMC_EUROBOT_H
-
-#include <stdbool.h>
-#include <pxmc.h>
-#include <mcu_regs.h>
-//=================================
-/* Definitions of some basic but
- usefull constants in form of
- directives. */
-//=================================
-#ifndef PXMC_CYCLEKHZ
-#define PXMC_CYCLEKHZ 20
-#endif
-//-------------
-#ifndef PXMC_PWM_PER
-#define PXMC_PWM_PER (((CPU_SYS_HZ/16)/1000)/PXMC_CYCLEKHZ)
-#endif
-//-------------
-#ifndef PXMC_SFI_TPU_CHN
-#define PXMC_SFI_TPU_CHN 4 /* TPU channel used for timing */
-#endif
-//-------------
-#ifndef PXMC_NR_OF_PHASES_I_
-#define PXMC_NR_OF_PHASES_I_ 2048
-#endif
-//-------------
-#ifndef PXMC_WITH_PHASE_TABLE
-#define PXMC_WITH_PHASE_TABLE 1
-#endif
-#ifndef PXMC_USE_VANG
-#define PXMC_USE_VANG 1
-#endif
-//=================================
-/* Definitions of some basic but
- usefull constants. */
-//=================================
-typedef unsigned char byte;
-
-/**
- * enum _MOTOR_KIND_ - names of avaible motors
- * @pxmc_m_stepper: stepper motor
- * @pxmc_m_brushless: brushless motor
- * @pxmc_m_test: for tests
- *
- * This defines kinds of motors which are or could be connected to the board.
-*/
-enum _MOTOR_KIND_ {pxmc_m_stepper=1,pxmc_m_brushless=2,pxmc_m_test=13};
-
-/**
- * typedef pxmc_motor_kind_e - defines pxmc_motor_kind_e type as _MOTOR_KIND_
- *
- * This defines new type: pxmc_motor_kind_e type as _MOTOR_KIND_.
-*/
-typedef enum _MOTOR_KIND_ pxmc_motor_kind_e;
-
-extern const unsigned char eurobot_bdc_hal_pos_table[8];
-//extern const unsigned char pxmc_hal_pos_table[];
-extern const short bldc_phase1[PXMC_NR_OF_PHASES_I_];
-extern const short bldc_phase2[PXMC_NR_OF_PHASES_I_];
-extern const short bldc_phase3[PXMC_NR_OF_PHASES_I_];
-
-extern pxmc_state_t mcs_right, mcs_left;
-extern volatile unsigned pxmc_msec_counter; // miliseconds counter
-extern short index_marking; // for index marking detection
-//=================================
-/* Definitions of all functions
- used in this library. */
-//=================================
-//-----------------------------
-//static void init_pwm(void);
-//static void init_irc(void);
-//static void init_hal(void);
-//static void init_sampling(void);
-int pxmc_initialize(void);
-//-----------------------------
-bool pxmc_set_default_functions_for_all_motors(pxmc_motor_kind_e motor);
-inline bool pxmc_set_default_functions(pxmc_state_t *mcs,pxmc_motor_kind_e motor);
-//inline short pxmc_set_max_energy(pxmc_state_t *mcs);
-void pxmc_sfi_isr(void) __attribute__ ((interrupt_handler)); // sampling frequency interrupt handler
-inline void interupt_input();
-inline void interupt_controller_and_output();
-inline void interupt_generator();
-inline void interupt_dbg();
-
-void index_mark_isr_1(void) __attribute__((interrupt_handler));
-void index_mark_isr_2(void)__attribute__((interrupt_handler));
-void index_mark_isr(void) __attribute__((interrupt_handler));
-//-----------------------------
-void init_irq(void);
-//-----------------------------
-inline int motor_do_input(struct pxmc_state *mcs);
-inline int motor_do_output(struct pxmc_state *mcs);
-inline int tpu_irc_ap2hw(struct pxmc_state *mcs);
-//inline int pxmc_do_detection_of_zero_crossing(struct pxmc_state *mcs); // will be removed
-//-----------------------------
-inline int motor_do_input_test(struct pxmc_state *mcs);// __attribute__ ((deprecated));
-inline int motor_do_output_test(struct pxmc_state *mcs); //__attribute__ ((deprecated));
-//inline int pxmc_do_con_test(struct pxmc_state *mcs); //__attribute__ ((deprecated));
-//-----------------------------
-//=================================
-/* Definitions of some basic but
- usefull macros. */
-//=================================
-#ifndef TPU_PHASE_CNT_INIT
-#define TEMP_TPU_PHASE_CNT_INIT(n) \
- do{ \
- /*TimerControlRegistr - TCNT clearing disabled; count rasing adge; internal clock fi/1 */\
- *TPU_TCR##n=0; \
- /*phase counting mode 1 - how will be TCNT incrementd */\
- *TPU_TMDR##n=TPMDR_MD_PHACN1; \
- /* Input capture at rising edge on TIOCAx pin (index mark) */\
- *TPU_TIOR##n = TIOR##n##_IOA3m; \
- /*AD conversion disabled; interrupt TGIA enabled.; */\
- *TPU_TIER##n=TIER##n##_TGIEAm; \
- excptvec_set(EXCPTVEC_TGI##n##A,index_mark_isr_##n); \
- /*start TCNT*/\
- *TPU_TSTR|=(1<<n); \
-}while (0)
-
-// TRICK: See cpp info pages, node Argument Prescan,
-// parahraph "Macros that call other macros that stringify or concatenate."
-#define TPU_PHASE_CNT_INIT(n) TEMP_TPU_PHASE_CNT_INIT(n)
-#endif
-
-/* ***** Debugging ******** */
-unsigned test_index, test_counter;
-
-/**
- * hal_read - reads hall sensor
- * @mcs: Motion controller state information
- * @return: Value of hall sensor
- *
- * Description: This function reads and return hall sensor value.
-*/
-static inline unsigned char hal_read(struct pxmc_state *mcs)
-{
- return (*DIO_PORTJ>>3)&0x07;
-}
-
-//=================================
-#endif
-
+++ /dev/null
-#include "pt2048.h"
+++ /dev/null
-/* Generated phase table, do not edit */
-
-/* ptirc=2048 ptper=1 ptamp=32767 phasenum=3 combined=0*/
-/* shape="sin3phup" */
-/* ptable_name="bldc" */
-
-#define bldc_ptirc 2048
-#define bldc_ptper 1
-
-const short
-bldc_phase1[]={
-28377,28426,28476,28526,28575,28624,28672,28721,28770,28817,28866,28913,28960,29006,29053,29100,29146,29192,29237,29282,29327,29372,29416,29460,29504,29548,29591,29635,29677,29720,29761,29804,29845,29887,29928,29969,30009,30049,30089,30129,30168,30207,30246,30284,30322,30361,30398,30436,30473,30510,30546,30582,30618,30654,30690,30725,30759,30794,30828,30861,30896,30929,30962,30994,31027,31060,31091,31122,31154,31185,31216,31246,31276,31306,31335,31365,31394,31422,31450,31479,31507,31534,31561,31588,31615,31641,31667,31693,31718,31743,31768,31792,31816,31840,31863,31887,31910,31933,31955,31978,31999,32021,32042,32063,32083,32103,32123,32143,32163,32182,32200,32218,32237,32254,32272,32289,32306,32323,32339,32356,32371,32386,32402,32416,32431,32445,32459,32473,32485,32498,32512,32524,32535,32547,32559,32570,32581,32591,32601,32611,32621,32630,32640,32648,32656,32664,32672,32680,32687,32694,32700,32707,32713,32718,32723,32729,32733,32737,32741,32745,32749,32751,32755,32757,32759,32761,32763,32764,32765,32765,32766,32766,32766,32765,32764,32763,32762,32760,32758,32755,32752,32750,32746,32742,32738,32734,32730,32725,32720,32714,32708,32702,32696,32689,32682,32675,32667,32659,32651,32642,32634,32624,32614,32605,32595,32585,32573,32563,32552,32540,32528,32515,32503,32490,32477,32463,32449,32436,32421,32407,32392,32376,32361,32344,32328,32311,32295,32278,32260,32243,32225,32206,32187,32168,32149,32130,32110,32090,32070,32049,32028,32006,31985,31962,31940,31917,31895,31872,31848,31825,31801,31776,31751,31726,31701,31676,31650,31623,31597,31570,31543,31516,31488,31460,31432,31403,31375,31346,31316,31287,31256,31226,31195,31164,31133,31102,31070,31038,31006,30973,30940,30906,30873,30839,30806,30771,30736,30701,30666,30630,30594,30558,30522,30485,30448,30411,30373,30336,30298,30259,30221,30181,30142,30103,30063,30022,29982,29942,29901,29860,29817,29776,29734,29691,29649,29606,29562,29519,29475,29431,29387,29342,29297,29253,29207,29161,29115,29069,29023,28976,28928,28881,28834,28785,28738,28689,28640,28591,28543,28493,28442,28393,28342,28292,28241,28190,28139,28087,28035,27983,27931,27878,27824,27771,27718,27664,27610,27556,27502,27447,27392,27336,27281,27225,27169,27113,27056,26999,26942,26884,26827,26769,26712,26653,26595,26536,26477,26417,26358,26297,26238,26178,26116,26056,25995,25933,25872,25810,25748,25686,25623,25560,25497,25434,25371,25307,25243,25178,25114,25049,24985,24919,24854,24789,24723,24657,24590,24524,24457,24390,24322,24255,24188,24120,24051,23983,23914,23846,23777,23707,23638,23568,23498,23428,23357,23287,23216,23145,23074,23002,22931,22858,22786,22714,22642,22569,22496,22423,22350,22276,22201,22128,22053,21979,21905,21830,21755,21679,21604,21528,21452,21376,21300,21223,21147,21070,20993,20916,20838,20760,20683,20604,20526,20448,20369,20290,20211,20132,20053,19973,19893,19813,19733,19653,19572,19491,19411,19329,19248,19167,19085,19003,18921,18839,18757,18674,18592,18509,18425,18342,18259,18175,18092,18008,17924,17840,17755,17671,17586,17501,17416,17331,17245,17160,17074,16988,16902,16816,16730,16643,16557,16470,16383,16296,16209,16121,16034,15946,15858,15770,15682,15594,15505,15417,15328,15239,15150,15061,14971,14882,14792,14702,14612,14523,14433,14342,14252,14161,14070,13979,13888,13798,13706,13615,13523,13431,13340,13248,13156,13064,12972,12879,12787,12694,12602,12508,12415,12322,12230,12136,12043,11949,11855,11762,11667,11574,11480,11385,11291,11196,11102,11007,10913,10818,10723,10628,10533,10437,10342,10246,10151,10056,9960,9864,9768,9672,9576,9480,9383,9287,9190,9094,8998,8901,8804,8707,8610,8513,8416,8319,8222,8124,8027,7929,7831,7734,7636,7538,7441,7342,7245,7146,7048,6950,6852,6753,6655,6557,6458,6359,6261,6162,6064,5965,5866,5767,5668,5569,5470,5371,5271,5172,5073,4974,4874,4775,4675,4576,4476,4377,4277,4177,4078,3977,3878,3778,3678,3578,3478,3378,3278,3178,3078,2978,2878,2778,2678,2578,2477,2377,2277,2177,2077,1976,1876,1775,1675,1575,1474,1374,1273,1173,1073,971,871,770,671,570,469,368,268,167,67,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,67,167,268,368,469,570,671,770,871,971,1073,1173,1273,1374,1474,1575,1675,1775,1876,1976,2077,2177,2277,2377,2477,2578,2678,2778,2878,2978,3078,3178,3278,3378,3478,3578,3678,3778,3878,3977,4078,4177,4277,4377,4476,4576,4675,4775,4874,4974,5073,5172,5271,5371,5470,5569,5668,5767,5866,5965,6064,6162,6261,6359,6458,6557,6655,6753,6852,6950,7048,7146,7245,7342,7441,7538,7636,7734,7831,7929,8027,8124,8222,8319,8416,8513,8610,8707,8804,8901,8998,9094,9190,9287,9383,9480,9576,9672,9768,9864,9960,10056,10151,10246,10342,10437,10533,10628,10723,10818,10913,11007,11102,11196,11291,11385,11480,11574,11667,11762,11855,11949,12043,12136,12230,12322,12415,12508,12602,12694,12787,12879,12972,13064,13156,13248,13340,13431,13523,13615,13706,13798,13888,13979,14070,14161,14252,14342,14433,14523,14612,14702,14792,14882,14971,15061,15150,15239,15328,15417,15505,15594,15682,15770,15858,15946,16034,16121,16209,16296,16383,16470,16557,16643,16730,16816,16902,16988,17074,17160,17245,17331,17416,17501,17586,17671,17755,17840,17924,18008,18092,18175,18259,18342,18425,18509,18592,18674,18757,18839,18921,19003,19085,19167,19248,19329,19411,19491,19572,19653,19733,19813,19893,19973,20053,20132,20211,20290,20369,20448,20526,20604,20683,20760,20838,20916,20993,21070,21147,21223,21300,21376,21452,21528,21604,21679,21755,21830,21905,21979,22053,22128,22201,22276,22350,22423,22496,22569,22642,22714,22786,22858,22931,23002,23074,23145,23216,23287,23357,23428,23498,23568,23638,23707,23777,23846,23914,23983,24051,24120,24188,24255,24322,24390,24457,24524,24590,24657,24723,24789,24854,24919,24985,25049,25114,25178,25243,25307,25371,25434,25497,25560,25623,25686,25748,25810,25872,25933,25995,26056,26116,26178,26238,26297,26358,26417,26477,26536,26595,26653,26712,26769,26827,26884,26942,26999,27056,27113,27169,27225,27281,27336,27392,27447,27502,27556,27610,27664,27718,27771,27824,27878,27931,27983,28035,28087,28139,28190,28241,28292,28342,28393,28442,28493,28543,28591,28640,28689,28738,28785,28834,28881,28928,28976,29023,29069,29115,29161,29207,29253,29297,29342,29387,29431,29475,29519,29562,29606,29649,29691,29734,29776,29817,29860,29901,29942,29982,30022,30063,30103,30142,30181,30221,30259,30298,30336,30373,30411,30448,30485,30522,30558,30594,30630,30666,30701,30736,30771,30806,30839,30873,30906,30940,30973,31006,31038,31070,31102,31133,31164,31195,31226,31256,31287,31316,31346,31375,31403,31432,31460,31488,31516,31543,31570,31597,31623,31650,31676,31701,31726,31751,31776,31801,31825,31848,31872,31895,31917,31940,31962,31985,32006,32028,32049,32070,32090,32110,32130,32149,32168,32187,32206,32225,32243,32260,32278,32295,32311,32328,32344,32361,32376,32392,32407,32421,32436,32449,32463,32477,32490,32503,32515,32528,32540,32552,32563,32573,32585,32595,32605,32614,32624,32634,32642,32651,32659,32667,32675,32682,32689,32696,32702,32708,32714,32720,32725,32730,32734,32738,32742,32746,32750,32752,32755,32758,32760,32762,32763,32764,32765,32766,32766,32766,32765,32765,32764,32763,32761,32759,32757,32755,32751,32749,32745,32741,32737,32733,32729,32723,32718,32713,32707,32700,32694,32687,32680,32672,32664,32656,32648,32640,32630,32621,32611,32601,32591,32581,32570,32559,32547,32535,32524,32512,32498,32485,32473,32459,32445,32431,32416,32402,32386,32371,32356,32339,32323,32306,32289,32272,32254,32237,32218,32200,32182,32163,32143,32123,32103,32083,32063,32042,32021,31999,31978,31955,31933,31910,31887,31863,31840,31816,31792,31768,31743,31718,31693,31667,31641,31615,31588,31561,31534,31507,31479,31450,31422,31394,31365,31335,31306,31276,31246,31216,31185,31154,31122,31091,31060,31027,30994,30962,30929,30896,30861,30828,30794,30759,30725,30690,30654,30618,30582,30546,30510,30473,30436,30398,30361,30322,30284,30246,30207,30168,30129,30089,30049,30009,29969,29928,29887,29845,29804,29761,29720,29677,29635,29591,29548,29504,29460,29416,29372,29327,29282,29237,29192,29146,29100,29053,29006,28960,28913,28866,28817,28770,28721,28672,28624,28575,28526,28476,28426};
-
-const short
-bldc_phase2[]={
-0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,33,134,235,336,435,536,637,737,837,938,1039,1139,1240,1340,1441,1540,1641,1742,1842,1942,2042,2143,2243,2344,2444,2544,2645,2745,2845,2945,3045,3145,3246,3345,3445,3546,3645,3745,3845,3945,4044,4144,4244,4344,4443,4542,4642,4742,4841,4941,5040,5139,5238,5338,5437,5535,5635,5734,5833,5932,6030,6129,6228,6327,6426,6524,6623,6721,6819,6917,7016,7114,7212,7310,7408,7506,7604,7701,7799,7897,7994,8091,8189,8286,8383,8481,8578,8674,8772,8869,8965,9062,9159,9255,9352,9448,9544,9640,9736,9832,9928,10024,10119,10215,10310,10406,10501,10596,10691,10786,10881,10976,11071,11165,11259,11353,11448,11542,11636,11730,11824,11918,12012,12105,12198,12291,12384,12478,12570,12663,12756,12848,12941,13033,13125,13217,13309,13401,13493,13584,13675,13767,13858,13949,14040,14131,14222,14311,14402,14492,14582,14672,14762,14852,14942,15031,15120,15209,15298,15387,15476,15564,15653,15741,15829,15917,16005,16092,16180,16267,16354,16440,16527,16614,16700,16787,16873,16959,17045,17131,17216,17302,17387,17472,17557,17642,17727,17811,17895,17980,18064,18147,18231,18315,18398,18481,18564,18647,18729,18812,18894,18976,19058,19139,19221,19303,19383,19464,19545,19625,19706,19787,19866,19946,20026,20106,20184,20264,20342,20422,20500,20578,20657,20734,20812,20889,20967,21044,21121,21198,21274,21350,21427,21503,21578,21654,21729,21805,21880,21954,22029,22103,22178,22251,22325,22398,22472,22544,22618,22690,22762,22835,22907,22978,23050,23121,23193,23264,23335,23404,23474,23545,23615,23684,23753,23823,23892,23960,24029,24097,24164,24233,24300,24367,24434,24501,24569,24635,24700,24766,24832,24898,24963,25028,25093,25157,25222,25286,25349,25413,25476,25539,25602,25665,25727,25789,25852,25913,25974,26036,26096,26157,26217,26278,26338,26397,26457,26516,26575,26633,26692,26750,26808,26866,26924,26980,27037,27094,27150,27206,27262,27318,27373,27429,27484,27538,27593,27646,27700,27754,27807,27861,27913,27966,28018,28069,28121,28173,28224,28275,28325,28377,28426,28476,28526,28575,28624,28672,28721,28770,28817,28866,28913,28960,29006,29053,29100,29146,29192,29237,29282,29327,29372,29416,29460,29504,29548,29591,29635,29677,29720,29761,29804,29845,29887,29928,29969,30009,30049,30089,30129,30168,30207,30246,30284,30322,30361,30398,30436,30473,30510,30546,30582,30618,30654,30690,30725,30759,30794,30828,30861,30896,30929,30962,30994,31027,31060,31091,31122,31154,31185,31216,31246,31276,31306,31335,31365,31394,31422,31450,31479,31507,31534,31561,31588,31615,31641,31667,31693,31718,31743,31768,31792,31816,31840,31863,31887,31910,31933,31955,31978,31999,32021,32042,32063,32083,32103,32123,32143,32163,32182,32200,32218,32237,32255,32272,32289,32306,32323,32339,32356,32371,32386,32402,32416,32431,32445,32459,32473,32485,32498,32512,32524,32535,32547,32559,32570,32581,32591,32601,32611,32621,32630,32640,32648,32656,32664,32672,32680,32687,32694,32700,32707,32713,32718,32723,32729,32733,32737,32741,32745,32749,32751,32755,32757,32759,32761,32763,32764,32765,32765,32766,32766,32766,32765,32764,32763,32762,32760,32758,32755,32752,32750,32746,32742,32738,32734,32730,32725,32720,32714,32708,32702,32696,32689,32682,32675,32667,32659,32651,32642,32634,32624,32614,32605,32595,32585,32573,32563,32552,32540,32528,32515,32503,32490,32477,32463,32449,32436,32421,32407,32392,32376,32361,32344,32328,32311,32295,32278,32260,32243,32225,32206,32187,32168,32149,32130,32110,32090,32070,32049,32028,32006,31985,31962,31940,31917,31895,31872,31848,31825,31801,31776,31751,31726,31701,31676,31650,31623,31597,31570,31543,31516,31488,31460,31432,31403,31375,31346,31316,31287,31256,31226,31195,31164,31133,31102,31070,31038,31006,30973,30940,30906,30873,30839,30806,30771,30736,30701,30666,30630,30594,30558,30522,30485,30448,30411,30373,30336,30298,30259,30221,30181,30142,30103,30063,30022,29982,29942,29901,29860,29817,29776,29734,29691,29649,29606,29562,29519,29475,29431,29387,29342,29297,29253,29207,29161,29115,29069,29023,28976,28928,28881,28834,28785,28738,28689,28640,28591,28543,28493,28442,28393,28409,28459,28509,28558,28608,28657,28706,28753,28802,28849,28897,28944,28991,29038,29084,29131,29177,29222,29268,29312,29358,29402,29446,29490,29533,29577,29620,29662,29705,29747,29790,29831,29873,29914,29955,29995,30036,30075,30116,30155,30194,30233,30272,30310,30348,30386,30423,30461,30497,30534,30570,30606,30642,30678,30713,30747,30782,30816,30851,30884,30918,30951,30984,31016,31048,31081,31112,31143,31174,31205,31236,31266,31296,31325,31355,31384,31413,31441,31469,31497,31525,31552,31579,31606,31632,31658,31684,31709,31735,31759,31784,31808,31832,31856,31879,31903,31926,31948,31969,31992,32013,32035,32056,32076,32097,32116,32137,32156,32175,32194,32213,32230,32249,32266,32284,32301,32318,32334,32350,32366,32381,32397,32412,32426,32441,32454,32468,32481,32495,32507,32520,32532,32544,32555,32567,32577,32588,32598,32608,32618,32627,32637,32645,32653,32662,32670,32677,32684,32692,32698,32704,32710,32716,32722,32726,32732,32736,32740,32744,32748,32750,32754,32756,32758,32760,32762,32764,32764,32766,32766,32766,32766,32766,32764,32764,32762,32760,32758,32756,32754,32750,32748,32744,32740,32736,32732,32726,32722,32716,32710,32704,32698,32692,32684,32677,32670,32662,32653,32645,32637,32627,32618,32608,32598,32588,32577,32567,32555,32544,32532,32520,32507,32495,32481,32468,32454,32441,32426,32412,32397,32381,32366,32350,32334,32318,32301,32284,32266,32249,32230,32213,32194,32175,32156,32137,32116,32097,32076,32056,32035,32013,31992,31969,31948,31926,31903,31879,31856,31832,31808,31784,31759,31735,31709,31684,31658,31632,31606,31579,31552,31525,31497,31469,31441,31413,31384,31355,31325,31296,31266,31236,31205,31174,31143,31112,31081,31048,31016,30984,30951,30918,30884,30851,30816,30782,30747,30713,30678,30642,30606,30570,30534,30497,30461,30423,30386,30348,30310,30272,30233,30194,30155,30116,30075,30036,29995,29955,29914,29873,29831,29790,29747,29705,29662,29620,29577,29533,29490,29446,29402,29358,29312,29268,29222,29177,29131,29084,29038,28991,28944,28897,28849,28802,28753,28706,28657,28608,28558,28509,28459,28409,28360,28308,28258,28207,28156,28104,28052,28001,27948,27896,27842,27789,27736,27683,27628,27575,27520,27465,27411,27355,27300,27244,27188,27131,27075,27018,26961,26904,26846,26789,26731,26672,26614,26556,26497,26436,26377,26318,26258,26197,26137,26077,26015,25954,25893,25831,25769,25706,25644,25581,25518,25455,25392,25328,25264,25201,25136,25072,25006,24941,24876,24811,24745,24679,24612,24546,24479,24412,24345,24278,24210,24142,24075,24006,23938,23869,23799,23731,23661,23591,23522,23452,23381,23311,23240,23169,23098,23027,22954,22882,22811,22739,22666,22593,22520,22447,22373,22300,22226,22153,22078,22004,21930,21855,21780,21704,21629,21553,21477,21401,21325,21249,21172,21095,21019,20942,20863,20786,20708,20631,20552,20474,20395,20316,20238,20158,20079,19999,19920,19840,19759,19680,19599,19519,19438,19356,19276,19194,19112,19030,18949,18867,18784,18702,18619,18536,18453,18371,18287,18204,18120,18036,17952,17868,17783,17699,17614,17529,17444,17359,17274,17188,17102,17017,16931,16845,16758,16672,16585,16499,16412,16326,16238,16151,16064,15976,15888,15800,15712,15624,15535,15447,15358,15269,15180,15091,15002,14912,14823,14733,14643,14553,14463,14372,14282,14191,14100,14010,13919,13828,13736,13645,13553,13462,13370,13278,13187,13095,13002,12910,12818,12725,12632,12539,12447,12353,12261,12167,12074,11980,11886,11793,11699,11605,11511,11417,11322,11228,11133,11039,10944,10850,10755,10660,10565,10469,10374,10278,10183,10088,9992,9896,9800,9704,9608,9512,9415,9319,9222,9126,9030,8933,8836,8740,8643,8546,8448,8351,8253,8157,8060,7962,7864,7766,7669,7571,7473,7375,7277,7179,7082,6983,6885,6787,6688,6590,6491,6392,6294,6196,6097,5998,5898,5800,5701,5602,5503,5404,5305,5205,5106,5007,4908,4808,4709,4609,4509,4409,4310,4210,4111,4011,3912,3811,3711,3612,3512,3412,3312,3212,3112,3011,2912,2811,2711,2611,2511,2410,2310,2210,2110,2009,1909,1808,1708,1608,1507,1407,1306,1206,1106,1005,904,804,703,603,503,402,302,201,101};
-
-const short
-bldc_phase3[]={
-0,101,201,302,402,503,603,703,804,904,1005,1106,1206,1306,1407,1507,1608,1708,1808,1909,2009,2110,2210,2310,2410,2511,2611,2711,2811,2912,3011,3112,3212,3312,3412,3512,3612,3711,3811,3912,4011,4111,4210,4310,4409,4509,4609,4709,4808,4908,5007,5106,5205,5305,5404,5503,5602,5701,5800,5898,5998,6097,6196,6294,6392,6491,6590,6688,6787,6885,6983,7082,7179,7277,7375,7473,7571,7669,7766,7864,7962,8060,8157,8253,8351,8448,8546,8643,8740,8836,8933,9030,9126,9222,9319,9415,9512,9608,9704,9800,9896,9992,10088,10183,10278,10374,10469,10565,10660,10755,10850,10944,11039,11133,11228,11322,11417,11511,11605,11699,11793,11886,11980,12074,12167,12261,12353,12447,12539,12632,12725,12818,12910,13002,13095,13187,13278,13370,13462,13553,13645,13736,13828,13919,14009,14100,14191,14282,14372,14463,14553,14643,14733,14823,14912,15002,15091,15180,15269,15358,15447,15535,15624,15712,15800,15888,15976,16064,16151,16238,16326,16412,16499,16585,16672,16758,16845,16931,17017,17102,17188,17274,17359,17444,17529,17614,17699,17783,17868,17952,18036,18120,18204,18287,18371,18453,18536,18619,18702,18784,18867,18949,19030,19112,19194,19276,19356,19438,19519,19599,19680,19759,19840,19920,19999,20079,20158,20238,20316,20395,20474,20552,20631,20708,20786,20863,20942,21019,21095,21172,21249,21325,21401,21477,21553,21629,21704,21780,21855,21930,22004,22078,22153,22226,22300,22373,22447,22520,22593,22666,22739,22811,22882,22954,23027,23098,23169,23240,23311,23381,23452,23522,23591,23661,23731,23799,23869,23938,24006,24075,24142,24210,24278,24345,24412,24479,24546,24612,24679,24745,24811,24876,24941,25006,25072,25136,25201,25264,25328,25392,25455,25518,25581,25644,25706,25769,25831,25893,25954,26015,26077,26137,26197,26258,26318,26377,26436,26497,26556,26614,26672,26731,26789,26846,26904,26961,27018,27075,27131,27188,27244,27300,27355,27411,27465,27520,27575,27628,27683,27736,27789,27842,27896,27948,28001,28052,28104,28156,28207,28258,28308,28360,28409,28459,28509,28558,28608,28657,28706,28753,28802,28849,28897,28944,28991,29038,29084,29131,29177,29222,29268,29312,29358,29402,29446,29490,29533,29577,29620,29662,29705,29747,29790,29831,29873,29914,29955,29995,30036,30075,30116,30155,30194,30233,30272,30310,30348,30386,30423,30461,30497,30534,30570,30606,30642,30678,30713,30747,30782,30816,30851,30884,30918,30951,30984,31016,31048,31081,31112,31143,31174,31205,31236,31266,31296,31325,31355,31384,31413,31441,31469,31497,31525,31552,31579,31606,31632,31658,31684,31709,31735,31759,31784,31808,31832,31856,31879,31903,31926,31948,31969,31992,32013,32035,32056,32076,32097,32116,32137,32156,32175,32194,32213,32230,32249,32266,32284,32301,32318,32334,32350,32366,32381,32397,32412,32426,32441,32454,32468,32481,32495,32507,32520,32532,32544,32555,32567,32577,32588,32598,32608,32618,32627,32637,32645,32653,32662,32670,32677,32684,32692,32698,32704,32710,32716,32722,32726,32732,32736,32740,32744,32748,32750,32754,32756,32758,32760,32762,32764,32764,32766,32766,32766,32766,32766,32764,32764,32762,32760,32758,32756,32754,32750,32748,32744,32740,32736,32732,32726,32722,32716,32710,32704,32698,32692,32684,32677,32670,32662,32653,32645,32637,32627,32618,32608,32598,32588,32577,32567,32555,32544,32532,32520,32507,32495,32481,32468,32454,32441,32426,32412,32397,32381,32366,32350,32334,32318,32301,32284,32266,32249,32230,32213,32194,32175,32156,32137,32116,32097,32076,32056,32035,32013,31992,31969,31948,31926,31903,31879,31856,31832,31808,31784,31759,31735,31709,31684,31658,31632,31606,31579,31552,31525,31497,31469,31441,31413,31384,31355,31325,31296,31266,31236,31205,31174,31143,31112,31081,31048,31016,30984,30951,30918,30884,30851,30816,30782,30747,30713,30678,30642,30606,30570,30534,30497,30461,30423,30386,30348,30310,30272,30233,30194,30155,30116,30075,30036,29995,29955,29914,29873,29831,29790,29747,29705,29662,29620,29577,29533,29490,29446,29402,29358,29312,29268,29222,29177,29131,29084,29038,28991,28944,28897,28849,28802,28753,28706,28657,28608,28558,28509,28459,28409,28393,28442,28493,28543,28591,28640,28689,28738,28785,28834,28881,28928,28976,29023,29069,29115,29161,29207,29253,29297,29342,29387,29431,29475,29519,29562,29606,29649,29691,29734,29776,29817,29860,29901,29942,29982,30022,30063,30103,30142,30181,30221,30259,30298,30336,30373,30411,30448,30485,30522,30558,30594,30630,30666,30701,30736,30771,30806,30839,30873,30906,30940,30973,31006,31038,31070,31102,31133,31164,31195,31226,31256,31287,31316,31346,31375,31403,31432,31460,31488,31516,31543,31570,31597,31623,31650,31676,31701,31726,31751,31776,31801,31825,31848,31872,31895,31917,31940,31962,31985,32006,32028,32049,32070,32090,32110,32130,32149,32168,32187,32206,32225,32243,32260,32278,32295,32311,32328,32344,32361,32376,32392,32407,32421,32436,32449,32463,32477,32490,32503,32515,32528,32540,32552,32563,32573,32585,32595,32605,32614,32624,32634,32642,32651,32659,32667,32675,32682,32689,32696,32702,32708,32714,32720,32725,32730,32734,32738,32742,32746,32750,32752,32755,32758,32760,32762,32763,32764,32765,32766,32766,32766,32765,32765,32764,32763,32761,32759,32757,32755,32751,32749,32745,32741,32737,32733,32729,32723,32718,32713,32707,32700,32694,32687,32680,32672,32664,32656,32648,32640,32630,32621,32611,32601,32591,32581,32570,32559,32547,32535,32524,32512,32498,32485,32473,32459,32445,32431,32416,32402,32386,32371,32356,32339,32323,32306,32289,32272,32254,32237,32218,32200,32182,32163,32143,32123,32103,32083,32063,32042,32021,31999,31978,31955,31933,31910,31887,31863,31840,31816,31792,31768,31743,31718,31693,31667,31641,31615,31588,31561,31534,31507,31479,31450,31422,31394,31365,31335,31306,31276,31246,31216,31185,31154,31122,31091,31060,31027,30994,30962,30929,30896,30861,30828,30794,30759,30725,30690,30654,30618,30582,30546,30510,30473,30436,30398,30361,30322,30284,30246,30207,30168,30129,30089,30049,30009,29969,29928,29887,29845,29804,29761,29720,29677,29635,29591,29548,29504,29460,29416,29372,29327,29282,29237,29192,29146,29100,29053,29006,28960,28913,28866,28817,28770,28721,28672,28624,28575,28526,28476,28426,28377,28325,28275,28224,28173,28121,28069,28018,27966,27913,27861,27807,27754,27700,27646,27593,27538,27484,27429,27373,27318,27262,27206,27150,27094,27037,26980,26924,26866,26808,26750,26692,26633,26575,26516,26457,26397,26338,26278,26217,26157,26096,26036,25974,25913,25852,25789,25727,25665,25602,25539,25476,25413,25349,25286,25222,25157,25093,25028,24963,24898,24832,24766,24700,24635,24569,24501,24434,24367,24300,24233,24164,24097,24029,23960,23892,23823,23753,23684,23615,23545,23474,23404,23335,23264,23193,23121,23050,22978,22907,22835,22762,22690,22618,22544,22472,22398,22325,22251,22178,22103,22029,21954,21880,21805,21729,21654,21578,21503,21427,21350,21274,21198,21121,21044,20967,20889,20812,20734,20657,20578,20500,20422,20342,20264,20184,20106,20026,19946,19866,19787,19706,19625,19545,19464,19383,19303,19221,19139,19058,18976,18894,18812,18729,18647,18564,18481,18398,18315,18231,18147,18064,17980,17895,17811,17727,17642,17557,17472,17387,17302,17216,17131,17045,16959,16873,16787,16700,16614,16527,16440,16354,16267,16180,16092,16005,15917,15829,15741,15653,15564,15476,15387,15298,15209,15120,15031,14942,14852,14762,14672,14582,14492,14402,14311,14222,14131,14040,13949,13858,13767,13676,13584,13493,13401,13309,13217,13125,13033,12941,12848,12756,12663,12570,12478,12384,12291,12198,12105,12012,11918,11824,11730,11636,11542,11448,11353,11259,11165,11071,10976,10881,10786,10691,10596,10501,10406,10310,10215,10119,10024,9928,9832,9736,9640,9544,9448,9352,9255,9159,9062,8965,8869,8772,8674,8578,8481,8383,8286,8189,8091,7994,7897,7799,7701,7604,7506,7408,7310,7212,7114,7016,6917,6819,6721,6623,6524,6426,6327,6228,6129,6030,5932,5833,5734,5635,5535,5437,5338,5238,5139,5040,4941,4841,4742,4642,4542,4443,4344,4244,4144,4044,3945,3845,3745,3645,3546,3445,3345,3246,3145,3045,2945,2845,2745,2645,2544,2444,2344,2243,2143,2042,1942,1842,1742,1641,1540,1441,1340,1240,1139,1039,938,837,737,637,536,435,336,235,134,33,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
-
+++ /dev/null
-/* increment subdivission is fixed, else selectable by pxms_subdiv */
-#define PXMC_WITH_FIXED_SUBDIV
-/* multiphase motor control */
-#define PXMC_WITH_PHASE_TABLE
-/* controller with finegrained speed generation */
-#define PXMC_WITH_FINE_GRAINED
-/* special calling convention for pxms_??? state functions */
-//#define PXMC_WITH_FAST_CALL
-/* possibility to select generator variant per axis */
-//#define PXMC_WITH_GEN_SELECTION
-/* whether to compile in debugger support */
-#define PXMC_WITH_DBG_HIST
-/* controller with current feedback */
-//#define PXMC_WITH_CURRENTFB
-/* The pxmc_set/clear_flags not supported, use one by one pxmc_set/clear_flag */
-#define PXMC_WITH_FLAGS_BYBITS_ONLY
-
-#ifndef PXMC_SUBDIV
-#ifdef PXMC_WITH_FIXED_SUBDIV
-/* number of fraction bits in integer part of possition */
-#define PXMC_SUBDIV(mcs) 8
-#else /*PXMC_WITH_FIXED_SUBDIV*/
-#define PXMC_SUBDIV(mcs) (mcs->pxms_subdiv)
-#endif /*PXMC_WITH_FIXED_SUBDIV*/
-#endif /*PXMC_SUBDIV*/
+++ /dev/null
-#define _GNU_SOURCE
-#include <stdio.h>
-#include <stdlib.h>
-#include <malloc.h>
-#include <getopt.h>
-#include <math.h>
-#include <string.h>
-
-typedef int phase_t;
-
-
-int
-gen_ptable_sin(phase_t *phase, int ptirc, int ptper, int ptlen, int ptamp, float offs)
-{
- int i;
- float a;
-
- for(i=0;i<ptlen;i++) {
- a=(i+offs)*(float)ptper*2*M_PI/ptirc;
- phase[i]=sin(a)*ptamp;
- }
- return 0;
-}
-
-int
-gen_ptable_sin3ph(phase_t *phase1, phase_t *phase2, phase_t *phase3, int ptirc, int ptper, int ptlen, int ptamp)
-{
- int i;
- float a;
-
- for(i=0;i<ptlen;i++) {
- a=i*(float)ptper*2*M_PI/ptirc;
- phase1[i]=cos(a)*ptamp;
- phase2[i]=cos(a+2*M_PI/3)*ptamp;
- phase3[i]=cos(a+4*M_PI/3)*ptamp;
- }
- return 0;
-}
-
-int
-gen_ptable_sin3phup(phase_t *phase1, phase_t *phase2, phase_t *phase3, int ptirc, int ptper, int ptlen, int ptamp)
-{
- int i;
- int min_val;
- float a, amp;
- long p1, p2, p3;
-
- amp=(float)ptamp/2/sin(M_PI/3);
-
- for(i=0;i<ptlen;i++) {
- a=i*(float)ptper*2*M_PI/ptirc;
- p1=cos(a)*amp;
- p2=cos(a+2*M_PI/3)*amp;
- p3=cos(a+4*M_PI/3)*amp;
-
- min_val=p1;
- if(min_val>p2)
- min_val=p2;
- if(min_val>p3)
- min_val=p3;
-
- phase1[i]=p1-min_val;
- phase2[i]=p2-min_val;
- phase3[i]=p3-min_val;
- }
- return 0;
-}
-
-
-
-/********************************************************************/
-
-int o_ptirc=4*16;
-int o_ptper=1;
-int o_ptamp=0x7fff;
-int o_phasenum=2;
-int o_combined=0;
-char *o_shape="sin";
-char *o_format=NULL;
-char *o_output=NULL;
-char *o_ptable_name="smc_ptable";
-
-int o_ptlen;
-int o_ptnum;
-
-float o_ptoffset;
-
-phase_t *phases[10];
-
-void gen_ptables(void)
-{
- int i;
- if (strcmp(o_shape, "sin") == 0) {
- for(i=0;i<o_ptnum;i++){
- gen_ptable_sin(phases[i], o_ptirc, o_ptper, o_ptlen, o_ptamp, i*o_ptoffset);
- }
- }
-
- if (o_ptnum>=3 && strcmp(o_shape, "sin3ph") == 0) {
- gen_ptable_sin3ph(phases[0], phases[1], phases[2], o_ptirc, o_ptper, o_ptlen, o_ptamp);
- }
-
- if (o_ptnum>=3 && strcmp(o_shape, "sin3phup") == 0) {
- gen_ptable_sin3phup(phases[0], phases[1], phases[2], o_ptirc, o_ptper, o_ptlen, o_ptamp);
- }
-}
-
-static void
-usage(void)
-{
- printf("usage: gen_phase_table <options>\n");
- printf(" -i, --ptirc <irc> number of the irc per one phase table\n");
- printf(" -p, --ptper <per> number of electrical periods per one phase table\n");
- printf(" -a, --ptamp <amp> phase table amplitude\n");
- printf(" -n, --phasenum <num> number of motor phases\n");
- printf(" -c, --combined combined phase table to conserve space\n");
- printf(" -s, --shape <shape> phase shape {sin,sin3ph,sin3phup}\n");
- printf(" -f, --format <format> output format\n");
- printf(" -N, --name <name> phase table name\n");
- printf(" -o, --output <file> output filename\n");
-}
-
-int main(int argc,char *argv[])
-{
- static struct option long_opts[] = {
- { "ptirc", 1, 0, 'i' },
- { "ptper", 1, 0, 'p' },
- { "ptamp", 1, 0, 'a' },
- { "phasenum",1, 0, 'n' },
- { "combined",0, 0, 'c' },
- { "shape", 1, 0, 's' },
- { "format", 1, 0, 'f' },
- { "name", 1, 0, 'N' },
- { "output", 1, 0, 'o' },
- { "help", 0, 0, 'h' },
- { 0, 0, 0, 0}
- };
- int opt, i, j;
- FILE *F;
-
- while ((opt = getopt_long(argc, argv, "i:p:a:n:cs:f:N:o:h",
- &long_opts[0], NULL)) != EOF){
- switch (opt) {
- case 'i':
- o_ptirc = strtol(optarg,NULL,0);
- break;
- case 'p':
- o_ptper = strtol(optarg,NULL,0);
- break;
- case 'a':
- o_ptamp = strtol(optarg,NULL,0);
- break;
- case 'n':
- o_phasenum = strtol(optarg,NULL,0);
- break;
- case 'c':
- o_combined = 1;
- break;
- case 's':
- o_shape = optarg;
- break;
- case 'f':
- o_format = optarg;
- break;
- case 'N':
- o_ptable_name = optarg;
- break;
- case 'o':
- o_output = optarg;
- break;
- case 'h':
- default:
- usage();
- exit(opt == 'h' ? 0 : 1);
- }
- }
-
- o_ptoffset=(float)o_ptirc/(float)o_ptper/(float)o_phasenum/2;
- if(!o_combined){
- o_ptlen=o_ptirc;
- o_ptnum=o_phasenum;
- }else{
- o_ptlen=o_ptirc+round(o_ptoffset*(o_phasenum-1));
- o_ptnum=1;
- }
-
- for(i=0;i<o_ptnum;i++){
- phases[i]=malloc(sizeof(phase_t)*o_ptlen);
- }
-
- gen_ptables();
-
- if(o_output==NULL){
- F=stdout;
- }else{
- F=fopen(o_output,"w");
- if(!F){
- fprintf(stderr, "%s: cannot open file %s for writting\n", argv[0], o_output);
- exit(1);
- }
- }
-
- fprintf(F,"/* Generated phase table, do not edit */\n\n");
- fprintf(F,"/* ptirc=%d ptper=%d ptamp=%d phasenum=%d combined=%d*/\n",
- o_ptirc,o_ptper,o_ptamp,o_phasenum,o_combined);
- fprintf(F,"/* shape=\"%s\" */\n",o_shape);
- fprintf(F,"/* ptable_name=\"%s\" */\n",o_ptable_name);
-
- fprintf(F,"\n");
- fprintf(F,"#define %s_ptirc %d\n",o_ptable_name,o_ptirc);
- fprintf(F,"#define %s_ptper %d\n",o_ptable_name,o_ptper);
- fprintf(F,"\n");
-
- for(i=0;i<o_ptnum;i++){
- fprintf(F,"const short\n%s_phase%d[]={\n",o_ptable_name,i+1);
- if (o_format == NULL) {
- for(j=0;j<o_ptlen;j++){
- fprintf(F," %6d%s",phases[i][j],j<o_ptlen-1?",\n":"");
- }
- fprintf(F,"\n};\n\n");
- } else {
- if (strcmp(o_format, "oneline")==0) {
- for(j=0;j<o_ptlen;j++){
- fprintf(F,"%d%s",phases[i][j],j<o_ptlen-1?",":"");
- }
- fprintf(F,"};\n\n");
- }
- }
- }
-
- if(o_combined){
- for(i=1;i<o_phasenum;i++){
- fprintf(F,"# define %s_phase%d (%s_phase1+%d)\n",
- o_ptable_name,i+1,o_ptable_name,(int)round(o_ptoffset*(i)));
- }
- }
-
- fclose(F);
-
- return 0;
-}
-
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc.h - generic multi axis motion controller interface
- position controller subsystem core definitions
-
- (C) 2001-2005 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (C) 2002-2005 by PiKRON Ltd. http://www.pikron.com
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
-
- *******************************************************************/
-
-#ifndef _PXMC_H_
-#define _PXMC_H_
-
-#include "pxmc_config.h"
-
-#ifdef PXMC_WITH_FINE_GRAINED
-#ifndef PXMC_SUBDIVFG
-/* additional number of fraction bits for fine grainned generator */
-#define PXMC_SUBDIVFG(mcs) 16
-#endif
-
-#if PXMC_SUBDIVFG(0)>16
-typedef long pxmc_fg_t;
-#else
-typedef short pxmc_fg_t;
-#endif
-
-#if PXMC_SUBDIVFG(0)>=32
- #define PXMC_SPDFG_ONLY
-#endif
-#endif /*PXMC_WITH_FINE_GRAINED*/
-
-
-/* General measurement, controller, output
- and profile generator structure */
-
-/* controler state flags stored in "pxms_flg" field */
-
-#define PXMS_ENI_b 0 /* enable input (IRC) update */
-#define PXMS_ENR_b 1 /* enable controller (regulator) execution and output update */
-#define PXMS_ENG_b 2 /* enable requested value (position) generator */
-#define PXMS_ERR_b 3 /* axis in error state */
-#define PXMS_BSY_b 4 /* axis busy */
-#define PXMS_DBG_b 5 /* enable debugging */
-#define PXMS_CMV_b 6 /* in coordinated group */
-#define PXMS_CQF_b 7 /* command queue full */
-#define PXMS_PHA_b 8 /* phase alligned */
-#define PXMS_PTI_b 9 /* update index pointing to the phase table (ptindx) */
-#define PXMS_ENO_b 10 /* enable output update only (without the controller); usefull for brushless motors */
-
-#define PXMS_ENI_m (1<<PXMS_ENI_b)
-#define PXMS_ENR_m (1<<PXMS_ENR_b)
-#define PXMS_ENG_m (1<<PXMS_ENG_b)
-#define PXMS_ERR_m (1<<PXMS_ERR_b)
-#define PXMS_BSY_m (1<<PXMS_BSY_b)
-#define PXMS_DBG_m (1<<PXMS_DBG_b)
-#define PXMS_CMV_m (1<<PXMS_CMV_b)
-#define PXMS_CQF_m (1<<PXMS_CQF_b)
-#define PXMS_PHA_m (1<<PXMS_PHA_b)
-#define PXMS_PTI_m (1<<PXMS_PTI_b)
-#define PXMS_ENO_m (1<<PXMS_ENO_b)
-
-/* cnfiguration flags stored in "pxms_cfg" field */
-
-#define PXMS_CFG_HDIR_b 3 /* initial direction */
-#define PXMS_CFG_HRI_b 4 /* use revolution index from HP HEDS */
-#define PXMS_CFG_HMC_b 5 /* find mark center */
-#define PXMS_CFG_HLS_b 6 /* use limit switch */
-#define PXMS_CFG_HPS_b 7 /* polarity of switch */
-#define PXMS_CFG_SMTH_b 8 /* smooth speed at changes and stop */
-#define PXMS_CFG_MD2E_b 10 /* difference abs(pxms_ap-pxms_rp)>pxms_md => error */
-#define PXMS_CFG_CYCL_b 11 /* axis is cyclic one => overflow is intended */
-
-#define PXMS_CFG_HSPD_m 7 /* initial speed of hardhome pxms_ms>>SSS */
-#define PXMS_CFG_HDIR_m (1<<PXMS_CFG_HDIR_b)
-#define PXMS_CFG_HRI_m (1<<PXMS_CFG_HRI_b)
-#define PXMS_CFG_HMC_m (1<<PXMS_CFG_HMC_b)
-#define PXMS_CFG_HLS_m (1<<PXMS_CFG_HLS_b)
-#define PXMS_CFG_HPS_m (1<<PXMS_CFG_HPS_b)
-#define PXMS_CFG_SMTH_m (1<<PXMS_CFG_SMTH_b)
-#define PXMS_CFG_MD2E_m (1<<PXMS_CFG_MD2E_b)
-#define PXMS_CFG_CYCL_m (1<<PXMS_CFG_CYCL_b)
-
-struct pxmc_state;
-
-/* these functions use special calling convention if PXMC_WITH_FAST_CALL deffined */
-typedef int pxmc_call_t(struct pxmc_state *mcs);
-
-/**
- * struct pxmc_state - Motor Controller State Information
- * @pxms_flg: Holds flag enabling encoder (%PXMS_ENI_m), controller
- * (%PXMS_ENR_m) and trajectory generator (%PXMS_ENG_m).
- * Other flags represent status information - busy (%PXMS_BSY_m),
- * error (%PXMS_ERR_m) and membership in coordinated group
- * (%PXMS_CMV_m). Flag %PXMS_PHA_m is used for initial phase
- * alignment. Flag %PXMS_DBG_m selects debugging for axis.
- * @pxms_do_inp: Pointer to function responsible for reading the encoder
- * and updating @pxms_ap and @pxms_as.
- * @pxms_do_con: Pointer to position controller which computes @pxms_ene
- * from axis state.
- * @pxms_do_out: Transfers computed @pxms_ene to PWM subsystem.
- * @pxms_do_deb: Debugging support routine.
- * @pxms_do_gen: Trajectory generator
- * @pxms_do_ap2hw: Preset new actual position into HW (is not mandatory)
- * @pxms_ap: Actual motor position multiplied by (1<<%PXMC_SUBDIV)
- * @pxms_as: Actual motor speed multiplied by (1<<%PXMC_SUBDIV)
- * @pxms_rp: Required motor position *(1<<%PXMC_SUBDIV)
- * @pxms_rpfg: Position extension for Fine Grained generator
- * @pxms_rs: Required motor speed *(1<<%PXMC_SUBDIV)
- * @pxms_rsfg: Speed extension for Fine Grained generator
- * @pxms_md: Maximal accepted position difference
- * @pxms_ms: Maximal speed in same units as @pxms_as and @pxms_rs
- * @pxms_ma: Maximal acceleration
- * @pxms_inp_info: Additional info for @pxms_do_inp to select which irc to use
- * @pxms_out_info: Additional info for @pxms_do_out where @pxms_ene should be sent
- * @pxms_ene: Computed output energy / value of PWM signal
- * @pxms_erc: Axis error counter
- * @pxms_p: Controller proportional constant
- * @pxms_i: Controller integration constant
- * @pxms_d: Controller derivative constant
- * @pxms_s1: Controller special 1 constant
- * @pxms_s2: Controller special 2 constant
- * @pxms_me: Maximal allowed output energy or PWM
- * @pxms_foi: Temporary for I computation
- * @pxms_fod: Temporary for D computation
- * @pxms_tmp: Temporary for help and debugging
- * @pxms_ptirc: IRC count per phase table
- * @pxms_ptper: Number of periods per table
- * @pxms_ptofs: Offset between table and IRC counter
- * @pxms_ptshift: Shift of generated phase curves
- * @pxms_ptvang: Angle (in irc) between rotor and stator mag. fld.
- * @pxms_ptindx: Index into commutation table
- * @pxms_ptptr1: Pointer to commutation table for phase 1
- * @pxms_ptptr2: Pointer to commutation table for phase 2
- * @pxms_ptptr3: Pointer to commutation table for phase 3
- * @pxms_ptamp: Amplitude of phase table profile (max value)
- * @pxms_pwm1cor: Correction for PWM1 generator
- * @pxms_pwm2cor: Correction for PWM2 generator
- * @pxms_pwm3cor: Correction for PWM3 generator
- * @pxms_errno: Error code
- * @pxms_cfg: Config information for axis
- * @pxms_ep: End position of movement
- * @pxms_gen_st: Status for generators
- * @pxms_gen_info: Field available for trajectory generators computations.
- * @pxms_hal: Last value read from HALL sensors
- * @pxms_halerc Error counter of hall errors
- *
- * This structure holds all state information for motion
- * control of one axis equipped with stepper or brush-less motor
- * with or without encoder feedback.
- */
-typedef struct pxmc_state {
- /* axis flags */
- short pxms_flg; /* controller and axis flags */
- /* sample time handling routines */
- pxmc_call_t *pxms_do_inp; /* update pxms_ap and pxms_as */
- pxmc_call_t *pxms_do_con; /* compute pxms_ene */
- pxmc_call_t *pxms_do_out; /* output pxms_ene */
- pxmc_call_t *pxms_do_deb; /* debugging support */
- pxmc_call_t *pxms_do_gen; /* position generator */
- /* helper routines */
- pxmc_call_t *pxms_do_ap2hw; /* set value of actual position to hw */
- /* axis actual state */
- long pxms_ap; /* actual motor position *(1<<PXMC_SUBDIV) */
- long pxms_as; /* actual motor speed *(1<<PXMC_SUBDIV) */
- /* controler requests */
- long pxms_rp; /* required motor position *(1<<pxms_SUBDIV) */
- #ifdef PXMC_WITH_FINE_GRAINED
- pxmc_fg_t pxms_rpfg; /* position extension for FG generator */
- #endif /*PXMC_WITH_FINE_GRAINED*/
- long pxms_rs; /* required motor speed *(1<<pxms_SUBDIV) */
- #ifdef PXMC_WITH_FINE_GRAINED
- pxmc_fg_t pxms_rsfg; /* speed extension for FG generator */
- #endif /*PXMC_WITH_FINE_GRAINED*/
- /* axis parameters */
- #ifndef PXMC_WITH_FIXED_SUBDIV
- short pxms_subdiv; /* subdivision of hardware unit */
- #endif /*PXMC_WITH_FIXED_SUBDIV*/
- long pxms_md; /* maximal pos. difference */
- long pxms_ms; /* maximal speed */
- long pxms_ma; /* maximal acceleration */
- long pxms_inp_info; /* which irc to use */
- long pxms_out_info; /* where put energy */
- /* controler outputs */
- short pxms_ene; /* output energy or PWM */
- short pxms_erc; /* error counter */
- /* basic parameters for pid */
- short pxms_p; /* proportional */
- short pxms_i; /* integration */
- short pxms_d; /* derivative */
- short pxms_s1; /* special 1 */
- short pxms_s2; /* special 2 */
- short pxms_me; /* maximal energy or PWM output */
- /* helpers for regulators */
- short pxms_foi; /* for I computation */
- short pxms_fod; /* for D computation */
- long pxms_tmp; /* for help and debugging */
- #ifdef PXMC_WITH_PHASE_TABLE
- /* informations for brushless motors */
- short pxms_ptirc; /* IRC count per phase table */
- short pxms_ptper; /* number of periods per table */
- short pxms_ptofs; /* offset between table and IRC counter */
- short pxms_ptshift; /* shift of generated phase curves */
- short pxms_ptvang; /* angle (in irc) between rotor and stator mag. fld.*/
- short pxms_ptindx; /* index into commutation table */
- short *pxms_ptptr1; /* pointer to commutation table for phase 1 */
- short *pxms_ptptr2; /* pointer to commutation table for phase 2 */
- short *pxms_ptptr3; /* pointer to commutation table for phase 3 */
- unsigned short pxms_ptamp; /* amplitude of phase table profile (max value) */
- /* correction constants for PWM outputs */
- short pxms_pwm1cor; /* correction for PWM1 generator */
- short pxms_pwm2cor; /* correction for PWM2 generator */
- short pxms_pwm3cor; /* correction for PWM3 generator */
- #endif /*PXMC_WITH_PHASE_TABLE*/
- #ifdef PXMC_WITH_CURRENTFB
- /* current feedback controller */
- long pxms_curfb_acum;/* accumulator for current measurement */
- long pxms_curfb_acur;/* accumulator for reactiv current */
- short pxms_curfb_act; /* actual meassured value of total current */
- short pxms_curfb_acr; /* actual meassured value of reactive current */
- short pxms_curfb_p; /* current feedback proportional constant */
- short pxms_curfb_i; /* current feedback integrative constant */
- short pxms_curfb_ir; /* current feedback integrative constant */
- long pxms_curfb_foi; /* storage of integration accumulation */
- short pxms_curfb_out; /* actual current controller output value */
- #endif /*PXMC_WITH_CURRENTFB*/
- /* error code */
- short pxms_errno; /* error code */
- /* informations for position generator */
- short pxms_cfg; /* config info for axis */
- long pxms_ep; /* end position of movement */
- short pxms_gen_st; /* status for generators */
- long pxms_gen_info[8];
- short pxms_hal; /* last value read from HALL sensors */
- short pxms_halerc; /* error counter of hall errors */
-} pxmc_state_t;
-
-/* evaluate offset of field from beginning of pxmc_state in bytes */
-#define pxmc_state_offs(_fld) \
- ((unsigned long)&((pxmc_state_t *)0L)->_fld)
-
-#define pxmc_set_errno(mcs,err) \
- { mcs->pxms_errno=err; pxmc_set_flag(mcs,PXMS_ERR_b); }
-
-#define pxmc_for_all_axes(i, pxms) for ((i)=0; (pxms)=pxmc_main_list.pxml_arr[(i)],(i)<pxmc_main_list.pxml_cnt; (i)++)
-
-#ifndef PXMC_FLAG_OPS_DEFINED
-#define PXMC_FLAG_OPS_DEFINED
-#ifndef PXMC_WITH_FLAGS_BYBITS_ONLY
-
-#define pxmc_clear_flags(mcs,mask) \
- atomic_clear_mask_w(mask,&(mcs->pxms_flg))
-
-#define pxmc_set_flags(mcs,mask) \
- atomic_set_mask_w(mask,&(mcs->pxms_flg))
-
-#define pxmc_clear_flag(mcs,nr) \
- atomic_clear_mask_w(1<<(nr),&(mcs->pxms_flg))
-
-#define pxmc_set_flag(mcs,nr) \
- atomic_set_mask_w(1<<(nr),&(mcs->pxms_flg))
-
-#else /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
-
-#define pxmc_clear_flag(mcs,nr) \
- clear_bit(nr,&((mcs)->pxms_flg))
-
-#define pxmc_set_flag(mcs,nr) \
- set_bit(nr,&((mcs)->pxms_flg))
-
-#endif /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
-#endif /*PXMC_FLAG_OPS_DEFINED*/
-
-#define PXMS_E_COMM 0x105 /* 261 - offset of commutation error */
-#define PXMS_E_MAXPD 0x106 /* 262 - difference of position over limit */
-#define PXMS_E_OVERL 0x107 /* 263 - overload */
-#define PXMS_E_HAL 0x108 /* 264 - hal problem */
-#define PXMS_E_POWER_STAGE 0x109/* 265 - power stage fault signal */
-
-#ifdef PXMC_WITH_FAST_CALL
-/* call routine with pxmc_fast calling convention */
-extern int pxmc_fast_call(pxmc_state_t *mcs, int fnc(pxmc_state_t *mcs));
-#define pxmc_call(_st,_fnc) pxmc_fast_call((_st),(_fnc))
-#else /*PXMC_WITH_FAST_CALL*/
-#define pxmc_call(_st,_fnc) ((_fnc)(_st))
-#endif /*PXMC_WITH_FAST_CALL*/
-
-
-/* List of controller states */
-
-typedef struct pxmc_state_list{
- pxmc_state_t **pxml_arr;
- short pxml_cnt;
-}pxmc_state_list_t;
-
-/* routines for axis/controller state manipulation and examination */
-
-/* set direct constant output */
-void pxmc_set_const_out(pxmc_state_t *mcs,int val);
-
-/* smooth connection of controler to axis */
-int pxmc_connect_controller(pxmc_state_t *mcs);
-
-/* prepares for change of generator */
-int pxmc_set_gen_prep(pxmc_state_t *mcs);
-
-/* sets generator, check non-zero speed and provide smooth stop
- before requested function, must pxmc_set_gen_prep be called before */
-int pxmc_set_gen_smth(pxmc_state_t *mcs, pxmc_call_t *do_gen_gi, int flg);
-
-/* start of trapezoidal speed profile motion to the position*/
-int pxmc_go(pxmc_state_t *mcs, long val, int res, int mode);
-#define PXMC_GO_ABS 0 /* absolute mode */
-#define PXMC_GO_REL 1 /* relative mode to RP */
-
-/* constant speed generator */
-int pxmc_spd(pxmc_state_t *mcs, long val, int timeout);
-
-#ifdef PXMC_WITH_FINE_GRAINED
-/* start of trapezoidal speed profile motion
- to requested the position with set up finegrained speed */
-int pxmc_go_spdfg(pxmc_state_t *mcs, long endpos, long speed, int mode);
-
-/* constant fine grained speed generator */
-int pxmc_spdfg(pxmc_state_t *mcs, long val, int timeout);
-#endif /*PXMC_WITH_FINE_GRAINED*/
-
-/* stop motion */
-int pxmc_stop(pxmc_state_t *mcs, int mode);
-
-/* set new actual and requested position */
-int pxmc_axis_set_pos(pxmc_state_t *mcs, long pos);
-
-/* tuning and development support */
-typedef struct pxmc_dbg_hist{
- long *ptr;
- long *buff;
- long *end;
-} pxmc_dbg_hist_t;
-
-pxmc_dbg_hist_t *pxmc_dbg_hist;
-
-#ifdef PXMC_WITH_DBG_HIST
-
-int pxmc_dbg_histfree(pxmc_dbg_hist_t *hist);
-pxmc_dbg_hist_t *pxmc_dbg_histalloc(int count);
-
-int pxmc_dbgset(pxmc_state_t *mcs, pxmc_call_t *do_deb, int dbgflg);
-
-int pxmc_dbg_gnr(pxmc_state_t *mcs);
-
-int pxmc_dbg_ene_as(pxmc_state_t *mcs);
-
-#endif /*PXMC_WITH_DBG_HIST*/
-
-/* Hard mome finding */
-int pxmc_hh(pxmc_state_t *mcs);
-
-/* Reads sampling frequency of axis */
-long pxmc_get_sfi_hz(pxmc_state_t *mcs);
-
-/* Setting sampling frequency of PXMC subsystem */
-long pxmc_sfi_sel(long sfi_hz);
-
-/* select operating mode of exis */
-int pxmc_axis_mode(pxmc_state_t *mcs, int mode);
-
-/* read mode number for axis */
-int pxmc_axis_rdmode(pxmc_state_t *mcs);
-
-/*fce for adding seznam of motors from Menu.c (where init charakters motors) to this file*/
-void pxmc_pass_motor_struct(pxmc_state_list_t *ArrayStructurMotors);
-
-/* The usage specific functions for different motors kinds */
-void pxmc_dcm_init_fbmode(pxmc_state_t *mcs);
-
-/* pxmc_go central flags enabling retargeting of the unfinished movement
- * and style of computation of the end position */
-int pxmc_go_flg;
-
-/* Controlers for main sample frequency */
-extern pxmc_state_list_t pxmc_main_list;
-
-/* initialize pxmc_main_list */
-int pxmc_initialize(void);
-
-/* main controller routine called from sampling frequency irq */
-void do_pxm_control(void);
-
-/* coordinated movement generator called from sampling frequency irq */
-void do_pxmc_coordmv(void);
-
-#endif /* _PXMC_H_ */
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc_base.c - generic multi axis motion controller
- version supports feedback and feedback-less stepper
- motor control, DC and brush-less motors
-
- (C) 2001-2005 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (C) 2002-2005 by PiKRON Ltd. http://www.pikron.com
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
-
- *******************************************************************/
-
-/* [extern API] characteristic means that the function is declared
-in the header file pxmc.h so it is part of the external API */
-
-#include <types.h>
-#include <cpu_def.h>
-#include <system_def.h>
-#include <string.h>
-#include "pxmc.h"
-#include "pxmc_internal.h"
-#include "pxmc_gen_info.h"
-
-/********************************************************************/
-/* controllers general interface */
-
-/**
- * pxmc_set_const_out - Sets direct constant output.[extern API]
- * @mcs: Motion controller state information
- * @val: New value of energy/PWM output limited
- * by @pxms_me.
- */
-void pxmc_set_const_out(pxmc_state_t *mcs,int val)
-{
- if(mcs->pxms_flg&PXMS_CMV_m){
- /* more inteligence should come here */
- pxmc_clear_flag(mcs,PXMS_CMV_b);
- }
- #ifndef PXMC_WITH_FLAGS_BYBITS_ONLY
- pxmc_clear_flags(mcs,PXMS_ENR_m|PXMS_ENG_m|PXMS_BSY_m);
- pxmc_set_flags(mcs,PXMS_ENO_m);
- #else /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
- pxmc_clear_flag(mcs,PXMS_ENG_b);
- pxmc_clear_flag(mcs,PXMS_ENR_b);
- pxmc_clear_flag(mcs,PXMS_BSY_b);
- pxmc_set_flag(mcs,PXMS_ENO_b);
- #endif /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
- if(val>mcs->pxms_me) val=mcs->pxms_me;
- if(val<-mcs->pxms_me) val=-mcs->pxms_me;
- mcs->pxms_ene=val;
- pxmc_call(mcs,mcs->pxms_do_out);
-}
-
-
-/**
- * pxmc_connect_controller_prep - Preparation of connection of controler to axis.
- * @mcs: Motion controller state information
- */
-int pxmc_connect_controller_prep(pxmc_state_t *mcs)
-{
- mcs->pxms_rp=mcs->pxms_ap;
- mcs->pxms_rs=mcs->pxms_foi=mcs->pxms_fod=mcs->pxms_erc=0;
- #ifdef PXMC_WITH_CURRENTFB
- mcs->pxms_curfb_foi=0;
- #endif /*PXMC_WITH_CURRENTFB*/
-
- return 0;
-}
-
-/**
- * pxmc_connect_controller - Smooth connection of controler to axis.[extern API]
- * @mcs: Motion controller state information
- */
-int pxmc_connect_controller(pxmc_state_t *mcs)
-{
- if(mcs->pxms_flg&PXMS_ERR_m) return -1;
- if(mcs->pxms_flg&PXMS_ENR_m) return 0;
-
- /* smooth connection of controler to axis */
- if(pxmc_connect_controller_prep(mcs)<0)
- return -1;
-
- __memory_barrier();
- #ifndef PXMC_WITH_FLAGS_BYBITS_ONLY
- pxmc_set_flags(mcs,PXMS_ENI_m|PXMS_ENR_m);
- #else /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
- pxmc_set_flag(mcs,PXMS_ENI_b);
- pxmc_set_flag(mcs,PXMS_ENR_b);
- #endif /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
- return 0;
-}
-
-/**
- * pxmc_set_gen_prep - Prepares axis for change of generator.[extern API]
- * @mcs: Motion controller state information
- */
-int pxmc_set_gen_prep(pxmc_state_t *mcs)
-{
- if(mcs->pxms_flg&PXMS_ERR_m)
- return -1;
- if(mcs->pxms_flg&PXMS_CMV_m)
- return -1;
- mcs->pxms_do_gen=pxmc_get_cont_gi_4axis(mcs);
- __memory_barrier();
-
- if(mcs->pxms_flg&PXMS_ERR_m) return -1;
- if(mcs->pxms_flg&PXMS_ENR_m) return 0;
-
- /* smooth connection of controler to axis */
- return pxmc_connect_controller_prep(mcs);
-}
-
-/**
- * pxmc_set_gen_smth - Smooth change to new generator.[extern API]
- * @mcs: Motion controller state information
- * @do_gen_gi: Initial state of next generator
- * @flg: Additional flags to set (%PXMS_BSY_m)
- *
- * Checks non-zero speed and provide smooth stop,
- * than change to requested generator.
- */
-int pxmc_set_gen_smth(pxmc_state_t *mcs, pxmc_call_t *do_gen_gi, int flg)
-{
- __memory_barrier();
- #ifndef PXMC_WITH_FLAGS_BYBITS_ONLY
- pxmc_set_flags(mcs,PXMS_ENI_m|PXMS_ENR_m|PXMS_ENG_m|flg);
- #else /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
- pxmc_set_flag(mcs,PXMS_ENI_b);
- pxmc_set_flag(mcs,PXMS_ENR_b);
- pxmc_set_flag(mcs,PXMS_ENG_b);
- if(flg&PXMS_BSY_m)
- pxmc_set_flag(mcs,PXMS_BSY_b);
- /* more flags may follow */
- #endif /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
-
- if((mcs->pxms_rs==0)||!(mcs->pxms_cfg&PXMS_CFG_SMTH_m)){
- __memory_barrier();
- mcs->pxms_do_gen=do_gen_gi;
- }else{
- mcs->pxms_gen_spd_next=(long)do_gen_gi; /* next generator */
- __memory_barrier();
- mcs->pxms_do_gen=pxmc_get_spdnext_gi_4axis(mcs);
- }
- return 0;
-}
-
-int pxmc_go_flg=0; /* pxmc_go central flags */
-
-/**
- * pxmc_go - Starts movement to requested position.[extern API]
- * @mcs: Motion controller state information
- * @val: Requested target position
- * @res: Left for future use
- * @mode: Motion mode flags.
- *
- * Starts trapezoidal speed profile motion to the position
- * @val.
- */
-int pxmc_go(pxmc_state_t *mcs, long val, int res, int mode)
-{
- pxmc_call_t *do_gen_init=pxmc_get_trp_gi_4axis(mcs, mode);
-
- if(!do_gen_init)
- return -1;
-
- /* Try to re-target to the new final position if previous command is still processed */
- if((mode!=PXMC_GO_REL)&&(pxmc_go_flg&1)&&(mcs->pxms_flg&PXMS_BSY_m)){
- pxmc_call_t *do_trp_retgt=pxmc_get_trp_retgt_4axis(mcs);
-
- if(do_trp_retgt){
- mcs->pxms_ep=val;
- if(pxmc_call(mcs,do_trp_retgt)>=0)
- return 0;
- }
- }
-
- if(pxmc_set_gen_prep(mcs)<0) return -1;
-
- if(mode==PXMC_GO_REL){
- if(pxmc_go_flg&2)
- mcs->pxms_ep=mcs->pxms_ep+val;
- else
- mcs->pxms_ep=mcs->pxms_rp+val;
- }else{
- mcs->pxms_ep=val;
- }
-
- pxmc_set_gen_smth(mcs,do_gen_init,PXMS_BSY_m);
- return 0;
-}
-
-#ifdef PXMC_WITH_FINE_GRAINED
-/**
- * pxmc_go_spdfg - Starts movement to position with selected speed.[extern API]
- * @mcs: Motion controller state information
- * @endpos: Requested target position
- * @speed: Fine-grained maximal speed of movement
- * @mode: Motion mode flags
- *
- * Starts trapezoidal speed profile motion
- * to requested end position with set up fine-grained speed
- */
-int pxmc_go_spdfg(pxmc_state_t *mcs, long endpos, long speed, int mode)
-{
- pxmc_call_t *do_gen_init=pxmc_get_trp_spdfg_gi_4axis(mcs);
-
- if(!do_gen_init)
- return -1;
-
- if(speed<=0)
- return -1;
- #ifndef PXMC_SPDFG_ONLY
- if((speed>>PXMC_SUBDIVFG(mcs))>mcs->pxms_ms)
- return -1;
- #endif
- if(pxmc_set_gen_prep(mcs)<0) return -1;
-
- if(mode&1){
- mcs->pxms_ep=mcs->pxms_rp+endpos;
- }else{
- mcs->pxms_ep=endpos;
- }
- #ifdef PXMC_SPDFG_ONLY
- mcs->pxms_gen_tsp=0;
- #else
- mcs->pxms_gen_tsp=speed>>PXMC_SUBDIVFG(mcs);
- #endif
- mcs->pxms_gen_tspfg=speed;
-
- pxmc_set_gen_smth(mcs,do_gen_init,PXMS_BSY_m);
- return 0;
-}
-#endif /*PXMC_WITH_FINE_GRAINED*/
-
-/**
- * pxmc_stop - Stops motion of axis.[extern API]
- * @mcs: Motion controller state information
- * @mode: Value 1 means emergency immediate stop,
- * else smooth stop is proceeded.
- */
-int pxmc_stop(pxmc_state_t *mcs, int mode)
-{
- if(mcs->pxms_flg&PXMS_CMV_m){
- /* more inteligence should come here */
- if((mcs->pxms_flg&PXMS_ENR_m)&&!(mcs->pxms_flg&PXMS_ENG_m)
- &&!(mcs->pxms_flg&PXMS_ERR_m)){
- mcs->pxms_do_gen=pxmc_get_cont_gi_4axis(mcs);
- __memory_barrier();
- pxmc_set_flag(mcs,PXMS_ENG_b);
- }
- pxmc_clear_flag(mcs,PXMS_CMV_b);
- }
-
- if(pxmc_set_gen_prep(mcs)<0){
- #ifndef PXMC_WITH_FLAGS_BYBITS_ONLY
- pxmc_clear_flags(mcs,PXMS_ENG_m|PXMS_BSY_m);
- #else /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
- pxmc_clear_flag(mcs,PXMS_ENG_b);
- pxmc_clear_flag(mcs,PXMS_BSY_b);
- #endif /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
- mcs->pxms_rs=0;
- return -1;
- }
-
- if((mcs->pxms_rs==0)||!(mcs->pxms_cfg&PXMS_CFG_SMTH_m)||
- !(mcs->pxms_flg&PXMS_ENR_m)||mode){
- #ifndef PXMC_WITH_FLAGS_BYBITS_ONLY
- pxmc_clear_flags(mcs,PXMS_ENG_m|PXMS_BSY_m);
- #else /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
- pxmc_clear_flag(mcs,PXMS_ENG_b);
- pxmc_clear_flag(mcs,PXMS_BSY_b);
- #endif /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
- mcs->pxms_rs=0;
- }else{
- mcs->pxms_gen_spd_next=0; /* no next generator */
- __memory_barrier();
- mcs->pxms_do_gen=pxmc_get_spdnext_gi_4axis(mcs);
- }
- return 0;
-}
-
-/**
- * pxmc_spd - Starts constant speed motion.[extern API]
- * @mcs: Motion controller state information
- * @val: Requested speed
- * @timeout: Non zero value results in smooth stop after
- * @timeout sampling periods
- */
-int pxmc_spd(pxmc_state_t *mcs, long val, int timeout)
-{
- pxmc_call_t *do_gen_init=pxmc_get_spd_gi_4axis(mcs);
-
- if(!do_gen_init)
- return -1;
-
- if(pxmc_set_gen_prep(mcs)<0) return -1;
-
- mcs->pxms_gen_st=0;
- mcs->pxms_gen_spd_ac=mcs->pxms_ma; /* acceleration */
- mcs->pxms_gen_spd_sp=val; /* speed */
- #ifdef PXMC_WITH_FINE_GRAINED
- mcs->pxms_gen_spd_spfg=0; /* fine grained speed */
- #endif /*PXMC_WITH_FINE_GRAINED*/
- mcs->pxms_gen_spd_timeout=timeout; /* timeout */
-
- #ifndef PXMC_WITH_FLAGS_BYBITS_ONLY
- pxmc_set_flags(mcs,PXMS_ENI_m|PXMS_ENR_m|PXMS_ENG_m|PXMS_BSY_m);
- #else /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
- pxmc_set_flag(mcs,PXMS_BSY_b);
- pxmc_set_flag(mcs,PXMS_ENI_b);
- pxmc_set_flag(mcs,PXMS_ENR_b);
- pxmc_set_flag(mcs,PXMS_ENG_b);
- #endif /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
- __memory_barrier();
-
- mcs->pxms_do_gen=do_gen_init;
-
- return 0;
-}
-
-#ifdef PXMC_WITH_FINE_GRAINED
-/**
- * pxmc_spdfg - Starts constant fine-grained speed motion.[extern API]
- * @mcs: Motion controller state information
- * @val: Requested fine-grained speed
- * @timeout: Non zero value results in smooth stop after
- * @timeout sampling periods
- */
-int pxmc_spdfg(pxmc_state_t *mcs, long val, int timeout)
-{
- pxmc_call_t *do_gen_init=pxmc_get_spdfg_gi_4axis(mcs);
-
- if(!do_gen_init)
- return -1;
-
- if(pxmc_set_gen_prep(mcs)<0) return -1;
-
- mcs->pxms_gen_st=0;
- mcs->pxms_gen_spd_ac=mcs->pxms_ma; /* acceleration */
- mcs->pxms_gen_spd_spfg=val; /* fine grained speed */
- #ifndef PXMC_SPDFG_ONLY
- mcs->pxms_gen_spd_sp=val>>PXMC_SUBDIVFG(mcs); /* speed */
- #else
- mcs->pxms_rs=val<0? -1: 0;
- #endif
-
- mcs->pxms_gen_spd_timeout=timeout; /* timeout */
-
- #ifndef PXMC_WITH_FLAGS_BYBITS_ONLY
- pxmc_set_flags(mcs,PXMS_ENI_m|PXMS_ENR_m|PXMS_ENG_m|PXMS_BSY_m);
- #else /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
- pxmc_set_flag(mcs,PXMS_BSY_b);
- pxmc_set_flag(mcs,PXMS_ENI_b);
- pxmc_set_flag(mcs,PXMS_ENR_b);
- pxmc_set_flag(mcs,PXMS_ENG_b);
- #endif /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
- __memory_barrier();
-
- mcs->pxms_do_gen=do_gen_init;
-
- return 0;
-}
-#endif /*PXMC_WITH_FINE_GRAINED*/
-
-/**
- * pxmc_axis_set_pos - Set axis actual position
- * @mcs: Motion controller state information
- * @pos: New forced position for IRC counter
- * in the standard subdiv. format
- */
-int pxmc_axis_set_pos(pxmc_state_t *mcs, long pos)
-{
- int old;
-
- if(mcs->pxms_flg&(PXMS_ENR_m|PXMS_ENG_m|PXMS_BSY_m|PXMS_CMV_m)){
- /* Cannot set new position value when busy */
- return -1;
- }
- if(!mcs->pxms_do_ap2hw){
- return -1;
- }
- old=mcs->pxms_flg&PXMS_ENI_m;
- pxmc_clear_flag(mcs,PXMS_ENI_b);
- mcs->pxms_ap=pos;
- mcs->pxms_rp=pos;
- pxmc_call(mcs, mcs->pxms_do_ap2hw);
- if(old)
- pxmc_set_flag(mcs,PXMS_ENI_b);
- return 0;
-}
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc_con_pid.c - generic multi axis motion controller
- basic PID controller
-
- (C) 2001-2005 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (C) 2002-2005 by PiKRON Ltd. http://www.pikron.com
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
-
- *******************************************************************/
-
-#include <types.h>
-#include <cpu_def.h>
-#include <system_def.h>
-#include <string.h>
-#include "pxmc.h"
-#include "pxmc_internal.h"
-
-/**
- * pxmc_pid_con - Position PID Controller
- * @mcs: Motion controller state information
- */
-int
-pxmc_pid_con(pxmc_state_t *mcs)
-{
- long dp, ds, pd, i;
- short ene;
- dp=mcs->pxms_rp-mcs->pxms_ap;
- ds=mcs->pxms_rs-mcs->pxms_as;
- if((dp>mcs->pxms_md)||(dp<-mcs->pxms_md)) {
- if(mcs->pxms_cfg&PXMS_CFG_MD2E_m) {
- pxmc_set_errno(mcs,PXMS_E_MAXPD);
- return 0;
- }
- if(dp>0) dp=mcs->pxms_md;
- else dp=-mcs->pxms_md;
- }
- dp>>=PXMC_SUBDIV(mcs);
- ds>>=PXMC_SUBDIV(mcs);
-
- pd=(short)dp*(long)mcs->pxms_p+(short)ds*(long)mcs->pxms_d;
- if(pd>0) {
- if(pd>mcs->pxms_me) pd=mcs->pxms_me;
- i=((short)pd*(long)mcs->pxms_i)>>12;
- i+=mcs->pxms_foi;
- if(i>mcs->pxms_me) i=mcs->pxms_me;
- mcs->pxms_foi=i;
- ene=pd+i;
- } else if(pd<0) {
- if(pd<-mcs->pxms_me) pd=-mcs->pxms_me;
- i=((short)pd*(long)mcs->pxms_i)>>12;
- i+=mcs->pxms_foi;
- if(i<-mcs->pxms_me) i=-mcs->pxms_me;
- mcs->pxms_foi=i;
- ene=pd+i;
- } else {
-
- ene=mcs->pxms_foi;
- }
-
- if(ene>mcs->pxms_me) {
- ene=mcs->pxms_me;
- if(!mcs->pxms_as)
- if((mcs->pxms_erc+=0x80)<0) {
- pxmc_set_errno(mcs,PXMS_E_OVERL);
- ene=0;
- }
- } else if(ene<-mcs->pxms_me) {
- ene=-mcs->pxms_me;
- if(!mcs->pxms_as)
- if((mcs->pxms_erc+=0x80)<0) {
- pxmc_set_errno(mcs,PXMS_E_OVERL);
- ene=0;
- }
- } else { mcs->pxms_erc=0; }
-
- mcs->pxms_ene=ene;
-
- return 0;
-}
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc_deb.c - generic multi axis motion controller
- debugging support
-
- (C) 2001-2005 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (C) 2002-2005 by PiKRON Ltd. http://www.pikron.com
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
-
- *******************************************************************/
-
-/* [extern API] characteristic means that the function is declared
-in the header file pxmc.h so it is part of the external API */
-
-#include <types.h>
-#include <cpu_def.h>
-#include <system_def.h>
-#include <malloc.h>
-#include <string.h>
-#include "pxmc.h"
-#include "pxmc_internal.h"
-
-pxmc_dbg_hist_t *pxmc_dbg_hist=NULL;
-
-/**
- * pxmc_dbg_histfree - Frees motion history buffer
- * @hist: Motion history buffer
- */
-int pxmc_dbg_histfree(pxmc_dbg_hist_t *hist)
-{
- if(hist==NULL){
- hist=pxmc_dbg_hist;
- pxmc_dbg_hist=NULL;
- }
- if(hist==NULL) return 0;
- if(hist->buff!=NULL) free(hist->buff);
- free(hist);
- return 0;
-}
-
-/**
- * pxmc_dbg_histalloc - Allocates new motion history buffer
- * @count: Number of allocated slots in motion history buffer
- */
-pxmc_dbg_hist_t *pxmc_dbg_histalloc(int count)
-{
- pxmc_dbg_hist_t *hist;
- hist=malloc(sizeof(pxmc_dbg_hist_t));
- if(hist==NULL) return NULL;
- if((hist->buff=malloc(sizeof(hist->buff[0])*count))==NULL)
- {
- free(hist);
- return 0;
- }
- memset(hist->buff,0,sizeof(hist->buff[0])*count);
- hist->end=hist->buff+count;
- hist->ptr=NULL;
- return hist;
-}
-
-/**
- * pxmc_dbg_ene_as - Stores actual speed and output energy
- * @mcs: Motion controller state information
- */
-int pxmc_dbg_ene_as(pxmc_state_t *mcs)
-{
- long *ptr;
- if(pxmc_dbg_hist){
- ptr=pxmc_dbg_hist->ptr;
- if(ptr&&(ptr<pxmc_dbg_hist->end-1)){
- *(ptr++)=mcs->pxms_as>>PXMC_SUBDIV(mcs);
- *(ptr++)=mcs->pxms_ene;
- pxmc_dbg_hist->ptr=ptr;
- }
- }
- return 0;
-}
-
-int pxmc_dbg_gnr_gi(pxmc_state_t *mcs)
-{
- long *ptr;
- if((mcs->pxms_flg&PXMS_DBG_m)&&!(mcs->pxms_flg&PXMS_ERR_m)
- &&pxmc_dbg_hist){
- ptr=pxmc_dbg_hist->ptr;
- if(!ptr){
- mcs->pxms_rs=0;
- return 0;
- }
- if(ptr<pxmc_dbg_hist->end-1){
- mcs->pxms_rp+=mcs->pxms_rs=ptr[1]<<PXMC_SUBDIV(mcs);
- return 0;
- }
- }
- mcs->pxms_rp+=mcs->pxms_rs;
- mcs->pxms_do_gen=pxmc_get_stop_gi_4axis(mcs);
- return 0;
-}
-
-/**
- * pxmc_dbg_gnr - Generator of speed profile stored in history buffer
- * @mcs: Motion controller state information
- */
-int pxmc_dbg_gnr(pxmc_state_t *mcs)
-{
- if(!pxmc_dbg_hist||!(mcs->pxms_flg&PXMS_DBG_m)) return -1;
- if(pxmc_set_gen_prep(mcs)<0) return -1;
- pxmc_set_gen_smth(mcs,pxmc_dbg_gnr_gi,PXMS_BSY_m);
- return 0;
-}
-
-/**
- * pxmc_dbgset - Selects debugging options for axis
- * @mcs: Motion controller state information
- * @do_deb: Debugging callback function
- * @dbgflg: 0 .. disables debugging, 1 .. enables debugging
- * for axis
- */
-int pxmc_dbgset(pxmc_state_t *mcs, pxmc_call_t *do_deb, int dbgflg)
-{
- if(dbgflg>0){
- __memory_barrier();
- if(!do_deb) {
- if(!mcs->pxms_do_deb)
- mcs->pxms_do_deb=pxmc_dbg_ene_as;
- } else mcs->pxms_do_deb=do_deb;
- __memory_barrier();
- pxmc_set_flag(mcs,PXMS_DBG_b);
- }else{
- pxmc_clear_flag(mcs,PXMS_DBG_b);
- }
- return 0;
-}
-
-
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc_gen_info.h - generic multi axis motion controller
- information fields allocation for generators
-
- (C) 2001-2005 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (C) 2002-2005 by PiKRON Ltd. http://www.pikron.com
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
-
- *******************************************************************/
-
-#ifndef _PXMC_GEN_INFO_H_
-#define _PXMC_GEN_INFO_H_
-
-/* the fields allocation for :
- * - pxmc_trp_gi
- */
-
-#define pxms_gen_tep pxms_gen_info[0] /* copied opxms_ep */
-#define pxms_gen_tac pxms_gen_info[1] /* copied opxms_ma */
-#define pxms_gen_tsp pxms_gen_info[2] /* copied opxms_ms */
-#define pxms_gen_tspfg pxms_gen_info[3] /* fine grained speed */
-#define pxms_gen_ttp pxms_gen_info[4] /* triggering position */
-#define pxms_gen_ttpfr pxms_gen_info[5] /* fractions */
-
-/* the fields allocation for :
- * - pxmc_stop_gi
- * - pxmc_spd_gi
- * - pxmc_spdnext_gi
- */
-
-#define pxms_gen_spd_next pxms_gen_info[0] /* next generator */
-#define pxms_gen_spd_ac pxms_gen_info[1] /* copied opxms_ma */
-#define pxms_gen_spd_sp pxms_gen_info[2] /* final speed */
-#define pxms_gen_spd_spfg pxms_gen_info[3] /* fine grained speed */
-#define pxms_gen_spd_timeout pxms_gen_info[4] /* timeout */
-
-/* the fields allocation for :
- * - pxmc_hh_gi
- * - pxmc_fast_hh_gi
- */
-
-#define pxms_gen_htim pxms_gen_info[0] /* timer */
-#define pxms_gen_hcfg pxms_gen_info[1] /* copied opxms_cfg */
-#define pxms_gen_hac pxms_gen_info[2] /* copied opxms_ma */
-#define pxms_gen_hsp pxms_gen_info[3] /* copied opxms_ms */
-
-
-#endif /*_PXMC_GEN_INFO_H_*/
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc_gen_spdtrp.c - generic multi axis motion controller
- generator for trapezoid and simple speed movement
-
- (C) 2001-2005 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (C) 2002-2005 by PiKRON Ltd. http://www.pikron.com
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
-
- *******************************************************************/
-
-#include <types.h>
-#include <cpu_def.h>
-#include <system_def.h>
-#include <string.h>
-#include "pxmc.h"
-#include "pxmc_internal.h"
-#include "pxmc_gen_info.h"
-
-/********************************************************************/
-/* Architecture optimized functions */
-
-#ifdef PXMC_WITH_FINE_GRAINED
-
-#if defined(__H8300__) || defined(H8300)
-/**
- * pxmc_add_cspdfg - Adds fine-grained speed to requested position
- * @mcs: Motion controller state information
- *
- * Adds fine-grained speed (@pxms_rs,@pxms_rsfg) to requested position
- * (@pxms_rp,@pxms_rpfg). Written is asm.
- */
-static inline void
-pxmc_add_cspdfg(pxmc_state_t *mcs)
-{
- asm (
- " add.w %3,%1\n"
- " bcc 1f\n"
- " adds #1,%0\n"
- "1: add.l %2,%0\n"
- : "=r" (mcs->pxms_rp),"=r" (mcs->pxms_rpfg)
- : "r" (mcs->pxms_rs),"r" (mcs->pxms_rsfg),
- "0" (mcs->pxms_rp),"1" (mcs->pxms_rpfg)
- : "cc"
- );
-}
-
-#elif defined(__MSP430) || defined(MSP430)
-
-static inline void
-pxmc_add_cspdfg(pxmc_state_t *mcs)
-{
- asm (
- " add.w %A3,%A1 /*pxmc_add_cspdfg*/\n"
- #if PXMC_SUBDIVFG(0)>16
- " addc.w %B3,%B1\n"
- #endif
- " addc.w %A2,%A0\n"
- " addc.w %B2,%B0\n"
- : "=g" (mcs->pxms_rp),"=g" (mcs->pxms_rpfg)
- : "g" (mcs->pxms_rs),"g" (mcs->pxms_rsfg),
- "0" (mcs->pxms_rp),"1" (mcs->pxms_rpfg)
- : "cc"
- );
-}
-
-#else
-#error pxmc_add_cspdfg not defined for this architecture
-#endif
-
-#else /*PXMC_WITH_FINE_GRAINED*/
-
-static inline void
-pxmc_add_cspdfg(pxmc_state_t *mcs)
-{
- mcs->pxms_rp+=mcs->pxms_rs;
-}
-
-#endif /*PXMC_WITH_FINE_GRAINED*/
-
-/********************************************************************/
-/* Speed related functions */
-
-/**
- * pxmc_add_vspd - Adds variable speed to requested position
- * @mcs: Motion controller state information
- * @spd: New requested speed
- *
- * Sets requested speed (@pxms_rs) and adds it to requested
- * position (@pxms_rp).
- */
-static inline void
-pxmc_add_vspd(pxmc_state_t *mcs, long spd)
-{
- mcs->pxms_rp+=mcs->pxms_rs=spd;
-}
-
-/**
- * pxmc_set_spd - Sets new immediate value of speed
- * @mcs: Motion controller state information
- * @spd: New requested speed
- *
- * Sets requested speed (@pxms_rs) and clears (@pxms_rsfg)
- */
-static inline void
-pxmc_set_spd(pxmc_state_t *mcs, long spd)
-{
- mcs->pxms_rs=spd;
- #ifdef PXMC_WITH_FINE_GRAINED
- mcs->pxms_rsfg=0;
- #endif /*PXMC_WITH_FINE_GRAINED*/
-}
-
-#ifdef PXMC_WITH_FINE_GRAINED
-/**
- * pxmc_set_spdfg - Sets new immediate value of fine-grained speed
- * @mcs: Motion controller state information
- * @spd: New requested fine-grained speed
- *
- * Sets requested speed (@pxms_rs) and clears (@pxms_rsfg)
- */
-static inline void
-pxmc_set_spdfg(pxmc_state_t *mcs, long spd)
-{
- mcs->pxms_rsfg=spd;
- #ifdef PXMC_SPDFG_ONLY
- mcs->pxms_rs=spd<0? -1: 0;
- #else
- mcs->pxms_rs=spd>>PXMC_SUBDIVFG(mcs);
- #endif
-}
-
-/**
- * pxmc_spdfg_gnr - Constant speed fine-grained generator
- * @mcs: Motion controller state information
- */
-int
-pxmc_spdfg_gnr(pxmc_state_t *mcs)
-{
- pxmc_add_cspdfg(mcs);
- return 0;
-}
-#endif /*PXMC_WITH_FINE_GRAINED*/
-
-/********************************************************************/
-/* generators */
-
-/**
- * pxmc_spd_gacc - Smooth transition to requested speed
- * @spd: Pointer to controlled speed
- * @rspd: Value of requested final speed
- * @acc: Allowed acceleration
- *
- * This function modifies @spd by @acc increments until
- * required value @rspd is reached. It returns 0 when
- * speed is reached, else returns 1.
- */
-int
-pxmc_spd_gacc(long *spd,long rspd,long acc)
-{
- if(rspd==*spd) return 0;
- if(rspd>*spd){
- long nspd=*spd;
- /*sat_add_slsl(nspd,acc);*/
- nspd+=acc;
- *spd=(rspd>nspd)?nspd:rspd;
- return 1;
- }else{
- long nspd=*spd;
- /*sat_sub_slsl(nspd,acc);*/
- nspd-=acc;
- *spd=(rspd<nspd)?nspd:rspd;
- return 1;
- }
-}
-
-/**
- * pxmc_nop_gd - No-operation generator state
- * @mcs: Motion controller state information
- */
-int
-pxmc_nop_gd(pxmc_state_t *mcs)
-{
- return 0;
-}
-
-/**
- * pxmc_cont_gi - Initializes continuation generator
- * @mcs: Motion controller state information
- *
- * This type of generator is temporarily used
- * when new generator parameters are computed.
- */
-int
-pxmc_cont_gi(pxmc_state_t *mcs)
-{
- if(mcs->pxms_flg&PXMS_ERR_m){
- pxmc_clear_flag(mcs,PXMS_ENG_b);
- pxmc_clear_flag(mcs,PXMS_BSY_b);
- return 0;
- }
- pxmc_add_cspdfg(mcs);
- return 0;
-}
-
-/*------------------------------------------------------------------*/
-
-static int pxmc_trp_gdu10(pxmc_state_t *mcs);
-static int pxmc_trp_gdu20(pxmc_state_t *mcs);
-static int pxmc_trp_gdu30(pxmc_state_t *mcs);
-static int pxmc_trp_gdd10(pxmc_state_t *mcs);
-static int pxmc_trp_gdd20(pxmc_state_t *mcs);
-static int pxmc_trp_gdd30(pxmc_state_t *mcs);
-int pxmc_trp_gend1(pxmc_state_t *mcs);
-int pxmc_trp_gend(pxmc_state_t *mcs);
-
-/**
- * pxmc_trp_gi - Initializes trapezoid generator
- * @mcs: Motion controller state information
- *
- * This complex generator realizes motion to requested
- * end position @pxms_ep with trapezoid speed profile
- * constrained by @pxms_acc and @pxms_ms parameters.
- */
-int
-pxmc_trp_gi(pxmc_state_t *mcs)
-{
- long rp, rs, ep;
- pxmc_set_flag(mcs,PXMS_BSY_b);
- rp=mcs->pxms_rp;
- rs=mcs->pxms_rs;
- ep=mcs->pxms_ep;
- mcs->pxms_gen_tep=ep;
- if(ep==rp)
- return pxmc_trp_gend1(mcs);
- if(ep>rp){
- ep=ep-rp;
- mcs->pxms_gen_ttpfr=ep&1;
- mcs->pxms_gen_ttp=rp+(ep>>1);
- mcs->pxms_gen_tac=mcs->pxms_ma;
- mcs->pxms_gen_tsp=mcs->pxms_ms;
- mcs->pxms_gen_tspfg=0;
- mcs->pxms_do_gen=pxmc_trp_gdu10;
- }else{
- ep=rp-ep;
- mcs->pxms_gen_ttpfr=ep&1;
- mcs->pxms_gen_ttp=rp-(ep>>1);
- mcs->pxms_gen_tac=mcs->pxms_ma;
- mcs->pxms_gen_tsp=-mcs->pxms_ms;
- mcs->pxms_gen_tspfg=0;
- mcs->pxms_do_gen=pxmc_trp_gdd10;
- }
- mcs->pxms_rs=0;
- return 0;
-}
-
-#ifdef PXMC_WITH_FINE_GRAINED
-/**
- * pxmc_trp_spdfg_gi - Initializes fine-grained trapezoid generator
- * @mcs: Motion controller state information
- *
- * This complex generator realizes motion to requested
- * end position @pxms_ep with trapezoid speed profile
- * constrained by @pxms_acc and @pxms_gen_tspfg parameters.
- */
-int
-pxmc_trp_spdfg_gi(pxmc_state_t *mcs)
-{
- long rp, rs, ep;
- pxmc_set_flag(mcs,PXMS_BSY_b);
- rp=mcs->pxms_rp;
- rs=mcs->pxms_rs;
- ep=mcs->pxms_ep;
- mcs->pxms_gen_tep=ep;
- if(ep==rp)
- return pxmc_trp_gend1(mcs);
- if(ep>rp){
- ep=ep-rp;
- mcs->pxms_gen_ttpfr=ep&1;
- mcs->pxms_gen_ttp=rp+(ep>>1);
- mcs->pxms_gen_tac=mcs->pxms_ma;
- if(mcs->pxms_gen_tspfg<0)
- mcs->pxms_gen_tspfg=-mcs->pxms_gen_tspfg;
- #ifdef PXMC_SPDFG_ONLY
- mcs->pxms_gen_tsp=0;
- #else
- mcs->pxms_gen_tsp=mcs->pxms_gen_tspfg>>PXMC_SUBDIVFG(&mcs);
- #endif
- mcs->pxms_do_gen=pxmc_trp_gdu10;
- }else{
- ep=rp-ep;
- mcs->pxms_gen_ttpfr=ep&1;
- mcs->pxms_gen_ttp=rp-(ep>>1);
- mcs->pxms_gen_tac=mcs->pxms_ma;
- if(mcs->pxms_gen_tspfg>0)
- mcs->pxms_gen_tspfg=-mcs->pxms_gen_tspfg;
- #ifdef PXMC_SPDFG_ONLY
- mcs->pxms_gen_tsp=-1;
- #else
- mcs->pxms_gen_tsp=mcs->pxms_gen_tspfg>>PXMC_SUBDIVFG(&mcs);
- #endif
- mcs->pxms_do_gen=pxmc_trp_gdd10;
- }
- mcs->pxms_rs=0;
- return 0;
-}
-#endif /*PXMC_WITH_FINE_GRAINED*/
-
-int
-pxmc_trp_gend(pxmc_state_t *mcs)
-{
- mcs->pxms_rp=mcs->pxms_gen_tep;
- return pxmc_trp_gend1(mcs);
-}
-
-int
-pxmc_trp_gend1(pxmc_state_t *mcs)
-{
- mcs->pxms_rs=0;
- #ifdef PXMC_WITH_FINE_GRAINED
- mcs->pxms_rsfg=0;
- #endif /*PXMC_WITH_FINE_GRAINED*/
- pxmc_clear_flag(mcs,PXMS_ENG_b);
- pxmc_clear_flag(mcs,PXMS_BSY_b);
- mcs->pxms_do_gen=pxmc_nop_gd;
- return 0;
-}
-
-/*------------------------------*/
-
-/* accelerated upward motion */
-int
-pxmc_trp_gdu10(pxmc_state_t *mcs)
-{
- long rp, rs;
- if(mcs->pxms_flg&PXMS_ERR_m)
- return pxmc_trp_gend1(mcs);
- rp=mcs->pxms_rp;
- rs=mcs->pxms_rs;
- /*sat_add_slsl(rs,mcs->pxms_gen_tac);*/
- rs+=mcs->pxms_gen_tac;
- if(rs<mcs->pxms_gen_tsp){
- sat_add_slsl(rp,rs);
- if(rp>=mcs->pxms_gen_ttp){
- /* one half of motion reached */
- rp=mcs->pxms_rp;
- /* rp=2*ttp+ttpfr-rp */
- rp=((mcs->pxms_gen_ttp<<1)|(mcs->pxms_gen_ttpfr&1))-rp;
- mcs->pxms_do_gen=pxmc_trp_gdu30;
- }
- }else{
- /* naximal speed reached */
- /* ttp=2*ttp+ttpfr-rp */
- mcs->pxms_gen_ttp=((mcs->pxms_gen_ttp<<1)|(mcs->pxms_gen_ttpfr&1))-rp;
- rs=mcs->pxms_gen_tsp;
- #ifdef PXMC_WITH_FINE_GRAINED
- mcs->pxms_rsfg=mcs->pxms_gen_tspfg;
- #endif /*PXMC_WITH_FINE_GRAINED*/
- rp+=rs;
- mcs->pxms_do_gen=pxmc_trp_gdu20;
- }
- mcs->pxms_rp=rp;
- mcs->pxms_rs=rs;
- return 0;
-}
-
-/* upward motion with constant speed */
-int
-pxmc_trp_gdu20(pxmc_state_t *mcs)
-{
- long rp, rs;
- if(mcs->pxms_flg&PXMS_ERR_m)
- return pxmc_trp_gend1(mcs);
- pxmc_add_cspdfg(mcs);
- rp=mcs->pxms_rp;
- if(rp<mcs->pxms_gen_ttp)
- return 0;
- rs=mcs->pxms_gen_tsp-mcs->pxms_gen_tac;
- if(rs<=0)
- return pxmc_trp_gend(mcs);
- rp=mcs->pxms_gen_ttp+rs;
- mcs->pxms_rs=rs;
- mcs->pxms_rp=rp;
- #ifdef PXMC_WITH_FINE_GRAINED
- mcs->pxms_rsfg=0;
- #endif /*PXMC_WITH_FINE_GRAINED*/
- mcs->pxms_do_gen=pxmc_trp_gdu30;
- return 0;
-}
-
-/* upward motion with deaccelerated speed */
-int
-pxmc_trp_gdu30(pxmc_state_t *mcs)
-{
- long rp, rs;
- if(mcs->pxms_flg&PXMS_ERR_m)
- return pxmc_trp_gend1(mcs);
- rp=mcs->pxms_rp;
- rs=mcs->pxms_rs;
- rs-=mcs->pxms_gen_tac;
- if(rs<=0)
- return pxmc_trp_gend(mcs);
- rp+=rs;
- mcs->pxms_rs=rs;
- mcs->pxms_rp=rp;
- return 0;
-}
-
-/*------------------------------*/
-
-/* accelerated downward motion */
-int
-pxmc_trp_gdd10(pxmc_state_t *mcs)
-{
- long rp, rs;
- if(mcs->pxms_flg&PXMS_ERR_m)
- return pxmc_trp_gend1(mcs);
- rp=mcs->pxms_rp;
- rs=mcs->pxms_rs;
- /*sat_sub_slsl(rs,mcs->pxms_gen_tac);*/
- rs-=mcs->pxms_gen_tac;
- if(rs>mcs->pxms_gen_tsp){
- sat_add_slsl(rp,rs);
- if(rp<=mcs->pxms_gen_ttp){
- /* one half of motion reached */
- rp=mcs->pxms_rp;
- /* rp=2*ttp-ttpfr-rp */
- rp=((mcs->pxms_gen_ttp<<1)-(mcs->pxms_gen_ttpfr&1))-rp;
- mcs->pxms_do_gen=pxmc_trp_gdd30;
- }
- }else{
- /* naximal speed reached */
- /* ttp=2*ttp-ttpfr-rp */
- mcs->pxms_gen_ttp=((mcs->pxms_gen_ttp<<1)-(mcs->pxms_gen_ttpfr&1))-rp;
- rs=mcs->pxms_gen_tsp;
- #ifdef PXMC_WITH_FINE_GRAINED
- mcs->pxms_rsfg=mcs->pxms_gen_tspfg;
- #endif /*PXMC_WITH_FINE_GRAINED*/
- rp+=rs;
- mcs->pxms_do_gen=pxmc_trp_gdd20;
- }
- mcs->pxms_rp=rp;
- mcs->pxms_rs=rs;
- return 0;
-}
-
-/* downward motion with constant speed */
-int
-pxmc_trp_gdd20(pxmc_state_t *mcs)
-{
- long rp, rs;
- if(mcs->pxms_flg&PXMS_ERR_m)
- return pxmc_trp_gend1(mcs);
- pxmc_add_cspdfg(mcs);
- rp=mcs->pxms_rp;
- if(rp>mcs->pxms_gen_ttp)
- return 0;
- rs=mcs->pxms_gen_tsp+mcs->pxms_gen_tac;
- if(rs>=0)
- return pxmc_trp_gend(mcs);
- rp=mcs->pxms_gen_ttp+rs;
- mcs->pxms_rs=rs;
- mcs->pxms_rp=rp;
- #ifdef PXMC_WITH_FINE_GRAINED
- mcs->pxms_rsfg=0;
- #endif /*PXMC_WITH_FINE_GRAINED*/
- mcs->pxms_do_gen=pxmc_trp_gdd30;
- return 0;
-}
-
-/* downward motion with deaccelerated speed */
-int
-pxmc_trp_gdd30(pxmc_state_t *mcs)
-{
- long rp, rs;
- if(mcs->pxms_flg&PXMS_ERR_m)
- return pxmc_trp_gend1(mcs);
- rp=mcs->pxms_rp;
- rs=mcs->pxms_rs;
- rs+=mcs->pxms_gen_tac;
- if(rs>=0)
- return pxmc_trp_gend(mcs);
- rp+=rs;
- mcs->pxms_rs=rs;
- mcs->pxms_rp=rp;
- return 0;
-}
-
-/*------------------------------------------------------------------*/
-
-int pxmc_spd_gd10(pxmc_state_t *mcs);
-int pxmc_spd_gd20(pxmc_state_t *mcs);
-int pxmc_stop_gi(pxmc_state_t *mcs);
-int pxmc_spdnext_gd(pxmc_state_t *mcs);
-int pxmc_spdnext_gend(pxmc_state_t *mcs);
-
-/**
- * pxmc_spd_gi - Initializes constant speed generator
- * @mcs: Motion controller state information
- *
- * Requested speed is smoothly changed to @pxms_gen_spd_sp
- * with acceleration defined by @pxms_gen_spd_ac.
- * New generator @pxms_gen_spd_next can be selected after
- * required speed is reached.
- */
-int
-pxmc_spd_gi(pxmc_state_t *mcs)
-{
- pxmc_set_flag(mcs,PXMS_BSY_b);
- #ifdef PXMC_WITH_FINE_GRAINED
- mcs->pxms_rsfg=0;
- #endif /*PXMC_WITH_FINE_GRAINED*/
- mcs->pxms_do_gen=pxmc_spd_gd10;
- return pxmc_spd_gd10(mcs);
-}
-
-int
-pxmc_spd_gd10(pxmc_state_t *mcs)
-{
- if(mcs->pxms_gen_spd_timeout){
- if(!mcs->pxms_gen_spd_timeout--)
- return pxmc_stop_gi(mcs);
- }
- if(mcs->pxms_flg&PXMS_ERR_m)
- return pxmc_spdnext_gend(mcs);
-
- if(!pxmc_spd_gacc(&(mcs->pxms_rs),
- mcs->pxms_gen_spd_sp,mcs->pxms_gen_spd_ac)){
- #ifdef PXMC_WITH_FINE_GRAINED
- mcs->pxms_rsfg=mcs->pxms_gen_spd_spfg;
- #endif /*PXMC_WITH_FINE_GRAINED*/
- mcs->pxms_do_gen=pxmc_spd_gd20;
- };
- mcs->pxms_rp+=mcs->pxms_rs;
- return 0;
-}
-
-int
-pxmc_spd_gd20(pxmc_state_t *mcs)
-{
- if(mcs->pxms_flg&PXMS_ERR_m)
- return pxmc_trp_gend1(mcs);
- pxmc_add_cspdfg(mcs);
- return 0;
-}
-
-/**
- * pxmc_spdnext_gi - Initializes transition to zero and then generator change
- * @mcs: Motion controller state information
- */
-int
-pxmc_spdnext_gi(pxmc_state_t *mcs)
-{
- #ifdef PXMC_WITH_FINE_GRAINED
- mcs->pxms_rsfg=0;
- #endif /*PXMC_WITH_FINE_GRAINED*/
- mcs->pxms_do_gen=pxmc_spdnext_gd;
- return pxmc_spdnext_gd(mcs);
-}
-
-/**
- * pxmc_stop_gi - Initializes transition to zero speed and then stop
- * @mcs: Motion controller state information
- */
-int
-pxmc_stop_gi(pxmc_state_t *mcs)
-{
- #ifdef PXMC_WITH_FINE_GRAINED
- mcs->pxms_rsfg=0;
- #endif /*PXMC_WITH_FINE_GRAINED*/
- mcs->pxms_gen_spd_next=0;
- mcs->pxms_do_gen=pxmc_spdnext_gd;
- return pxmc_spdnext_gd(mcs);
-}
-
-
-int
-pxmc_spdnext_gd(pxmc_state_t *mcs)
-{
- if(mcs->pxms_flg&PXMS_ERR_m)
- return pxmc_spdnext_gend(mcs);
-
- if(pxmc_spd_gacc(&(mcs->pxms_rs),0,mcs->pxms_ma)){
- mcs->pxms_rp+=mcs->pxms_rs;
- return 0;
- }
-
- if(!mcs->pxms_gen_spd_next)
- return pxmc_spdnext_gend(mcs);
-
- mcs->pxms_do_gen=(pxmc_call_t*)mcs->pxms_gen_spd_next;
- return 0;
-}
-
-int
-pxmc_spdnext_gend(pxmc_state_t *mcs)
-{
- pxmc_set_spd(mcs,0);
- pxmc_clear_flag(mcs,PXMS_ENG_b);
- pxmc_clear_flag(mcs,PXMS_BSY_b);
- mcs->pxms_do_gen=pxmc_nop_gd;
- return 0;
-}
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc_hhfind.c - generic multi axis motion controller
- hard home finding support
-
- (C) 2001-2005 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (C) 2002-2005 by PiKRON Ltd. http://www.pikron.com
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
-
- *******************************************************************/
-
-#include <types.h>
-#include <cpu_def.h>
-#include <system_def.h>
-#include <string.h>
-#include "pxmc.h"
-#include "pxmc_internal.h"
-#include "pxmc_gen_info.h"
-
-/**
- * pxmc_hh - Starts reference search - Hard Home
- * @mcs: Motion controller state information
- */
-int pxmc_hh(pxmc_state_t *mcs)
-{
- long ispeed;
- pxmc_call_t *do_gen_init=pxmc_get_hh_gi_4axis(mcs);
-
- if(!do_gen_init)
- return -1;
-
- if(pxmc_set_gen_prep(mcs)<0) return -1;
-
- mcs->pxms_gen_st=0;
- ispeed=mcs->pxms_ms>>(mcs->pxms_cfg&PXMS_CFG_HSPD_m); /* initial speed */
- if(!ispeed)ispeed=1;
- if(mcs->pxms_cfg&PXMS_CFG_HDIR_m)ispeed=-ispeed; /* initial direction */
- mcs->pxms_gen_hsp=ispeed; /* initial speed */
- mcs->pxms_gen_hac=mcs->pxms_ma; /* axis acceleration */
- mcs->pxms_gen_hcfg=mcs->pxms_cfg; /* home config */
-
- #ifndef PXMC_WITH_FLAGS_BYBITS_ONLY
- pxmc_set_flags(mcs,PXMS_ENI_m|PXMS_ENR_m|PXMS_ENG_m|PXMS_BSY_m);
- #else /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
- pxmc_set_flag(mcs,PXMS_BSY_b);
- pxmc_set_flag(mcs,PXMS_ENI_b);
- pxmc_set_flag(mcs,PXMS_ENR_b);
- pxmc_set_flag(mcs,PXMS_ENG_b);
- #endif /*PXMC_WITH_FLAGS_BYBITS_ONLY*/
- __memory_barrier();
-
- /* mcs->pxms_do_gen=usd_irc_find_mark; */
- mcs->pxms_do_gen=do_gen_init;
-
- return 0;
-}
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc_hh_basic.c - generic multi axis motion controller
- hard home finding support
-
- (C) 2001-2005 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (C) 2002-2005 by PiKRON Ltd. http://www.pikron.com
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
-
- *******************************************************************/
-
-#include <types.h>
-#include <cpu_def.h>
-#include <h8s2638h.h>
-#include <system_def.h>
-#include <string.h>
-#include "pxmc.h"
-#include "pxmc_internal.h"
-#include "pxmc_gen_info.h"
-
-int pxmc_hh_gd10(pxmc_state_t *mcs);
-int pxmc_hh_gd20(pxmc_state_t *mcs);
-
-int
-pxmc_hh_gi(pxmc_state_t *mcs)
-{
- long spd;
- pxmc_set_flag(mcs,PXMS_BSY_b);
- mcs->pxms_rsfg=0;
- spd=mcs->pxms_ms;
- spd>>=mcs->pxms_cfg&PXMS_CFG_HSPD_m;
- if(!spd) spd=1;
- if(mcs->pxms_cfg&PXMS_CFG_HDIR_m)
- spd=-spd;
- mcs->pxms_gen_hsp=spd;
- mcs->pxms_do_gen=pxmc_hh_gd10;
- return pxmc_hh_gd10(mcs);
-}
-
-int
-pxmc_hh_gd10(pxmc_state_t *mcs)
-{
- long spd;
- if(mcs->pxms_flg&PXMS_ERR_m)
- return pxmc_spdnext_gend(mcs);
-
- pxmc_spd_gacc(&(mcs->pxms_rs),mcs->pxms_gen_hsp,mcs->pxms_ma);
- mcs->pxms_rp+=mcs->pxms_rs;
-
- if(!(*DIO_PORTF&0x80)){
- spd=mcs->pxms_gen_hsp;
- if(spd>0){
- spd>>=2;
- spd=spd?-spd:-1;
- }else{
- spd>>=2;
- spd=spd?-spd:1;
- }
- mcs->pxms_gen_hsp=spd;
- mcs->pxms_do_gen=pxmc_hh_gd20;
- }
-
- return 0;
-}
-
-int
-pxmc_hh_gd20(pxmc_state_t *mcs)
-{
- if(mcs->pxms_flg&PXMS_ERR_m)
- return pxmc_spdnext_gend(mcs);
-
- pxmc_spd_gacc(&(mcs->pxms_rs),mcs->pxms_gen_hsp,mcs->pxms_ma);
- mcs->pxms_rp+=mcs->pxms_rs;
-
- if(*DIO_PORTF&0x80){
- pxmc_axis_set_pos(mcs,0);
- mcs->pxms_do_gen=pxmc_stop_gi;
- }
-
- return 0;
-}
-
-pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
-{
- return pxmc_hh_gi;
-}
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc_inp_common.h - generic multi axis motion controller
- common reusable routines for position input
-
- (C) 2001-2005 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (C) 2002-2005 by PiKRON Ltd. http://www.pikron.com
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
-
- *******************************************************************/
-
-#ifndef _PXMC_INP_COMMON_H_
-#define _PXMC_INP_COMMON_H_
-
-#include "pxmc.h"
-
-/* update PXMC state according 16 bit IRC counter value */
-static inline void
-pxmc_irc_16bit_update(pxmc_state_t *mcs, short irc)
-{
- long pos;
- long spd;
-
- long val_mag=1l<<(PXMC_SUBDIV(mcs)+16);
- pos=(long)(unsigned short)irc<<PXMC_SUBDIV(mcs);
- pos|= mcs->pxms_ap &~(val_mag-1);
- spd=pos-mcs->pxms_ap;
- if(spd<=-(val_mag>>1)) {pos+=val_mag;spd+=val_mag;}
- else if(spd>=(val_mag>>1)) {pos-=val_mag;spd-=val_mag;}
-
- mcs->pxms_ap=pos;
- mcs->pxms_as=spd;
-}
-
-#ifdef PXMC_WITH_PHASE_TABLE
-static inline void
-pxmc_irc_16bit_commindx(struct pxmc_state *mcs, short irc)
-{
- short ofs;
- irc+=mcs->pxms_ptshift;
- ofs=irc-mcs->pxms_ptofs;
- if((unsigned short)ofs>=(unsigned short)mcs->pxms_ptirc)
- {
- if(ofs>0) {
- mcs->pxms_ptofs+=mcs->pxms_ptirc;
- } else {
- mcs->pxms_ptofs-=mcs->pxms_ptirc;
- }
- ofs=irc-mcs->pxms_ptofs;
- if((unsigned short)ofs>=(unsigned short)mcs->pxms_ptirc) {
- mcs->pxms_ptindx=0;
- pxmc_clear_flag(mcs,PXMS_PHA_b);
- pxmc_set_errno(mcs,PXMS_E_COMM);
- return;
- }
- }
- mcs->pxms_ptindx=ofs;
-}
-#else /*PXMC_WITH_PHASE_TABLE*/
-static inline void
-pxmc_irc_16bit_commindx(struct pxmc_state *mcs, short irc){}
-#endif /*PXMC_WITH_PHASE_TABLE*/
-
-#endif /*_PXMC_INP_COMMON_H_*/
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc_internal.h - generic multi axis motion controller
- internal functions
-
- (C) 2001-2005 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (C) 2002-2005 by PiKRON Ltd. http://www.pikron.com
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
-
- *******************************************************************/
-
-#ifndef _PXMC_INTERNAL_H_
-#define _PXMC_INTERNAL_H_
-
-#include "pxmc.h"
-
-int pxmc_spd_gacc(long *spd,long rspd,long acc);
-
-int pxmc_cont_gi(pxmc_state_t *mcs);
-int pxmc_trp_gi(pxmc_state_t *mcs);
-int pxmc_trp_spdfg_gi(pxmc_state_t *mcs);
-int pxmc_spd_gi(pxmc_state_t *mcs);
-int pxmc_spdnext_gi(pxmc_state_t *mcs);
-int pxmc_spdnext_gend(pxmc_state_t *mcs);
-int pxmc_stop_gi(pxmc_state_t *mcs);
-
-int pxmc_pid_con(pxmc_state_t *mcs);
-
-short pxmc_ptvang_deg2irc(pxmc_state_t *mcs, int deg);
-
-int pxmc_init_ptable_sin(pxmc_state_t *mcs);
-int pxmc_init_ptable_triang(pxmc_state_t *mcs);
-int pxmc_init_ptable_trapez(pxmc_state_t *mcs, int flatrat);
-int pxmc_init_ptable_sin3ph(pxmc_state_t *mcs);
-int pxmc_init_ptable_sin3phup(pxmc_state_t *mcs);
-int pxmc_init_ptable(pxmc_state_t *mcs, int profile);
-
-#define PXMC_PTPROF_DEFAULT 0
-#define PXMC_PTPROF_SIN 1
-#define PXMC_PTPROF_TRIANG 2
-#define PXMC_PTPROF_TRAPEZ 3
-#define PXMC_PTPROF_3PH 16
-#define PXMC_PTPROF_SIN3PH 17
-#define PXMC_PTPROF_SIN3FUP 18
-
-/********************************************************************/
-/* generator variant selection from axis configuration */
-
-#ifndef PXMC_WITH_GEN_SELECTION
-static inline pxmc_call_t *
-pxmc_get_stop_gi_4axis(pxmc_state_t *mcs)
- {return pxmc_stop_gi;}
-static inline pxmc_call_t *
-pxmc_get_cont_gi_4axis(pxmc_state_t *mcs)
- {return pxmc_cont_gi;}
-static inline pxmc_call_t *
-pxmc_get_spdnext_gi_4axis(pxmc_state_t *mcs)
- {return pxmc_spdnext_gi;}
-static inline pxmc_call_t *
-pxmc_get_trp_gi_4axis(pxmc_state_t *mcs, int mode)
- {return pxmc_trp_gi;}
-static inline pxmc_call_t *
-pxmc_get_trp_retgt_4axis(pxmc_state_t *mcs)
- {return 0 /*pxmc_trp_retgt*/;}
-static inline pxmc_call_t *
-pxmc_get_trp_spdfg_gi_4axis(pxmc_state_t *mcs)
- #ifdef PXMC_WITH_FINE_GRAINED
- {return pxmc_trp_spdfg_gi;}
- #else /*PXMC_WITH_FINE_GRAINED*/
- {return 0;}
- #endif /*PXMC_WITH_FINE_GRAINED*/
-static inline pxmc_call_t *
-pxmc_get_trprel_gi_4axis(pxmc_state_t *mcs)
- {return 0 /*pxmc_trprel_gi*/;}
-static inline pxmc_call_t *
-pxmc_get_spd_gi_4axis(pxmc_state_t *mcs)
- {return pxmc_spd_gi;}
-static inline pxmc_call_t *
-pxmc_get_spdfg_gi_4axis(pxmc_state_t *mcs)
- #ifdef PXMC_WITH_FINE_GRAINED
- {return pxmc_spd_gi;}
- #else /*PXMC_WITH_FINE_GRAINED*/
- {return 0;}
- #endif /*PXMC_WITH_FINE_GRAINED*/
-#else /*PXMC_WITH_GEN_SELECTION*/
-pxmc_call_t *pxmc_get_stop_gi_4axis(pxmc_state_t *mcs);
-pxmc_call_t *pxmc_get_cont_gi_4axis(pxmc_state_t *mcs);
-pxmc_call_t *pxmc_get_spdnext_gi_4axis(pxmc_state_t *mcs);
-pxmc_call_t *pxmc_get_trp_gi_4axis(pxmc_state_t *mcs, int mode);
-pxmc_call_t *pxmc_get_trp_retgt_4axis(pxmc_state_t *mcs);
-pxmc_call_t *pxmc_get_trp_spdfg_gi_4axis(pxmc_state_t *mcs);
-pxmc_call_t *pxmc_get_trprel_gi_4axis(pxmc_state_t *mcs);
-pxmc_call_t *pxmc_get_spd_gi_4axis(pxmc_state_t *mcs);
-pxmc_call_t *pxmc_get_spdfg_gi_4axis(pxmc_state_t *mcs);
-#endif /*PXMC_WITH_GEN_SELECTION*/
-
-pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs);
-
-#endif /*_PXMC_INTERNAL_H_*/
+++ /dev/null
-/*******************************************************************
- Components for embedded applications builded for
- laboratory and medical instruments firmware
-
- pxmc_ptable.c - generic multi axis motion controller
- phase tables generation support
-
- (C) 2001-2005 by Pavel Pisa pisa@cmp.felk.cvut.cz
- (C) 2002-2005 by PiKRON Ltd. http://www.pikron.com
-
- This file can be used and copied according to next
- license alternatives
- - MPL - Mozilla Public License
- - GPL - GNU Public License
- - other license provided by project originators
-
- *******************************************************************/
-
-/* [extern API] characteristic means that the function is declared
-in the header file pxmc.h so it is part of the external API */
-
-#include <types.h>
-#include <cpu_def.h>
-#include <system_def.h>
-#include <string.h>
-#include <malloc.h>
-#include <math.h>
-#include <limits.h>
-#include "pxmc.h"
-#include "pxmc_internal.h"
-
-#define PXMC_PTAMP (0x7fff)
-
-/**
- * pxmc_ptvang_deg2irc - Converts phase shift from deg to IRC
- * @mcs: Motion controller state information
- * @deg: Phase shift angle in degree
- */
-short pxmc_ptvang_deg2irc(pxmc_state_t *mcs, int deg)
-{
- short ptvang;
- ptvang=(((long)mcs->pxms_ptirc*deg)/mcs->pxms_ptper+(2*90))/(4*90);
- return ptvang;
-}
-
-int
-pxmc_init_ptable_sin(pxmc_state_t *mcs)
-{
- int ptirc=mcs->pxms_ptirc;
- int ptper=mcs->pxms_ptper;
- float ptamp=mcs->pxms_ptamp;
- int i;
- float a;
-
- for(i=0;i<ptirc;i++) {
- a=i*(float)ptper*2*M_PI/ptirc;
- mcs->pxms_ptptr1[i]=sin(a)*ptamp;
- mcs->pxms_ptptr2[i]=cos(a)*ptamp;
- }
- return 0;
-}
-
-int
-pxmc_init_ptable_sin3ph(pxmc_state_t *mcs)
-{
- int ptirc=mcs->pxms_ptirc;
- int ptper=mcs->pxms_ptper;
- float ptamp=mcs->pxms_ptamp;
- int i;
- float a;
-
- for(i=0;i<ptirc;i++) {
- a=i*(float)ptper*2*M_PI/ptirc;
- mcs->pxms_ptptr1[i]=cos(a)*ptamp;
- mcs->pxms_ptptr2[i]=cos(a+2*M_PI/3)*ptamp;
- mcs->pxms_ptptr3[i]=cos(a+4*M_PI/3)*ptamp;
- }
- return 0;
-}
-
-int
-pxmc_init_ptable_sin3phup(pxmc_state_t *mcs)
-{
- int ptirc=mcs->pxms_ptirc;
- int ptper=mcs->pxms_ptper;
- float ptamp=mcs->pxms_ptamp;
- int i;
- int min_val;
- float a;
-
- ptamp/=2;
-
- for(i=0;i<ptirc;i++) {
- a=i*(float)ptper*2*M_PI/ptirc;
- mcs->pxms_ptptr1[i]=cos(a)*ptamp;
- mcs->pxms_ptptr2[i]=cos(a+2*M_PI/3)*ptamp;
- mcs->pxms_ptptr3[i]=cos(a+4*M_PI/3)*ptamp;
-
- min_val=mcs->pxms_ptptr1[i];
- if(min_val>mcs->pxms_ptptr2[i])
- min_val=mcs->pxms_ptptr2[i];
- if(min_val>mcs->pxms_ptptr3[i])
- min_val=mcs->pxms_ptptr3[i];
-
- mcs->pxms_ptptr1[i]-=min_val;
- mcs->pxms_ptptr2[i]-=min_val;
- mcs->pxms_ptptr3[i]-=min_val;
- }
- return 0;
-}
-
-int
-pxmc_init_ptable_triang(pxmc_state_t *mcs)
-{
- int ptirc=mcs->pxms_ptirc;
- int ptper=mcs->pxms_ptper;
- unsigned int ptamp=mcs->pxms_ptamp;
- int i;
- long l;
-
- for(i=0;i<ptirc;i++) {
- l=i;
- l=(l*ptper*2+3*ptirc/2)%(ptirc*2); /* phase per ^= 2*ptirc */
- if(l<=ptirc) l=ptirc-2*l;
- else l=2*l-3*ptirc;
- mcs->pxms_ptptr1[i]=(ptamp*l)/ptirc;
-
- l=i;
- l=(l*ptper*2)%(ptirc*2); /* phase per ^= 2*ptirc */
- if(l<=ptirc) l=ptirc-2*l;
- else l=2*l-3*ptirc;
- mcs->pxms_ptptr2[i]=(ptamp*l)/ptirc;
- }
- return 0;
-}
-
-int
-pxmc_init_ptable_trapez(pxmc_state_t *mcs, int flatrat)
-{
- int ptirc=mcs->pxms_ptirc;
- int ptper=mcs->pxms_ptper;
- unsigned int ptamp=mcs->pxms_ptamp;
- int i;
- long l;
- long lim;
-
- lim=((long)256*ptamp)/flatrat;
- for(i=0;i<ptirc;i++) {
- l=i;
- l=(l*ptper*2+3*ptirc/2)%(ptirc*2); /* phase per ^= 2*ptirc */
- if(l<=ptirc) l=ptirc-2*l;
- else l=2*l-3*ptirc;
- l=(ptamp*l)/ptirc;
-
- if(l>lim) l=ptamp;
- else if(l<-lim) l=-ptamp;
- else l=(l*flatrat)/256;
-
- mcs->pxms_ptptr1[i]=l;
-
- l=i;
- l=(l*ptper*2)%(ptirc*2); /* phase per ^= 2*ptirc */
- if(l<=ptirc) l=ptirc-2*l;
- else l=2*l-3*ptirc;
- l=(ptamp*l)/ptirc;
-
- if(l>lim) l=ptamp;
- else if(l<-lim) l=-ptamp;
- else l=(l*flatrat)/256;
-
- mcs->pxms_ptptr2[i]=l;
- }
- return 0;
-}
-
-/**
- * pxmc_init_ptable - Initializes phase tables
- * @mcs: Motion controller state information
- * @profile: Phase profile selection
- *
- * This function initializes phase tables @pxms_ptptr1 and @pxms_ptptr2.
- * Profile is computed by one of profile functions -
- * pxmc_init_ptable_sin(), pxmc_init_ptable_triang(),
- * pxmc_init_ptable_trapez()
- */
-int
-pxmc_init_ptable(pxmc_state_t *mcs, int profile)
-{
- int ptirc=mcs->pxms_ptirc;
- int ret;
-
- if(!mcs->pxms_ptper || !mcs->pxms_ptirc)
- return -1 /*CMDERR_BADPAR*/;
-
- if(mcs->pxms_ptptr1)
- free(mcs->pxms_ptptr1);
- if(mcs->pxms_ptptr2)
- free(mcs->pxms_ptptr2);
- if(mcs->pxms_ptptr3)
- free(mcs->pxms_ptptr3);
-
- mcs->pxms_ptptr1=malloc(ptirc*sizeof(mcs->pxms_ptptr1[0]));
- mcs->pxms_ptptr2=malloc(ptirc*sizeof(mcs->pxms_ptptr2[0]));
- if(!mcs->pxms_ptptr1 || !mcs->pxms_ptptr2)
- return -1 /*CMDERR_NOMEM*/;
-
- if(profile>=PXMC_PTPROF_3PH){
- mcs->pxms_ptptr3=malloc(ptirc*sizeof(mcs->pxms_ptptr3[0]));
- if(!mcs->pxms_ptptr3)
- return -1 /*CMDERR_NOMEM*/;
- }
-
- if(!mcs->pxms_ptamp)
- mcs->pxms_ptamp=PXMC_PTAMP;
-
- mcs->pxms_ptindx=0;
-
- switch(profile){
- case PXMC_PTPROF_SIN:
- ret=pxmc_init_ptable_sin(mcs);
- break;
- case PXMC_PTPROF_TRIANG:
- ret=pxmc_init_ptable_triang(mcs);
- break;
- case PXMC_PTPROF_TRAPEZ:
- ret=pxmc_init_ptable_trapez(mcs, 512);
- break;
- case PXMC_PTPROF_3PH:
- case PXMC_PTPROF_SIN3PH:
- ret=pxmc_init_ptable_sin3ph(mcs);
- break;
- case PXMC_PTPROF_SIN3FUP:
- ret=pxmc_init_ptable_sin3phup(mcs);
- break;
-
- default:
- ret=pxmc_init_ptable_triang(mcs);
- }
-
- return ret;
-}
projects / eurobot / public.git / commitdiff