From 3724f24eb6f2adb9c92b84080ac073346c747da8 Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Mon, 13 Nov 2006 08:55:00 +0000 Subject: [PATCH] Added simple autonomous demo. Robot follows preprogrammed trajectory. darcs-hash:20061113085556-f2ef6-0583521a914d3fc2f99e08d58f28797e17627b79.gz --- autodemo/.emacs-dirvars | 11 + autodemo/Makefile | 14 + autodemo/Makefile.omk | 8 + autodemo/cmd_bth.h | 16 + autodemo/cmd_pxmc.c | 610 ++++++++++++++++++++++++++++++++++++ autodemo/cmd_pxmc.h | 18 ++ autodemo/mirosot_autodemo.c | 248 +++++++++++++++ autodemo/timer3.c | 35 +++ autodemo/timer3.h | 7 + 9 files changed, 967 insertions(+) create mode 100644 autodemo/.emacs-dirvars create mode 100644 autodemo/Makefile create mode 100644 autodemo/Makefile.omk create mode 100644 autodemo/cmd_bth.h create mode 100644 autodemo/cmd_pxmc.c create mode 100644 autodemo/cmd_pxmc.h create mode 100644 autodemo/mirosot_autodemo.c create mode 100644 autodemo/timer3.c create mode 100644 autodemo/timer3.h diff --git a/autodemo/.emacs-dirvars b/autodemo/.emacs-dirvars new file mode 100644 index 0000000..346c0d8 --- /dev/null +++ b/autodemo/.emacs-dirvars @@ -0,0 +1,11 @@ +;; -*- 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 + diff --git a/autodemo/Makefile b/autodemo/Makefile new file mode 100644 index 0000000..f595272 --- /dev/null +++ b/autodemo/Makefile @@ -0,0 +1,14 @@ +# 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 + diff --git a/autodemo/Makefile.omk b/autodemo/Makefile.omk new file mode 100644 index 0000000..7b09113 --- /dev/null +++ b/autodemo/Makefile.omk @@ -0,0 +1,8 @@ +# -*- makefile -*- + +bin_PROGRAMS = miroautodemo + +miroautodemo_SOURCES = mirosot_autodemo.c cmd_pxmc.c timer3.c + +miroautodemo_LIBS = boot_fn arch_drivers excptvec misc pxmc m sci_channels +miroautodemo_MOREOBJS = $(USER_LIB_DIR)/system_stub.o diff --git a/autodemo/cmd_bth.h b/autodemo/cmd_bth.h new file mode 100644 index 0000000..fc45735 --- /dev/null +++ b/autodemo/cmd_bth.h @@ -0,0 +1,16 @@ +#ifndef CMD_BTH_H +#define CMD_BTH_H + +/* Bluetooth specific */ + +cmd_des_t const **cmd_bth; + +cmd_io_t cmd_io_bth; + +int cmd_bth_line_out(cmd_io_t *cmd_io); + +int cmd_bth_line_in(cmd_io_t *cmd_io); + +char *cmd_bth_rdline(cmd_io_t *cmd_io, int mode); + +#endif diff --git a/autodemo/cmd_pxmc.c b/autodemo/cmd_pxmc.c new file mode 100644 index 0000000..4bdc4f8 --- /dev/null +++ b/autodemo/cmd_pxmc.c @@ -0,0 +1,610 @@ +/******************************************************************* + Components for embedded applications builded for + laboratory and medical instruments firmware + + cmd_pxmc.c - interconnection of PXMC library + 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 +#include +#include +#include +#include +#include +#include +#include + + +/** + * 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])<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), 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;ibuff[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;ibuff[i]end)) + val=hist->buff[i]; + else + val=0; + /* printf("%ld\r\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;ipxms_flg&PXMS_DBG_m)){ + if(pxmc_dbg_gnr(mcs)<0){ + for(i=0;ipxms_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_pxmc_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 +}; diff --git a/autodemo/cmd_pxmc.h b/autodemo/cmd_pxmc.h new file mode 100644 index 0000000..ccd280f --- /dev/null +++ b/autodemo/cmd_pxmc.h @@ -0,0 +1,18 @@ +// +// C++ Interface: cmd_pxmc +// +// Description: +// +// +// Author: Petr Kovacik , (C) 2005 +// +// Copyright: See COPYING file that comes with this distribution +// +// + +extern cmd_des_t const *cmd_pxmc_default[]; + +//void +//CmdGetParam(pxmc_state_list_t *ArrayStructurMotors); + + diff --git a/autodemo/mirosot_autodemo.c b/autodemo/mirosot_autodemo.c new file mode 100644 index 0000000..44942b9 --- /dev/null +++ b/autodemo/mirosot_autodemo.c @@ -0,0 +1,248 @@ +/* +* C Implementation: main_test +* +* Description: + +* +* +* Author: Petr Kovacik , (C) 2006 +* +* Copyright: +* +*/ +#define _USE_EXR_LEVELS 1 +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include "cmd_pxmc.h" + + +/*struktury prikazu cmd*/ +cmd_des_t const cmd_des_help={0, 0,"HELP","prints help for commands", + cmd_do_help,{(char*)&cmd_rs232}}; + +cmd_des_t const *cmd_rs232_default[]={ + + &cmd_des_help, + CMD_DES_CONTINUE_AT, /* list continues at new address */ + (cmd_des_t*)cmd_pxmc_default, + NULL +}; +cmd_des_t const **cmd_bth=cmd_rs232_default; /*cmd prikazy pro bth*/ +cmd_des_t const **cmd_rs232=cmd_rs232_default; /*cmd prikazy pro PC*/ + + +/*struktury charakterizujici motor 0*/ +pxmc_state_t mcsX0={ + pxms_flg:0, + pxms_do_inp:0, + pxms_do_con:0, + pxms_do_out:0, + pxms_do_deb:0, + pxms_do_gen:0, + pxms_ap:0, pxms_as:0, + pxms_rp:155l*256, + pxms_rs:0, //pxms_subdiv:8, + pxms_md:8000l<<8, pxms_ms:5000, pxms_ma:10, + pxms_inp_info:(long)TPU_TCNT1,//TPU_TCNT1 /*chanel TPU A,B*/ + pxms_out_info:(long)PWM_PWBFR1A, /*chanel PWM A,B*/ + pxms_ene:0, pxms_erc:0, + pxms_p:40, pxms_i:0, pxms_d:1, pxms_s1:0, pxms_s2:0, + pxms_me:0x1800, //6144 + pxms_ptirc:40, // 2000 irc per rev, 200/4 steps / + pxms_ptper:1, + pxms_ptptr1:NULL, + pxms_ptptr2:NULL, + pxms_cfg:PXMS_CFG_MD2E_m|PXMS_CFG_HLS_m| + PXMS_CFG_HPS_m|PXMS_CFG_HDIR_m|0x1 +}; + + +/*struktury charakterizujici motor 1*/ +pxmc_state_t mcsX1={ + pxms_flg:0, + pxms_do_inp:0, + pxms_do_con:0, + pxms_do_out:0, + pxms_do_deb:0, + pxms_do_gen:0, + pxms_ap:0, pxms_as:0, + pxms_rp:155l*256, + pxms_rs:0, //pxms_subdiv:8, + pxms_md:8000l<<8, pxms_ms:5000, pxms_ma:10, + pxms_inp_info:(long)TPU_TCNT2, /*chanel TPU C,D*/ + pxms_out_info:(long)PWM_PWBFR1C, /*chanel PWM C,D*/ + pxms_ene:0, pxms_erc:0, + pxms_p:40, pxms_i:0, pxms_d:1, pxms_s1:0, pxms_s2:0, + pxms_me:0x1800, //6144 + pxms_ptirc:40, // 2000 irc per rev, 200/4 steps / + pxms_ptper:1, + pxms_ptptr1:NULL, + pxms_ptptr2:NULL, + pxms_cfg:PXMS_CFG_MD2E_m|PXMS_CFG_HLS_m| //FIXME: nastavit spravne priznaky pro dalsi motorove struktur + PXMS_CFG_HPS_m|PXMS_CFG_HDIR_m|0x1 +}; + +pxmc_state_t *pxmc_main_arr[] = {&mcsX0,&mcsX1}; + +pxmc_state_list_t pxmc_main_list = { + pxml_arr:pxmc_main_arr, + pxml_cnt:sizeof(pxmc_main_arr)/sizeof(pxmc_main_arr[0]) +}; + + + +//******************************************************* + +void unhandled_exception(void) __attribute__ ((interrupt_handler)); + +/*Interrupt routines*/ +void unhandled_exception(void) +{ +}; +//******************************************************** + + +int cmd_rs232_processor_run(void) +{ + int val; + cmd_io_t* cmd_io; + + cmd_io=&cmd_io_rs232; + if(cmd_rs232_line_out(cmd_io)) + return 1; + + if(cmd_rs232_line_in(cmd_io)<=0) + return 0; + + if(cmd_rs232){ + val=proc_cmd_line(cmd_io, cmd_rs232, cmd_io->priv.ed_line.in->buf); + }else{ + val=-CMDERR_BADCMD; + } + + if(cmd_io->priv.ed_line.out->inbuf){ + cmd_io_putc(cmd_io,'\r'); + cmd_io_putc(cmd_io,'\n'); + + }else if(val<0){ + char s[20]; + cmd_io_write(&cmd_io_rs232,"ERROR ",6); + i2str(s,-val,0,0); + cmd_io_write(cmd_io,s,strlen(s)); + cmd_io_putc(cmd_io,'\r'); + cmd_io_putc(cmd_io,'\n'); + } + return 1; +} + +extern void _print(char *str); + + +int main() +{ + + /********************************************************************************/ + *DIO_PJDDR=0xff; /*output gate*/ + *DIO_PEDDR=0xff; /*output gate*/ + *DIO_PEDR=0x60; /*0x0-LED - light all; 0x6 -ENA,ENB=1, LE33CD=0*/ + *DIO_PJDR=0x00; //rozsviceni vsech diod na */ + + + /*priority preruseni - SCI > TPU*/ +/* *SYS_SYSCR|=SYSCR_INTM1m; */ +/* *INT_IPRA=0x22; *INT_IPRB=0x22; *INT_IPRC=0x04; */ +/* *INT_IPRD=0x40; *INT_IPRE=0x44; *INT_IPRF=0x55; */ +/* *INT_IPRG=0x55; *INT_IPRH=0x55; *INT_IPRJ=0x06; */ +/* *INT_IPRK=0x67; *INT_IPRM=0x66; */ + + /*povoleni vsech preruseni atd...*/ + cli(); + excptvec_initfill(unhandled_exception, 0); + + /*nastaveni seriovych linek - Bth, PC*/ + //sci_rs232_setmode(RS232_BAUD_RAW | 3, 0, 0, 2); // HCI - hardcoded 115200 + //sci_rs232_setmode(115200, 0, 0, 2); // HCI + sci_rs232_setmode(19200, 0, 0, sci_rs232_chan_default); //PC + sti(); + + _print("Start\n"); + + /*nastaveni HW (TPU, PWM)*/ + pxmc_set_pwm_tpu(); + + /*nastaveni DC motoru*/ + pxmc_add_pservice_and_mode(4); /*Macro - mod=4 tj. all motors are DC*/ + + /*nekonecna smycka obsluhujici bth, pc ...*/ +/* do{ */ +/* cmd_rs232_processor_run(); /\*sber + odesilani cmd prikazu PC*\/ */ +/* }while(1); */ +#include "timer3.h" + + init_timer3(); + + long int before = get_timer(); + int a=1; + while (1) { + cmd_rs232_processor_run(); /*sber + odesilani cmd prikazu PC*/ + + long int now = get_timer(); + + if (now != before) + switch (now % 10) { + case 0: + pxmc_spd(&mcsX0,a*400,0); + pxmc_spd(&mcsX1,-a*700,0); + break; + case 1: + pxmc_spd(&mcsX0,a*700,0); + pxmc_spd(&mcsX1,-a*400,0); + break; + case 2: + pxmc_spd(&mcsX0,a*2000,0); + pxmc_spd(&mcsX1,-a*1200,0); + break; + case 3: + pxmc_spd(&mcsX0,a*1200,0); + pxmc_spd(&mcsX1,-a*2000,0); + break; + case 4: + pxmc_spd(&mcsX0,a*1300,0); + pxmc_spd(&mcsX1,-a*1000,0); + break; + case 5: + pxmc_spd(&mcsX0,a*1300,0); + pxmc_spd(&mcsX1,-a*1000,0); + break; + case 6: + pxmc_spd(&mcsX0,a*100,0); + pxmc_spd(&mcsX1,-a*300,0); + break; + case 7: + pxmc_spd(&mcsX0,a*300,0); + pxmc_spd(&mcsX1,-a*100,0); + break; + case 8: + pxmc_spd(&mcsX0,a*2000,0); + pxmc_spd(&mcsX1,-a*100,0); + break; + case 9: + pxmc_spd(&mcsX0,a*4000,0); + pxmc_spd(&mcsX1,-a*4000,0); + if(a>0) a=-1; + else a=1; + break; + + } + } +} diff --git a/autodemo/timer3.c b/autodemo/timer3.c new file mode 100644 index 0000000..259a3ec --- /dev/null +++ b/autodemo/timer3.c @@ -0,0 +1,35 @@ +#include +#include +#include + +static volatile timer_t timer; + +static void timer_isr(void) __attribute__ ((interrupt_handler)); + +static void +timer_isr(void) +{ + timer++; + *TPU_TSR3 &= ~TSR2_TCFVm ; //reset overflow flag (clear interrupt) +} + +//timer initialisation +/*free running counter*/ +void init_timer3() +{ + *SYS_MSTPCRA &= ~MSTPCRA_TPUm; // power TPU unit + + *TPU_TCR3 =0x00 | 0x06; //rising edge, f divided by 256 + *TPU_TMDR3 =0x00; // normal mode + *TPU_TSR3 &= ~TSR3_TCFVm ; //reset overflow flag + *TPU_TIER3 |=TIER3_TCIEVm; //enable overflow interrupt + + *TPU_TSTR |=TSTR_CST3m; //start timer + + excptvec_set(52, timer_isr); +} + +timer_t get_timer() +{ + return timer; +} diff --git a/autodemo/timer3.h b/autodemo/timer3.h new file mode 100644 index 0000000..0bee22d --- /dev/null +++ b/autodemo/timer3.h @@ -0,0 +1,7 @@ + + +typedef long long timer_t; + +void init_timer3(); +timer_t get_timer(); + -- 2.39.2