--- /dev/null
+/* procesor H8S/2638 ver 1.1 */
+#include <types.h>
+#include <cpu_def.h>
+#include <mcu_regs.h>
+#include <system_def.h>
+#include <string.h>
+#include <boot_fn.h>
+#include <pxmc.h>
+//#include <pxmc_internal.h>
+#include <pxmcbsp.h>
+
+#include <ctype.h>
+#include <stdlib.h>
+#include <utils.h>
+#include <periph/sci_rs232.h>
+#include <periph/sci_channels.h>
+#include <stdio.h>
+
+#include <cmd_proc.h>
+
+#include "cmd_pxmc.h"
+//#include <math.h>
+#include <cmd_proc.h>
+#include <candriver.h>
+#include <canOpenDriver.h>
+#include <can_ids.h>
+
+
+
+int cmd_do_quit(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ printf("Good bye!\n");
+ *HCAN1_MCR=1; // software reset
+ //exit(0);
+ return 0;
+}
+
+int cmd_do_setflags(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_state_t *mcs = &mcs_left;
+
+ pxmc_set_flag(mcs,PXMS_PTI_b); // start to updating index pointing to the phase table (ptindx)
+ pxmc_set_flag(mcs,PXMS_PHA_b); // FIXME: is it really need here
+ printf("Flags were set!\n");
+ return 0;
+}
+
+int cmd_do_haltest(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_state_t *mcs;
+ int i;
+ short vang;
+ //int turns = 2;
+ int turn = 0;
+
+ i = atoi(param[1]);
+ if (i >= pxmc_main_list.pxml_cnt)
+ return -1;
+ mcs = pxmc_main_list.pxml_arr[i];
+
+ vang = mcs->pxms_ptvang;
+ mcs->pxms_ptvang=0;
+ mcs->pxms_ene=mcs->pxms_me/5;
+ i=0;
+ while (turn<2)
+ {
+ pxmc_initialize(); // reset index mark detection
+ do
+ {
+ if(turn<1)i++;
+ else i--;
+ int index = ((long)i*mcs->pxms_ptirc/360)%mcs->pxms_ptirc;
+ char h;
+ mcs->pxms_flg = 0;
+ pxmc_call(mcs, mcs->pxms_do_inp);
+ mcs->pxms_flg = PXMS_PHA_m | PXMS_PTI_m;
+ mcs->pxms_ptindx=index;
+ motor_do_output(mcs);
+ h = hal_read(mcs);
+ printf("%d %d %ld %d\n", mcs->pxms_ptindx, h, mcs->pxms_ap, mcs->pxms_ptofs);
+ }
+ while(i<2*360 && i>0);
+ turn++;
+ }
+ mcs->pxms_ene=0;
+ mcs->pxms_ptvang = vang;
+ motor_do_output(mcs);
+ mcs->pxms_flg = PXMS_ENI_m;
+
+ return 0;
+}
+
+int cmd_do_setindex(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_state_t *mcs = &mcs_left;
+ int index=atol(param[1]);
+ printf("index=%d\n", index);
+ //printf("No errors detected.\n", index);
+ mcs->pxms_ptindx=index;
+ return 0;
+}
+
+int cmd_do_setvang(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_state_t *mcs = &mcs_left;
+ int index=atol(param[1]);
+ printf("ptvang=%d\n", index);
+ //printf("No errors detected.\n", index);
+ mcs->pxms_ptvang=index;
+ return 0;
+}
+
+
+int cmd_do_setshift(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_state_t *mcs = &mcs_left;
+ int index=atol(param[1]);
+ printf("ptshift=%d\n", index);
+ //printf("No errors detected.\n", index);
+ mcs->pxms_ptshift=index;
+ return 0;
+}
+
+int cmd_do_showene(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_state_t *mcs = &mcs_left;
+// if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ printf("Current ene is: %d\n",mcs->pxms_ene);
+ printf("Current ptindx is: %d\n",mcs->pxms_ptindx);
+ printf("Current ptvang is: %d\n",mcs->pxms_ptvang);
+
+ printf("Current ptshift is: %d\n",mcs->pxms_ptshift);
+ printf("Current ptofs is: %d\n",mcs->pxms_ptofs);
+
+
+ printf("Actual pos is: %ld\n",mcs->pxms_ap>>PXMC_SUBDIV(mcs));
+ printf("Request pos is: %ld\n",mcs->pxms_rp>>PXMC_SUBDIV(mcs));
+
+ printf("inp_info is: %d\n",*(volatile short*)mcs->pxms_inp_info);
+
+ //printf("hal is: %d\n", hal_read(mcs) );
+
+ if((mcs->pxms_flg&PXMS_PTI_m)) printf("PXMS_PTI_m is: enabled\n"); else printf("PXMS_PTI_m is: disabled\n");
+ if((mcs->pxms_flg&PXMS_PHA_m)) printf("PXMS_PHA_m is: enabled\n"); else printf("PXMS_PHA_m is: disabled\n");
+ return 0;
+}
+
+int cmd_do_nopxmc(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_state_t *mcs = &mcs_left;
+ pxmc_clear_flag(mcs,PXMS_ENR_b);
+ pxmc_clear_flag(mcs,PXMS_ENO_b);
+ *PWM_PWBFR1A=0;
+ *PWM_PWBFR1C=0;
+ *PWM_PWBFR1E=0;
+ return 0;
+}
+
+int cmd_do_mypwm(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ int r;
+ int val;
+#define _PXMC_MAX_OUT_ 0x6f
+ const unsigned long ph1[6]={2, 1, 1, 0, 1, 1};
+ const unsigned long ph2[6]={1, 0, 1, 1, 2, 1};
+ const unsigned long ph3[6]={1, 1, 2, 1, 1, 0};
+#undef _PMXC_MAX_OUT_
+
+ r=atol(param[1]);
+ val=atol(param[2])/2;
+ if (val==0) val = 0;
+ printf("r=%d val=%d\n", r, val);
+
+ unsigned pwm1=val*ph1[r];
+ unsigned pwm2=val*ph2[r];
+ unsigned pwm3=val*ph3[r];
+ *PWM_PWBFR1A=(pwm1&0x3ff);
+ *PWM_PWBFR1C=(pwm2&0x3ff);
+ *PWM_PWBFR1E=(pwm3&0x3ff);
+ return 0;
+}
+
+int runtime_display = 0;
+int slowgo = 0;
+
+cmd_des_t const **cmd_list;
+
+/* command descriptions */
+cmd_des_t const cmd_des_help={0, 0,"HELP","prints help for commands",
+ cmd_do_help,{(char*)&cmd_list}};
+cmd_des_t const cmd_des_showene={0, 0,"showene","shows ene and couple others parameters",
+ cmd_do_showene};
+
+cmd_des_t const cmd_des_mypwm={0, 0,"x#","test",
+ cmd_do_mypwm};
+cmd_des_t const cmd_des_nopxmc={0, 0,"nopxmc","switch of pxmc controller and output",
+ cmd_do_nopxmc};
+cmd_des_t const cmd_des_disp={0, CDESM_OPCHR|CDESM_RW,"disp","switch on/off runtime display of variables",
+ cmd_do_rw_int, {(char*)&runtime_display}};
+cmd_des_t const cmd_des_slowgo={0, CDESM_OPCHR|CDESM_RW,"slowgo","program controlled movement",
+ cmd_do_rw_int, {(char*)&slowgo}};
+cmd_des_t const cmd_des_setindex={0, 0,"setindex","changes pxms_ptindx",
+ cmd_do_setindex};
+cmd_des_t const cmd_des_setvang={0, 0,"setvang","changes pxms_ptvang",
+ cmd_do_setvang};
+cmd_des_t const cmd_des_halindex={0, 0,"haltest","show hal state with respect to ptindex",
+ cmd_do_haltest};
+cmd_des_t const cmd_des_setshift={0, 0,"setshift","changes pxms_ptshift",
+ cmd_do_setshift};
+cmd_des_t const cmd_des_quit={0, 0,"quit","exit",
+ cmd_do_quit};
+
+cmd_des_t const cmd_des_setflags={0, 0,"setflags","set some flags",
+ cmd_do_setflags};
+
+
+extern cmd_io_t cmd_io_rs232_line;
+cmd_des_t const *cmd_list_default[]={
+
+ &cmd_des_help,
+ &cmd_des_quit,
+ &cmd_des_setflags,
+ &cmd_des_mypwm,
+ &cmd_des_setindex,
+ &cmd_des_setvang,
+ &cmd_des_halindex,
+ &cmd_des_setshift,
+ &cmd_des_showene,
+ &cmd_des_nopxmc,
+ &cmd_des_disp,
+ &cmd_des_slowgo,
+ (cmd_des_t*)1,
+ (cmd_des_t*)cmd_stm_default,
+ NULL
+};
+
+cmd_des_t const **cmd_list=cmd_list_default; /*cmd prikazy pro PC*/
+
+//Interrupt routines
+void unhandled_exception(void) __attribute__ ((interrupt_handler));
+void unhandled_exception(void)
+{
+ DEB_LED_ON(LED_ERROR);
+ FlWait(20000);
+ DEB_LED_OFF(LED_ERROR);
+ FlWait(20000);
+}
+
+void print_mcs_err(pxmc_state_t *mcs, char *name)
+{
+ const char *errmsg[] = {
+ [PXMS_E_COMM&0xff] = "COMM",
+ [PXMS_E_MAXPD&0xff] = "MAXPD",
+ [PXMS_E_OVERL&0xff] = "OVERL",
+ [PXMS_E_HAL&0xff] = "HAL",
+ [PXMS_E_POWER_STAGE&0xff] = "POWER"};
+ printf("PXMC ERROR: %s - %s\n", name, errmsg[mcs->pxms_errno&0xff]);
+}
+
+short idxRecive =0;
+short idxStore=0;
+#define ODOMETRY_TABLE_SIZE 10
+struct odometryData_t
+{
+ short dx,dy;
+ long angle;
+} oddata[ODOMETRY_TABLE_SIZE];
+
+#if 0
+static inline short sendOdometry()
+{
+ static short idxSend=0;
+ static short prevIdxRecive=0xff;
+
+ if(prevIdxRecive==idxRecive)
+ {
+ // Re-send can frames.
+ }
+ else
+ {
+ prevIdxRecive=idxRecive;
+ // Here should be function to send.
+ }
+
+ return 0;
+}
+#endif
+
+/**
+ * Sends only pxms_ap. Odometry is calculated elsewhere and since we
+ * don't send defferences, sequence numbers are not necessary.
+ */
+static inline void sendOdometrySimple()
+{
+ /* If the variable is static, it is initialized to zero, co we don't have to initialize it all the time again. */
+ static Message m;
+ int32_t lap, rap;
+
+ lap = mcs_left.pxms_ap;
+ rap = mcs_right.pxms_ap;
+
+
+ m.cob_id.w = CAN_MOTION_ODOMETRY_SIMPLE;
+ m.len = 8;
+ /* Data in big-endian */
+ memcpy(&m.data[0], &lap, sizeof(lap));
+ memcpy(&m.data[4], &rap, sizeof(lap));
+ canMsgTransmit(0, m);
+}
+
+#if O
+static inline short storeOdometryInTable()
+{
+ /*FIXME: add detection of error when idxStore>idxRecive.*/
+ oddata[idxStore].dx=x;
+ oddata[idxStore].dy=y;
+ oddata[idxStore].angle=alfa;
+ // Start to count from zero ;)
+ x=0; y=0; alfa=0;
+ idxStore++;
+ idxStore%=ODOMETRY_TABLE_SIZE;
+
+ return 0;
+}
+#endif
+
+
+static inline void handle_motor_errors() {
+ static unsigned last_msg_time=0;
+
+ if (mcs_left.pxms_flg&PXMS_ERR_m||mcs_right.pxms_flg&PXMS_ERR_m) {
+ pxmc_stop(&mcs_left, 0);
+ pxmc_stop(&mcs_right, 0);
+ if (pxmc_msec_counter - last_msg_time >= 2000) {
+ last_msg_time = pxmc_msec_counter;
+ if (mcs_left.pxms_flg&PXMS_ERR_m) print_mcs_err(&mcs_left, "L");
+ if (mcs_right.pxms_flg&PXMS_ERR_m) print_mcs_err(&mcs_right, "R");
+ }
+ }
+}
+
+static inline void handle_can_receive(void)
+{
+ Message msg_rcv;
+ static unsigned last_rec;
+
+ if (f_can_receive(0, &msg_rcv)) {
+ switch (msg_rcv.cob_id.w) {
+ case CAN_MOTION_CMD: {
+ short spd_left, spd_right;
+ spd_left = (msg_rcv.data[0] << 8) | msg_rcv.data[1];
+ spd_right= (msg_rcv.data[2] << 8) | msg_rcv.data[3];
+
+ pxmc_spd(&mcs_left, -spd_left, pxmc_sfikhz*1000 /*timeout in sampling periods = 0.5ms*/);
+ pxmc_spd(&mcs_right,+spd_right, pxmc_sfikhz*1000);
+/* printf("Left motor speed command: %08x\n",spd_left); */
+/* printf("Right motor speed command: %08x\n",spd_right); */
+ break;
+ }
+ case CAN_MOTION_RESET: {
+ unsigned now = pxmc_msec_counter;
+ wdg_enable(0); /* 25 us */
+ while (pxmc_msec_counter - now < 10);
+ /* Hopefully, we are reset now. */
+ break;
+ }
+ }
+
+#ifdef LED_CAN_REC
+ last_rec = pxmc_msec_counter;
+ DEB_LED_ON(LED_CAN_REC);
+#endif
+
+
+/* int i; */
+/* printf("Received message: id=%lx\n", msg_rcv.cob_id.w); */
+/* for (i = 0 ; i < 8; i++) printf("%02x ", msg_rcv.data[i]); */
+/* printf("\n"); */
+ }
+#ifdef LED_CAN_REC
+ if (pxmc_msec_counter - last_rec > 100)
+ DEB_LED_OFF(LED_CAN_REC);
+#endif
+}
+
+static inline void handle_odometry_send()
+{
+ static unsigned last_send_time=0;
+
+ if (pxmc_msec_counter - last_send_time >= 50) {
+ last_send_time = pxmc_msec_counter;
+ //sendOdometry();
+ sendOdometrySimple();
+ }
+}
+
+static inline void handle_status_send()
+{
+ static unsigned last_send_time=0;
+ Message m;
+
+ if (pxmc_msec_counter - last_send_time >= 500) {
+ last_send_time = pxmc_msec_counter;
+ memset(&m, 0, sizeof(m));
+ m.cob_id.w = CAN_MOTION_STATUS;
+ m.len = 4;
+ m.data[0] = mcs_left.pxms_errno >> 8;
+ m.data[1] = mcs_left.pxms_errno & 0xff;
+ m.data[2] = mcs_right.pxms_errno >> 8;
+ m.data[3] = mcs_right.pxms_errno & 0xff;
+ canMsgTransmit(0, m);
+ }
+}
+
+void _print(char *ptr);
+int main()
+{
+ cli();
+ excptvec_initfill(unhandled_exception, 0);
+ sci_rs232_setmode(19200, 0, 0, sci_rs232_chan_default); //PC
+ wdg_enable(6); /* 420 ms */
+ sti();
+ _print("CPU initialized\r\n");
+
+ pxmc_initialize();
+ printf("PXMC initialized\n");
+
+ canInit(0, 1000000);
+ printf("CAN initialized\n");
+
+ unsigned i=0;
+ do {
+ wdg_clear();
+ handle_can_receive();
+ handle_odometry_send();
+ handle_status_send();
+ handle_motor_errors();
+
+ cmd_processor_run(&cmd_io_rs232_line, cmd_list_default); // run command processor on serial line
+ i++;
+
+ if (pxmc_msec_counter%1000 > 500) DEB_LED_ON(LED_LIVE);
+ else DEB_LED_OFF(LED_LIVE);
+
+ if (runtime_display) {
+ //printf("c=%d idx=%d\n", test_counter, test_index);
+ //printf("ene=%d\n", mcs_left.pxms_ene);
+ printf("rs=%ld as=%ld\n", mcs_left.pxms_rs, mcs_left.pxms_as);
+ }
+ } while (1);
+
+ return 0;
+};
--- /dev/null
+/*******************************************************************
+ Components for embedded applications builded for
+ laboratory and medical instruments firmware
+
+ cmd_stm.c - interconnection of Stepper Motor control
+ subsystem with RS-232 command processor
+ used mainly for controller tuning
+
+ Copyright (C) 2001 by Pavel Pisa pisa@cmp.felk.cvut.cz
+ (C) 2002 by PiKRON Ltd. http://www.pikron.com
+
+ *******************************************************************/
+
+#include <types.h>
+#include <ctype.h>
+#include <string.h>
+#include <stdlib.h>
+#include <cmd_proc.h>
+#include <cpu_def.h>
+#include <pxmc.h>
+#include "cmd_pxmc.h"
+#include <utils.h>
+
+/**
+ * cmd_opchar_getreg - selects the right axis
+ *
+ * pxmc is designed for multi axis motion control, so each axis must be identificated.
+ * This done by a capital letter. The first axis must be A, the 2nd B, etc.
+ */
+pxmc_state_t *cmd_opchar_getreg(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ unsigned chan;
+ pxmc_state_t *mcs;
+ chan=*param[1]-'A';
+ if(chan>=pxmc_main_list.pxml_cnt) return NULL;
+ mcs=pxmc_main_list.pxml_arr[chan];
+ if(!mcs) return NULL;
+ return mcs;
+}
+
+/**
+ * cmd_do_reg_go - checks the command format validity and calls pxmc_go.
+ *
+ * if pxmc_go returns -1, cmd_do_reg_go returns -1.
+ */
+int cmd_do_reg_go(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ long val;
+ pxmc_state_t *mcs;
+ if(*param[2]!=':') return -CMDERR_OPCHAR;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ val=atol(param[3])<<PXMC_SUBDIV(mcs);
+ val=pxmc_go(mcs,val,0,0);
+ if(val<0)
+ return val;
+ return 0;
+}
+
+
+/**
+ * cmd_do_pwm - checks the command format validity and calls pxmc_set_const_out.
+ *
+ */
+int cmd_do_pwm(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ int val;
+ pxmc_state_t *mcs;
+ if(*param[2]!=':') return -CMDERR_OPCHAR;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ val=atoi(param[3]);
+ pxmc_set_const_out(mcs,val);
+ return 0;
+}
+
+/**
+ * cmd_do_reg_hh - checks the command format validity and calls pxmc_hh (home hardware).
+ *
+ * if pxmc_hh returns -1, cmd_do_reg_hh returns -1.
+ */
+int cmd_do_reg_hh(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ long val;
+ pxmc_state_t *mcs;
+ if(*param[2]!=':') return -CMDERR_OPCHAR;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ val=pxmc_hh(mcs);
+ if(val<0)
+ return val;
+ return 0;
+}
+
+
+/**
+ * cmd_do_reg_spd - checks the command format validity and calls pxmc_spd.
+ *
+ * if pxmc_spd returns -1, cmd_do_reg_spd returns -1.
+ */
+int cmd_do_reg_spd(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ long val;
+ pxmc_state_t *mcs;
+ if(*param[2]!=':') return -CMDERR_OPCHAR;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ val=atol(param[3]);
+ val=pxmc_spd(mcs,val,0);
+ if(val<0)
+ return val;
+ return 0;
+}
+
+/**
+ * cmd_do_reg_spdfg - checks the command format validity and calls pxmc_spdfg.
+ *
+ * if pxmc_spdfg returns -1, cmd_do_reg_spdfg returns -1.
+ */
+int cmd_do_reg_spdfg(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ long val;
+ pxmc_state_t *mcs;
+ if(*param[2]!=':') return -CMDERR_OPCHAR;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ val=atol(param[3]);
+ val=pxmc_spdfg(mcs,val,0);
+ if(val<0)
+ return val;
+ return 0;
+}
+
+/**
+ * cmd_do_stop - checks the command format validity and calls pxmc_stop.
+ *
+ */
+int cmd_do_stop(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_state_t *mcs;
+ if(*param[2]!=':') return -CMDERR_OPCHAR;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ pxmc_stop(mcs,0);
+ return 0;
+}
+
+/**
+ * cmd_do_release - checks the command format validity and calls pxmc_set_const_out(mcs,0).
+ *
+ */
+int cmd_do_release(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_state_t *mcs;
+ if(*param[2]!=':') return -CMDERR_OPCHAR;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ pxmc_set_const_out(mcs,0);
+ return 0;
+}
+
+/**
+ * cmd_do_clrerr - checks the command format validity, clears the error flag.
+ *
+ * it also stop the rotation calling pxmc_set_const_out(mcs,0)
+ */
+int cmd_do_clrerr(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_state_t *mcs;
+ if(*param[2]!=':') return -CMDERR_OPCHAR;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ pxmc_set_const_out(mcs,0);
+ pxmc_clear_flag(mcs,PXMS_ERR_b);
+ return 0;
+}
+
+/**
+ * cmd_do_zero - checks the command format validity, sets axis position to 0.
+ *
+ * it also stop the rotation calling pxmc_set_const_out(mcs,0)
+ */
+int cmd_do_zero(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_state_t *mcs;
+ if(*param[2]!=':') return -CMDERR_OPCHAR;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ pxmc_set_const_out(mcs,0);
+ pxmc_axis_set_pos(mcs,0);
+ return 0;
+}
+
+/**
+ * cmd_do_align - checks the command format validity, sets offset between table and IRC counter to 0 .
+ *
+ * it also stop the rotation calling pxmc_set_const_out(mcs,0) and sets axis position to 0.
+ */
+int cmd_do_align(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_state_t *mcs;
+ if(*param[2]!=':') return -CMDERR_OPCHAR;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ pxmc_set_const_out(mcs,0);
+ pxmc_axis_set_pos(mcs,0);
+ mcs->pxms_ptofs=0;
+ return 0;
+}
+
+/**
+ * cmd_do_reg_rw_pos - read or write function, param is converted in 'long' and shifted
+ *
+ * if the command typed is a write function, records the value,
+ * if it is a read function returns the value asked.
+ */
+
+int cmd_do_reg_rw_pos(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ long val;
+ long *ptr;
+ int opchar;
+ pxmc_state_t *mcs;
+
+ if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ ptr=(long*)((long)des->info[0]+(char*)mcs);
+ if(opchar==':'){
+ val=atol(param[3]);
+ *ptr=val<<PXMC_SUBDIV(mcs);
+ }else{
+ return cmd_opchar_replong(cmd_io, param, (*ptr)>>PXMC_SUBDIV(mcs), 0, 0);
+ }
+ return 0;
+}
+
+/**
+ * cmd_do_reg_short_val - read or write function, param is converted in 'integer'
+ *
+ * if the command typed is a write function, records the value,
+ * if it is a read function returns the value asked.
+ */
+
+int cmd_do_reg_short_val(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ int val;
+ short *ptr;
+ int opchar;
+ pxmc_state_t *mcs;
+
+ if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ ptr=(short*)((long)des->info[0]+(char*)mcs);
+ if(opchar==':'){
+ val=atoi(param[3]);
+ *ptr=val;
+ }else{
+ return cmd_opchar_replong(cmd_io, param, (long)*ptr, 0, 0);
+ }
+ return 0;
+}
+
+/**
+ * cmd_do_reg_long_val - read or write function, param is converted in 'long'
+ *
+ * if the command typed is a write function, records the value,
+ * if it is a read function returns the value asked.
+ */
+int cmd_do_reg_long_val(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ long val;
+ long *ptr;
+ int opchar;
+ pxmc_state_t *mcs;
+
+ if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ ptr=(long*)((long)des->info[0]+(char*)mcs);
+ if(opchar==':'){
+ val=atol(param[3]);
+ *ptr=val;
+ }else{
+ return cmd_opchar_replong(cmd_io, param, (long)*ptr, 0, 0);
+ }
+ return 0;
+}
+
+
+/**
+ * cmd_do_axis_mode - checks the command format and busy flag validity, calls pxmc_axis_mode
+ *
+ * if pxmc_axis_mode returns -1, cmd_do_axis_mode returns -1.
+ */
+int cmd_do_axis_mode(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ int val;
+ pxmc_state_t *mcs;
+ if(*param[2]!=':') return -CMDERR_OPCHAR;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ if(mcs->pxms_flg&PXMS_BSY_m) return -CMDERR_BSYREG;
+ val=atol(param[3]);
+ //val=pxmc_axis_mode(mcs,val);
+ if(val<0)
+ return val;
+ return 0;
+}
+
+
+int cmd_do_regptmod_short_val(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ int val;
+ short *ptr;
+ int opchar;
+ pxmc_state_t *mcs;
+
+ if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ ptr=(short*)((long)des->info[0]+(char*)mcs);
+ if(opchar==':'){
+ if(mcs->pxms_flg&PXMS_BSY_m) return -CMDERR_BSYREG;
+ pxmc_set_const_out(mcs,0);
+ val=atoi(param[3]);
+ if((val<((long)des->info[1])) || (val>((long)des->info[2])))
+ return -CMDERR_BADPAR;
+ *ptr=val;
+ //val=pxmc_axis_mode(mcs,0);
+ if(val<0)
+ return val;
+ }else{
+ return cmd_opchar_replong(cmd_io, param, (long)*ptr, 0, 0);
+ }
+ return 0;
+}
+
+
+/**
+ * cmd_do_reg_type - no code written, will set controller structure
+ */
+int cmd_do_reg_type(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ /* should set controller structure */
+ return 0;
+}
+
+
+/**
+ * cmd_do_regsfrq - sets or returns smapling frequency
+ */
+#ifdef WITH_SFI_SEL
+int cmd_do_regsfrq(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ int val;
+ int opchar;
+
+ if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
+ if(opchar==':'){
+ val=atoi(param[3]);
+ return pxmc_sfi_sel(val);
+ }else{
+ return cmd_opchar_replong(cmd_io, param, pxmc_get_sfi_hz(NULL), 0, 0);
+ }
+ return 0;
+}
+#endif /* WITH_SFI_SEL */
+
+
+/* debugging functions */
+int cmd_do_reg_dbgset(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_state_t *mcs;
+ int val;
+ int opchar;
+
+ if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
+ if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
+ if(opchar==':'){
+ val=atoi(param[3]);
+ pxmc_dbgset(mcs,pxmc_dbg_ene_as,val);
+ }else{
+ cmd_io_write(cmd_io,param[0],param[2]-param[0]);
+ cmd_io_putc(cmd_io,'=');
+ cmd_io_putc(cmd_io,mcs->pxms_flg&PXMS_DBG_m?'1':'0');
+ }
+ return 0;
+}
+
+
+int cmd_do_reg_dbgpre(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_dbg_hist_t *hist;
+ long count, val;
+ int i;
+ int opchar;
+ char *ps;
+
+ if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
+ ps=param[3];
+ if(si_long(&ps,&count,0)<0) return -CMDERR_BADPAR;
+ if(!count||(count>0x10000)) return -CMDERR_BADPAR;
+ pxmc_dbg_histfree(NULL);
+ if((hist=pxmc_dbg_histalloc(count+2))==NULL) return -CMDERR_NOMEM;
+ for(i=0;i<count;i++){
+ /* ps=cmd_rs232_rdline(cmd_io,1); */ /* !!!!!!!!! */
+ if(si_long(&ps,&val,0)<0) return -CMDERR_BADPAR;
+ hist->buff[i]=val;
+ }
+ pxmc_dbg_hist=hist;
+ return 0;
+}
+
+
+int cmd_do_reg_dbghis(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_dbg_hist_t *hist;
+ long count, val;
+ int i, opchar;
+ char *ps;
+
+ if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
+ ps=param[3];
+ if(si_long(&ps,&count,0)<0) return -CMDERR_BADPAR;
+ if(!count||(count>0x10000)) return -CMDERR_BADPAR;
+ hist=pxmc_dbg_hist;
+ for(i=0;i<count;i++){
+ if(hist&&(&hist->buff[i]<hist->end))
+ val=hist->buff[i];
+ else
+ val=0;
+ printf("%ld\n",val); /* !!!!!!!! */
+ }
+ return 0;
+}
+
+
+int cmd_do_reg_dbggnr(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+ pxmc_state_t *mcs;
+ int i;
+ int opchar;
+
+ if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
+ for(i=0;i<pxmc_main_list.pxml_cnt;i++){
+ mcs=pxmc_main_list.pxml_arr[i];
+ if(mcs&&(mcs->pxms_flg&PXMS_DBG_m)){
+ if(pxmc_dbg_gnr(mcs)<0){
+ for(i=0;i<pxmc_main_list.pxml_cnt;i++){
+ mcs=pxmc_main_list.pxml_arr[i];
+ if(mcs&&(mcs->pxms_flg&PXMS_DBG_m))
+ pxmc_stop(pxmc_main_list.pxml_arr[i],0);
+ }
+ return -CMDERR_BADREG;
+ }
+ }
+ }
+ pxmc_dbg_hist->ptr=pxmc_dbg_hist->buff;
+ return 0;
+}
+
+
+/* motor executable commands */
+cmd_des_t const cmd_des_go={0, CDESM_OPCHR|CDESM_RW,
+ "G?","go to target position",cmd_do_reg_go,{}};
+cmd_des_t const cmd_des_pwm={0, CDESM_OPCHR|CDESM_RW,
+ "PWM?","direct axis PWM output",cmd_do_pwm,{}};
+cmd_des_t const cmd_des_hh={0, CDESM_OPCHR,"HH?","hard home request for axis",cmd_do_reg_hh,{}};
+cmd_des_t const cmd_des_spd={0, CDESM_OPCHR,"SPD?","speed request for axis",cmd_do_reg_spd,{}};
+cmd_des_t const cmd_des_spdfg={0, CDESM_OPCHR,"SPDFG?",
+ "fine grained speed request for axis",cmd_do_reg_spdfg,{}};
+cmd_des_t const cmd_des_stop={0, CDESM_OPCHR,
+ "STOP?","stop motion of requested axis",cmd_do_stop,{}};
+cmd_des_t const cmd_des_release={0, CDESM_OPCHR,
+ "RELEASE?","releases axis closed loop control",cmd_do_release,{}};
+cmd_des_t const cmd_des_zero={0, CDESM_OPCHR,
+ "ZERO?","zero actual position",cmd_do_zero,{}};
+cmd_des_t const cmd_des_align={0, CDESM_OPCHR,
+ "ALIGN?","align commutator",cmd_do_align,{}};
+cmd_des_t const cmd_des_clrerr={0, CDESM_OPCHR,
+ "PURGE?"," clear 'axis in error state' flag",cmd_do_clrerr,{}};
+/* motors and controllers variables */
+cmd_des_t const cmd_des_ap={0, CDESM_OPCHR|CDESM_RD,
+ "AP?","actual position",cmd_do_reg_rw_pos,
+ {(char*)pxmc_state_offs(pxms_ap),
+ 0}};
+cmd_des_t const cmd_des_st={0, CDESM_OPCHR|CDESM_RD,
+ "ST?","axis status bits encoded in number",cmd_do_reg_short_val,
+ {(char*)pxmc_state_offs(pxms_flg),
+ 0}};
+cmd_des_t const cmd_des_axerr={0, CDESM_OPCHR|CDESM_RD,
+ "AXERR?","last axis error code",cmd_do_reg_short_val,
+ {(char*)pxmc_state_offs(pxms_errno),
+ 0}};
+#if 0
+cmd_des_t const cmd_des_r_one={0, CDESM_OPCHR,
+ "R?","send R?! or FAIL?! at axis finish",cmd_do_r_one,{}};
+#endif
+cmd_des_t const cmd_des_regp={0, CDESM_OPCHR|CDESM_RW,
+ "REGP?","controller proportional gain",cmd_do_reg_short_val,
+ {(char*)pxmc_state_offs(pxms_p),
+ 0}};
+cmd_des_t const cmd_des_regi={0, CDESM_OPCHR|CDESM_RW,
+ "REGI?","controller integral gain",cmd_do_reg_short_val,
+ {(char*)pxmc_state_offs(pxms_i),
+ 0}};
+cmd_des_t const cmd_des_regd={0, CDESM_OPCHR|CDESM_RW,
+ "REGD?","controller derivative gain",cmd_do_reg_short_val,
+ {(char*)pxmc_state_offs(pxms_d),
+ 0}};
+cmd_des_t const cmd_des_regs1={0, CDESM_OPCHR|CDESM_RW,
+ "REGS1?","controller S1",cmd_do_reg_short_val,
+ {(char*)pxmc_state_offs(pxms_s1),
+ 0}};
+cmd_des_t const cmd_des_regs2={0, CDESM_OPCHR|CDESM_RW,
+ "REGS2?","controller S2",cmd_do_reg_short_val,
+ {(char*)pxmc_state_offs(pxms_s2),
+ 0}};
+cmd_des_t const cmd_des_regmd={0, CDESM_OPCHR|CDESM_RW,
+ "REGMD?","maximal allowed position error",cmd_do_reg_rw_pos,
+ {(char*)pxmc_state_offs(pxms_md),
+ 0}};
+cmd_des_t const cmd_des_regms={0, CDESM_OPCHR|CDESM_RW,
+ "REGMS?","maximal speed",cmd_do_reg_long_val,
+ {(char*)pxmc_state_offs(pxms_ms),
+ 0}};
+cmd_des_t const cmd_des_regacc={0, CDESM_OPCHR|CDESM_RW,
+ "REGACC?","maximal acceleration",cmd_do_reg_long_val,
+ {(char*)pxmc_state_offs(pxms_ma),
+ 0}};
+cmd_des_t const cmd_des_regme={0, CDESM_OPCHR|CDESM_RW,
+ "REGME?","maximal PWM energy or voltage for axis",cmd_do_reg_short_val,
+ {(char*)pxmc_state_offs(pxms_me),
+ 0}};
+cmd_des_t const cmd_des_regcfg={0, CDESM_OPCHR|CDESM_RW,
+ "REGCFG?","hard home and profile configuration",cmd_do_reg_short_val,
+ {(char*)pxmc_state_offs(pxms_cfg),
+ 0}};
+cmd_des_t const cmd_des_ptirc={0, CDESM_OPCHR|CDESM_RW,
+ "REGPTIRC?","number of irc pulses per phase table",cmd_do_regptmod_short_val,
+ {(char*)pxmc_state_offs(pxms_ptirc),
+ (char*)4,(char*)10000}};
+cmd_des_t const cmd_des_ptper={0, CDESM_OPCHR|CDESM_RW,
+ "REGPTPER?","number of elmag. revolutions per phase table",cmd_do_regptmod_short_val,
+ {(char*)pxmc_state_offs(pxms_ptper),
+ (char*)1,(char*)100}};
+cmd_des_t const cmd_des_ptshift={0, CDESM_OPCHR|CDESM_RW,
+ "REGPTSHIFT?","shift (in irc) of generated phase curves",cmd_do_reg_short_val,
+ {(char*)pxmc_state_offs(pxms_ptshift),
+ 0}};
+cmd_des_t const cmd_des_ptvang={0, CDESM_OPCHR|CDESM_RW,
+ "REGPTVANG?","angle (in irc) between rotor and stator mag. fld.",cmd_do_reg_short_val,
+ {(char*)pxmc_state_offs(pxms_ptvang),
+ 0}};
+cmd_des_t const cmd_des_pwm1cor={0, CDESM_OPCHR|CDESM_RW,
+ "REGPWM1COR?","PWM1 correction",cmd_do_reg_short_val,
+ {(char*)pxmc_state_offs(pxms_pwm1cor),
+ 0}};
+cmd_des_t const cmd_des_pwm2cor={0, CDESM_OPCHR|CDESM_RW,
+ "REGPWM2COR?","PWM2 correction",cmd_do_reg_short_val,
+ {(char*)pxmc_state_offs(pxms_pwm2cor),
+ 0}};
+cmd_des_t const cmd_des_axis_mode={0, CDESM_OPCHR|CDESM_WR,
+ "REGMODE?","axis working mode",cmd_do_axis_mode,
+ {}};
+#ifdef WITH_SFI_SEL
+cmd_des_t const cmd_des_regsfrq={0, CDESM_OPCHR|CDESM_RW,
+ "REGSFRQ","set new sampling frequency",cmd_do_regsfrq,{}};
+#endif /* WITH_SFI_SEL */
+/* axes debugging and tuning */
+cmd_des_t const cmd_des_reg_type={0, CDESM_OPCHR|CDESM_RW,
+ "REGTYPE?","unused",cmd_do_reg_type,{}};
+cmd_des_t const cmd_des_reg_dbgset={0, CDESM_OPCHR|CDESM_RW,
+ "REGDBG?","sets debug flag",cmd_do_reg_dbgset,{}};
+cmd_des_t const cmd_des_reg_dbgpre={0, CDESM_OPCHR|CDESM_WR,
+ "REGDBGPRE","store reference course",cmd_do_reg_dbgpre,{}};
+cmd_des_t const cmd_des_reg_dbghis={0, CDESM_OPCHR|CDESM_WR,
+ "REGDBGHIS","read history course",cmd_do_reg_dbghis,{}};
+cmd_des_t const cmd_des_reg_dbggnr={0, CDESM_OPCHR|CDESM_WR,
+ "REGDBGGNR","controller response to HIST course",cmd_do_reg_dbggnr,{}};
+
+cmd_des_t const *cmd_stm_default[]={
+ &cmd_des_go,
+ &cmd_des_pwm,
+ /*&cmd_des_hh,*/
+ &cmd_des_spd,
+ &cmd_des_spdfg,
+ &cmd_des_stop,
+ &cmd_des_release,
+ &cmd_des_zero,
+ &cmd_des_align,
+ &cmd_des_clrerr,
+ &cmd_des_ap,
+ &cmd_des_st,
+ &cmd_des_axerr,
+ /*&cmd_des_r_one,*/
+ &cmd_des_regp,
+ &cmd_des_regi,
+ &cmd_des_regd,
+ &cmd_des_regs1,
+ &cmd_des_regs2,
+ &cmd_des_regmd,
+ &cmd_des_regms,
+ &cmd_des_regacc,
+ &cmd_des_regme,
+ &cmd_des_regcfg,
+ &cmd_des_ptirc,
+ &cmd_des_ptper,
+ &cmd_des_ptshift,
+ &cmd_des_ptvang,
+ &cmd_des_pwm1cor,
+ &cmd_des_pwm2cor,
+ &cmd_des_axis_mode,
+ &cmd_des_reg_type,
+#ifdef WITH_SFI_SEL
+ &cmd_des_regsfrq,
+#endif /* WITH_SFI_SEL */
+ &cmd_des_reg_dbgset,
+ &cmd_des_reg_dbgpre,
+ &cmd_des_reg_dbghis,
+ &cmd_des_reg_dbggnr,
+ NULL
+};
projects / eurobot / public.git / commitdiff