From dde832db41b7598ef6dd7821c6ddf8843eb1b74e Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Wed, 28 Jun 2006 21:49:00 +0000 Subject: [PATCH] Added the correct final version of Petr's application. darcs-hash:20060628214915-f2ef6-fb866e59212a139ad75e9dcce1b5badb73340a1e.gz --- testapp/Makefile.omk | 19 +- testapp/cmd_bth.h | 16 ++ testapp/cmd_pxmc.c | 610 +++++++++++++++++++++++++++++++++++++++++ testapp/cmd_pxmc.h | 18 ++ testapp/main.c | 121 -------- testapp/mirosot_main.c | 349 +++++++++++++++++++++++ 6 files changed, 997 insertions(+), 136 deletions(-) create mode 100644 testapp/cmd_bth.h create mode 100644 testapp/cmd_pxmc.c create mode 100644 testapp/cmd_pxmc.h delete mode 100644 testapp/main.c create mode 100644 testapp/mirosot_main.c diff --git a/testapp/Makefile.omk b/testapp/Makefile.omk index b103163..be40280 100644 --- a/testapp/Makefile.omk +++ b/testapp/Makefile.omk @@ -1,19 +1,8 @@ # -*- makefile -*- -bin_PROGRAMS = bth_comp2 -#bth_comp1 +bin_PROGRAMS = Main +Main_SOURCES = mirosot_main.c cmd_pxmc.c -#bth_comp1_SOURCES = main.c bth_main.c bth_command.c bth_event_acc.c bth_error.c bth_cmd_complete_ev.c l2cap.c bth_inface.c h2638_pkt_controll.c -#bth_comp1_LIBS = boot_fn arch_drivers excptvec misc pxmc m sci_channels -#bth_comp1_MOREOBJS = $(USER_LIB_DIR)/system_stub.o - -bth_comp2_SOURCES = main.c -bth_comp2_LIBS = boot_fn arch_drivers excptvec misc pxmc m sci_channels bluetooth -bth_comp2_MOREOBJS = $(USER_LIB_DIR)/system_stub.o - -#lib_LIBRARIES = bluetooth - -#bluetooth_SOURCES = bth_command.c bth_event_acc.c bth_error.c bth_cmd_complete_ev.c l2cap.c bth_inface.c bth_main.c h2638_pkt_controll.c -#include_HEADERS = bth_inface.h bth_fce_out.h bth_h8s2638.h - +Main_LIBS = boot_fn arch_drivers excptvec misc pxmc m sci_channels bluetooth +Main_MOREOBJS = $(USER_LIB_DIR)/system_stub.o diff --git a/testapp/cmd_bth.h b/testapp/cmd_bth.h new file mode 100644 index 0000000..fc45735 --- /dev/null +++ b/testapp/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/testapp/cmd_pxmc.c b/testapp/cmd_pxmc.c new file mode 100644 index 0000000..9e0200c --- /dev/null +++ b/testapp/cmd_pxmc.c @@ -0,0 +1,610 @@ +/******************************************************************* + 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 +#include +#include +#include +#include +#include +#include +#include "cmd_pxmc.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])<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_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 +}; diff --git a/testapp/cmd_pxmc.h b/testapp/cmd_pxmc.h new file mode 100644 index 0000000..c6531f4 --- /dev/null +++ b/testapp/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_stm_default[]; + +//void +//CmdGetParam(pxmc_state_list_t *ArrayStructurMotors); + + diff --git a/testapp/main.c b/testapp/main.c deleted file mode 100644 index 8b2a98e..0000000 --- a/testapp/main.c +++ /dev/null @@ -1,121 +0,0 @@ -/******************************************************************* - bluetooth test aplication - bth chat - - bth_h8s2638.h - bth chat - write a recieve data to RS232 link - as recieve data include char is 'a' aplication - send 'ahoj' - - Copyright (C) 2006 by Petr Kovacik petr_kovacik@gmail.com - - *******************************************************************/ -#ifdef BTH_LX -#include -#include -#include -#include -#include -#include -#include -#include "bth_h8s2638.h" - - -#else -#include "types.h" -#include -#endif - -#include "bth_inface.h" -#include "bth_fce_out.h" - -#ifdef BTH_LX -//******************************************************* - -void unhandled_exception(void) __attribute__ ((interrupt_handler)); -/** - * init - shaddow registers, outputs.. - * - * Initializes P1 and P3 shaddow registers, - * sets PJ.1, PJ.2, PJ.3 LED as outputs, - * initialises interrupt vector. - */ -void init() -{ - /* initialises interrupt vector */ - excptvec_initfill(unhandled_exception, 0); -} - -/*Interrupt routines*/ -void unhandled_exception(void) -{ -}; -//***************************************#define _USE_EXR_LEVELS 1***************** -#endif - - - -int main() -{ - int zz,zn_bth; - - - /********************************************************************************/ -#ifdef BTH_LX -// *SYS_SYSCR|=(SYSCR_INTM1m); -// *INT_IPRH=0x34; -// *INT_IPRK=0x67; /*highest priority SCI 1 and 2*/ - - *DIO_PEDDR=0xff; /*output gate*/ - *DIO_PEDR=0x60; /*0x0-LED - light all; 0x6 -ENA,ENB=1, LE33CD=0*/ - - cli(); - init(); - sti(); - - sci_rs232_setmode(115200, 0, 0, 2); //bth - sci_rs232_setmode(19200, 0, 0, sci_rs232_chan_default); //PC - -#endif - bth_inface_setup(0); /*init I/O data buffer for bluetooth*/ - bth_init(); /*init bth device*/ - - - bth_init_pkt_controll(); - bth_parametr_slave(); /* set up bluetooth device to slave*/ - bth_start_TPU_counter(); - - do{ - zn_bth=sci_rs232_recch(2); - }while(bth_get_timer()<3); - bth_start(); - - /* - do{ - zn=sci_rs232_recch(sci_rs232_chan_default); - zn_bth=sci_rs232_recch(2); - }while (zn==-1); - */ - - - do{ - zn_bth=sci_rs232_recch(2); - if(zn_bth!=-1) - { - bth_conv_char_text(zn_bth); - bth_recieve_packet(zn_bth); - }; - - if((zz=bth_inface_recch(0))>-1) - { - if(zz=='a') - { - bth_inface_sendch('a',0); bth_inface_sendch('h',0); bth_inface_sendch('o',0); - bth_inface_sendch('j',0); bth_inface_sendch('\n',0); - }; - l2cap_send_data(0, 0); - sci_rs232_sendch(zz,sci_rs232_chan_default); - }; - bth_send_queue(); -// sci_rs232_sendch('.',sci_rs232_chan_default); - }while(1); - return 0; -}; diff --git a/testapp/mirosot_main.c b/testapp/mirosot_main.c new file mode 100644 index 0000000..662e701 --- /dev/null +++ b/testapp/mirosot_main.c @@ -0,0 +1,349 @@ +/* +* 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 + +#include +#include "cmd_bth.h" +#include "cmd_pxmc.h" +#include + + +/*struktury prikazu cmd*/ +cmd_des_t const cmd_des_help={0, 0,"HELP","prints help for commands", + cmd_do_help,{(char*)&cmd_bth}}; + +cmd_des_t const *cmd_rs232_default[]={ + + &cmd_des_help, + (cmd_des_t*)1, + (cmd_des_t*)cmd_stm_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}; + +#define SUMMOTORS (sizeof(pxmc_main_arr)/sizeof(pxmc_main_arr[0])) + +pxmc_state_list_t pxmc_main_list = { + pxml_arr:pxmc_main_arr, + pxml_cnt:SUMMOTORS +}; + + + +//******************************************************* + +void unhandled_exception(void) __attribute__ ((interrupt_handler)); +/** + * init - shaddow registers, outputs.. + * + * Initializes P1 and P3 shaddow registers, + * sets PJ.1, PJ.2, PJ.3 LED as outputs, + * initialises interrupt vector. + */ +void init() +{ + /* initialises interrupt vector */ + excptvec_initfill(unhandled_exception, 0); +} + +/*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; +} + + +int cmd_bth_processor_run(void) +{ + int val; + cmd_io_t* cmd_io; + + cmd_io=&cmd_io_bth; + if(cmd_bth_line_out(cmd_io)) + return 1; + + if(cmd_bth_line_in(cmd_io)<=0) + return 0; + + if(cmd_bth){ + val=proc_cmd_line(cmd_io, cmd_bth, 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_bth,"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; +} + + + +int main() +{ + int zn_bth; + + /********************************************************************************/ + *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(); + init(); + + /*nastaveni seriovych linek - Bth, PC*/ + sci_rs232_setmode(115200, 0, 0, 2); //bth + sci_rs232_setmode(19200, 0, 0, sci_rs232_chan_default); //PC + sti(); + + /*inicializace bluetooth*/ + bth_init(); + + /*inicializace komunikacnich datovych I/O bufferu */ + bth_inface_setup(0); + + /*TPU kanal 1 - inicialize (kontrola paketu + pocatecni zpozdeni)*/ + bth_init_pkt_controll(); + + /*nastaveni zarizeni Bth do slave role*/ + bth_parametr_slave(); + + /*kratka cas. pouza pro bth zarizeni - mazani zasilanych dat*/ + +// bth_start_TPU_counter(); + do + { + zn_bth=sci_rs232_recch(2); + }while(bth_get_timer()<11); + bth_start(); + + /*nastaveni HW (TPU, PWM)*/ + pxmc_set_pwm_tpu(SUMMOTORS); + + /*nastaveni DC motoru*/ + pxmc_add_pservice_and_mode(4); /*Macro - mod=4 tj. all motors are DC*/ + + /*nekonecna smycka obsluhujici bth, pc ...*/ + do{ + zn_bth=sci_rs232_recch(2); + if(zn_bth!=-1) + { + bth_recieve_packet(zn_bth); + }; + + l2cap_send_data(0, 0); + bth_send_queue(); /*odesli sestavene pakety, pokod jsou*/ + cmd_bth_processor_run(); /*sber + odesilani cmd prikazu bth*/ + cmd_rs232_processor_run(); /*sber + odesilani cmd prikazu PC*/ + + }while(1); + return 0; +}; + + + + + + + + + + + + + + + + + + + +#if 0 //male pohybove demo - pohyb robota +//#include "timer.h" + +// init_timer3(); + +// long int before = -1; +// int a=1; + do{ +/* 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; + + } + */ +#endif -- 2.39.2