From b48ca00f4a1d4238af4da33faf9c55d199131ef3 Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Fri, 27 Jun 2008 12:29:00 +0000 Subject: [PATCH] Added sources of demo application by Milan Navratil It uses either IR distance sensor to move the robot while avoiding obstacles or it can follow a black line thanks to reflection sensors. darcs-hash:20080627122935-f2ef6-bef3421c743ae0c0af56608d9732a015c480322b.gz --- navratil_ad/Makefile | 14 + navratil_ad/Makefile.omk | 8 + navratil_ad/cmd_pxmc.c | 610 ++++++++++++++++++++++++++++++++++++++ navratil_ad/cmd_pxmc.h | 18 ++ navratil_ad/navratil_ad.c | 489 ++++++++++++++++++++++++++++++ navratil_ad/timer3.c | 57 ++++ navratil_ad/timer3.h | 7 + 7 files changed, 1203 insertions(+) create mode 100644 navratil_ad/Makefile create mode 100644 navratil_ad/Makefile.omk create mode 100644 navratil_ad/cmd_pxmc.c create mode 100644 navratil_ad/cmd_pxmc.h create mode 100644 navratil_ad/navratil_ad.c create mode 100644 navratil_ad/timer3.c create mode 100644 navratil_ad/timer3.h diff --git a/navratil_ad/Makefile b/navratil_ad/Makefile new file mode 100644 index 0000000..f595272 --- /dev/null +++ b/navratil_ad/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/navratil_ad/Makefile.omk b/navratil_ad/Makefile.omk new file mode 100644 index 0000000..6b04754 --- /dev/null +++ b/navratil_ad/Makefile.omk @@ -0,0 +1,8 @@ +# -*- makefile -*- + +bin_PROGRAMS = navratilad + +navratilad_SOURCES = navratil_ad.c cmd_pxmc.c timer3.c + +navratilad_LIBS = boot_fn arch_drivers excptvec misc pxmcbsp pxmc m sci_channels cmdproc cmdprocio +navratilad_MOREOBJS = $(USER_LIB_DIR)/system_stub.o diff --git a/navratil_ad/cmd_pxmc.c b/navratil_ad/cmd_pxmc.c new file mode 100644 index 0000000..4bdc4f8 --- /dev/null +++ b/navratil_ad/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/navratil_ad/cmd_pxmc.h b/navratil_ad/cmd_pxmc.h new file mode 100644 index 0000000..ccd280f --- /dev/null +++ b/navratil_ad/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/navratil_ad/navratil_ad.c b/navratil_ad/navratil_ad.c new file mode 100644 index 0000000..1606c5d --- /dev/null +++ b/navratil_ad/navratil_ad.c @@ -0,0 +1,489 @@ +/* +* +* Author funkci na pohyb motoru: Petr Kovacik , (C) 2006 +* +* Autor funkci na autonomni pohyb: Milan Navratil 2008 +* +* Popis: main vola funkce autonomni_pohyb() , nebo sleduj_caru(), +* obe funkce pak volaji funkce na vycteni dat z 10-bitoveho A/D prevodniku +* +*/ +#define _USE_EXR_LEVELS 1 +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include "cmd_pxmc.h" +#include "timer3.h" +#include + +uint16_t read_GP1(void); // deklarace funkce read_GP1 +uint16_t read_GP2(void); // deklarace funkce read_GP2 +uint16_t read_QRD1(void); // deklarace funkce read_QRD1 +uint16_t read_QRD2(void); // deklarace funkce read_QRD2 +uint16_t read_QRD3(void); // deklarace funkce read_QRD3 + + + +cmd_des_t const *cmd_rs232_default[]; + +/*struktury prikazu cmd*/ +cmd_des_t const cmd_des_help={0, 0,"HELP","prints help for commands", + cmd_do_help,{(char*)&cmd_rs232_default}}; + +cmd_des_t const *cmd_rs232_default[]={ + + &cmd_des_help, + CMD_DES_CONTINUE_AT(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:5, // pxms_ma je akcelerace robota byla nastavena na 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:5, + 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) +{ +}; +//******************************************************** + +extern cmd_io_t cmd_io_rs232_line; + +extern void _print(char *str); + +void +init(void) +{ + /********************************************************************************/ + *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"); + + pxmc_initialize(); + + if (init_timer3((long long)100/*ms*/*1000*1000) < 0) { + /* ERROR */ + DEB_LED_OFF(0); + } +} + + +// Distance of wheels - d +#define WHEEL_DIST 74 /* mm */ +#define MAX_R 300 /* mm */ +#define MIN_R (WHEEL_DIST/2) /* mm */ + +void move(int speed, int r) +{ + int sl, sr; + int d = WHEEL_DIST; + + + if (r != 0) { + sr = speed + (long long)speed * (d/2) / r; + sl = speed - (long long)speed * (d/2) / r; + } else { + sr = speed; + sl = speed + 10; // minus 100 je kompenzace zataceni + } + pxmc_spd(&mcsX0, +sl, 0); + pxmc_spd(&mcsX1, -sr, 0); + printf("speed=%5d, r=%5d, sl=%5d, sr=%5d\n", speed, r, sl, sr); +} + +void autonomni_pohyb(void) +{ + long int now = get_timer(); + long int next = now; + long int otoc_next = now; + long int time_next = now; + int speed = 3000; + int radius = 100; + int stav = 1; // podle priznaku stavu se rozhoduje,zda pojede rovne, nebo zatoci + int otoc = 0; // priznak pro otoceni + int smer = 1; // smer = 1 robot jede dopredu, smer = -1 robot jede dozadu + uint16_t vzdalenost, vzdalenost_zpet, cas=0; + + while (1) { + + long int now = get_timer(); + + vzdalenost = read_GP1(); + vzdalenost_zpet = read_GP2(); + + if (now >= time_next){ // podminka vypisu, jednou za vterinu + time_next = now + 10; + cas++; + /*Vypsani vzdalenosti na terminal*/ + printf("Predni: %d\tZadni: %d\tSpeed: %d\tSmer: %d\tOtoc: %d\n", vzdalenost, vzdalenost_zpet, speed, smer, otoc); + } // konec if podminky na vypis za 1s + + if (stav == 1 && now >= next){ // podminka pro jizdu + stav = 0; + move(-speed,0); + otoc++; + } + + if (stav == 0 && vzdalenost >= 280 && smer == 1){ // podminka pro zatoceni 260 odpovida (mensi nez 20cm) + + if (rand() % 100 < 70) // 50 % predpoklad pro zatoceni doprava + move(-speed,radius); // zatoc rychlosti speed a polomerem radius(doprava) + else + move(-speed,-radius); // zatoc rychlosti speed a polomerem -radius (doleva) + stav = 1; + + next = now + ((rand() % 8)+5); // zataceni po dobu 5 - 12 (nahodne cislo) + } // konec if + + + if (stav == 0 && vzdalenost_zpet >= 280 && smer == -1){ // jizda zpet!!! podminka pro zatoceni 260 odpovida (mensi nez 20cm) + + if (rand() % 100 < 70) // 50 % predpoklad pro zatoceni doprava + move(-speed,radius); // zatoc rychlosti speed a radiusem, + else + move(-speed,-radius); + stav = 1; + + next = now + ((rand() % 8)+5); // zataceni po dobu 5 - 12 (nahodne cislo) + } //konec if + +// NEVIM JAK UDELAT OPRENI SE O PREKAZKU A OTOCENI // osetreni, kdyz se robot opre o prekazku, chceme,aby se rozjel opacnym smerem + /*if (vzdalenost <=30 && now >= otoc_next) { // pokud se sensor dotyka prekazky, tak dava hodnotu 110 az 140, kontroluji to 10x 0,1s + otoc++; + otoc_next = now + 1; + } + + if (vzdalenost_zpet >= 110 && vzdalenost_zpet <=140 && now >= otoc_next) { // pokud se sensor dotyka prekazky, tak dava hodnotu 110 az 140, kontroluji to 10x 0,1s + otoc++; + otoc_next = now + 1; + } + + if (otoc >= 10){ + speed = -speed; // zmena smeru + otoc = 0; + } */ + + //zmena_smeru + if (otoc >= 6){ + speed = -speed; + otoc = 0; + smer = -smer; + move(-speed,0); //otoc normalne je pouzit o if cyklus drive, ted si ho jen pujcuji + } + + } //konec while +} //konec funkce + + +void sleduj_caru(void){ + long int now = get_timer(); + long int next = now; + long int time_next = now; + int speed = 2000; + int radius = 100; + uint16_t leva, stred, prava, cas=0; + + move(2000,0); + while(1){ + + long int now = get_timer(); + + stred = read_QRD3(); // pohled zpredu, cteni senzoru odrazu QRD1114 1-3 + leva = read_QRD1(); + prava = read_QRD2(); + + if (stred > 1000 && leva < 500 && prava <500 && now >= next){ // rozhodovani + move(-speed,0); + } + + if (stred < 1000 && leva > 500){ + move(-speed,radius); + next = now + 1; + } + + if (stred < 1000 && prava > 500){ + move(-speed,-radius); + next = now + 1; + } + + if (now >= time_next){ // podminka vypisu, jednou za vterinu + time_next = now + 10; + cas++; + /*Vypsani vzdalenosti na terminal*/ + printf("V case %ds\tLeva: %d\tStredni: %d\tPrava: %d\n", cas, leva, stred, prava); // vypisuje hodnoty povrchu po 1s + } // konec if podminklz na vypis za 1s + } // konec hlavni smycky +} // konec funkce sleduj_caru() + + +void jezdi_ctverec(void) // funkce jezdeni, zmena podle casovace +{ + long int now = get_timer(); + long int next = now; + int speed = 3000; + int radius = 100; + int stav = 1; // podle priznaku stavu se rozhoduje,zda pojede rovne, nebo zatoci + while (1) { + + long int now = get_timer(); + + if (stav == 1 && now >= next){ // podminka pro jizdu rovne po dobu 5s + stav = 0; + move(-speed,0); + next = now + 50; + } + + if (stav == 0 && now >= next){ // podminka pro zatoceni + stav = 1; + if (rand() % 100 < 50) + move(-speed,radius); + else + move(-speed,-radius); + next = now + 10; + } + + } +} + +uint16_t read_QRD1(void) // funkce na vycteni informaci z AD prevodniku z ADDRC kde je nastaven AN6 tedy QRD114-1-leva zpredu +{ + *SYS_MSTPCRA = *SYS_MSTPCRA & ~MSTPCRA_MSTPA1m; // Zapnuti AD prevodniku + + *AD_ADCSR = ADCSR_CH2m | ADCSR_CH1m; // nastaveni analog vstupu AN8 - tedy vystup ze senzoru QRD114-3-prostredni. CH3=0,CH2=1,CH1=1,CH0=0 + + uint8_t AN6h, AN6d; + uint16_t AN6; // AN6h je horni byte AD prevodu, AN6d je dolni byte. Jsou cteny z ADDRC (viz tabulka 4-2 [navratil], AN6 je cele 10-bitove cislo) + int i; + + for(i = 0; i < 5; i++){ // for cyklus na 5 krat probehnuti smicky + *AD_ADCSR = *AD_ADCSR & ~ADCSR_ADFm; // softwarove nastaveni ADF do 0 + *AD_ADCSR = *AD_ADCSR | ADCSR_ADSTm; // spusteni AD prevodu + + while((*AD_ADCSR & ADCSR_ADFm) == 0) { // cekaci smycka dokud neskonci AD prevod + ; + } + + /*Precteni vysledku AD prevodu z ADDRC*/ + AN6h = *AD_ADDRCH; // Cteni horniho bytu + AN6d = *AD_ADDRCL; // Cteni dolniho bytu + AN6 = (AN6d >> 6) | (AN6h << 2); // Posun jednotlivych bitu, abychom dostali 10-bitove cislo + + } // konec for + return (AN6); // return pro main_loop, muzu vypnout while(1) a }pro main +} // konec funkce + + +uint16_t read_QRD2(void) // funkce na vycteni informaci z AD prevodniku z ADDRD kde je nastaven AN7 tedy QRD114-2-prava zpredu +{ + *SYS_MSTPCRA = *SYS_MSTPCRA & ~MSTPCRA_MSTPA1m; // Zapnuti AD prevodniku + + *AD_ADCSR = ADCSR_CH2m | ADCSR_CH1m | ADCSR_CH0m; // nastaveni analog vstupu AN8 - tedy vystup ze senzoru QRD114-3-prostredni. CH3=0,CH2=1,CH1=1,CH0=1 + + uint8_t AN7h, AN7d; + uint16_t AN7; // AN7h je horni byte AD prevodu, AN7d je dolni byte. Jsou cteny z ADDRD (viz tabulka 4-2 [navratil], AN7 je cele 10-bitove cislo) + int i; + + for(i = 0; i < 5; i++){ // for cyklus na 5 krat probehnuti smicky + *AD_ADCSR = *AD_ADCSR & ~ADCSR_ADFm; // softwarove nastaveni ADF do 0 + *AD_ADCSR = *AD_ADCSR | ADCSR_ADSTm; // spusteni AD prevodu + + while((*AD_ADCSR & ADCSR_ADFm) == 0) { // cekaci smycka dokud neskonci AD prevod + ; + } + + /*Precteni vysledku AD prevodu z ADDRD*/ + AN7h = *AD_ADDRDH; //Cteni horniho bytu + AN7d = *AD_ADDRDL; //Cteni dolniho bytu + AN7 = (AN7d >> 6) | (AN7h << 2); //Posun jednotlivych bitu, abychom dostali 10-bitove cislo + + } //konec for + return (AN7); // return pro main_loop, muzu vypnout while(1) a }pro main +} // konec funkce + + +uint16_t read_QRD3(void) // funkce na vycteni informaci z AD prevodniku z ADDRA kde je nastaven AN8 tedy QRD114-3-prostredni +{ + *SYS_MSTPCRA = *SYS_MSTPCRA & ~MSTPCRA_MSTPA1m; //Zapnuti AD prevodniku + + *AD_ADCSR = ADCSR_CH3m; // nastaveni analog vstupu AN8 - tedy vystup ze senzoru QRD114-3-prostredni. CH3=1,CH2=0,CH1=0,CH0=0 + + uint8_t AN8h, AN8d; + uint16_t AN8; //AN8h je horni byte AD prevodu, AN8d je dolni byte. Jsou cteny z ADDRA (viz tabulka 4-2 [navratil], AN8 je cele 10-bitove cislo) + int i; + + for(i = 0; i < 5; i++){ // for cyklus na 5 krat probehnuti smicky + *AD_ADCSR = *AD_ADCSR & ~ADCSR_ADFm; // softwarove nastaveni ADF do 0 + *AD_ADCSR = *AD_ADCSR | ADCSR_ADSTm; // spusteni AD prevodu + + while((*AD_ADCSR & ADCSR_ADFm) == 0) { // cekaci smycka dokud neskonci AD prevod + ; + } + + /*Precteni vysledku AD prevodu z ADDRA*/ + AN8h = *AD_ADDRAH; // Cteni horniho bytu + AN8d = *AD_ADDRAL; // Cteni dolniho bytu + AN8 = (AN8d >> 6) | (AN8h << 2); // Posun jednotlivych bitu, abychom dostali 10-bitove cislo + + } // konec for + return (AN8); // return pro main_loop, muzu vypnout while(1) a }pro main +} // konec funkce + + +uint16_t read_GP1(void) // funkce na vycteni informaci z AD prevodniku z ADDRB kde je nastaven AN9 tedy GP2D12 predni +{ // vraci hodnotu vzdalenosti podle tabulky 4-neco + *SYS_MSTPCRA = *SYS_MSTPCRA & ~MSTPCRA_MSTPA1m; // Zapnuti AD prevodniku + + *AD_ADCSR = ADCSR_CH3m | ADCSR_CH0m; // nastaveni analog vstupu AN9 - tedy vzstup ze senzoru GP2D12-predni. CH3=1,CH2=0,CH1=0,CH0=1 + + uint8_t AN9h, AN9d; + uint16_t AN9; // AN9h je horni byte AD prevodu, AN9d je dolni byte. Jsou cteny z ADDRB (viz tabulka 4-2 [navratil], AN9 je cele 10-bitove cislo) + int i; + + for (i = 0; i < 5; i++){ // for cyklus na 5 krat probehnuti smicky , aby se ustalily hodnoty ze senzoru a nedavaly nahodnou hodnotu + *AD_ADCSR = *AD_ADCSR & ~ADCSR_ADFm; // softwarove nastaveni ADF do 0 + *AD_ADCSR = *AD_ADCSR | ADCSR_ADSTm; // spusteni AD prevodu + + while((*AD_ADCSR & ADCSR_ADFm) == 0) { // cekaci smycka dokud neskonci AD prevod + ; + } + + /*Precteni vysledku AD prevodu z ADDRB*/ + AN9h = *AD_ADDRBH; // Cteni horniho bytu + AN9d = *AD_ADDRBL; // Cteni dolniho bytu + AN9 = (AN9d >> 6) | (AN9h << 2); // Posun jednotlivych bitu, abychom dostali 10-bitove cislo + + } // konec for + return (AN9); // return pro main_loop, muzu vypnout while(1) a }pro main +} // konec funkce + + +uint16_t read_GP2(void) // funkce na vycteni informaci z AD prevodniku z ADDRC kde je nastaven AN10 tedy GP2D12 zadni +{ + *SYS_MSTPCRA = *SYS_MSTPCRA & ~MSTPCRA_MSTPA1m; // Zapnuti AD prevodniku + *AD_ADCSR = ADCSR_CH3m | ADCSR_CH1m; // nastaveni analog vstupu AN10 - tedy vstup ze senzoru GP2D12-predni. CH3=1,CH2=0,CH1=1,CH0=0 + + uint8_t AN10h, AN10d; + uint16_t AN10; //AN10h je horni byte AD prevodu, AN10d je dolni byte. Jsou cteny z ADDRC (viz tabulka 4-2 [navratil], AN10 je cele 10-bitove cislo) + int i; + + for(i = 0; i < 5; i++){ // for cyklus na 5 krat probehnuti smicky, aby se ustalily hodnoty ze senzoru a nedavali nahodne hodnoty + *AD_ADCSR = *AD_ADCSR & ~ADCSR_ADFm; // softwarove nastaveni ADF do 0 + *AD_ADCSR = *AD_ADCSR | ADCSR_ADSTm; // spusteni AD prevodu + + while((*AD_ADCSR & ADCSR_ADFm) == 0) { // cekaci smycka dokud neskonci AD prevod + ; + } + + /*Precteni vysledku AD prevodu z ADDRC*/ + AN10h = *AD_ADDRCH; // Cteni horniho bytu + AN10d = *AD_ADDRCL; // Cteni dolniho bytu + AN10 = (AN10d >> 6) | (AN10h << 2); // Posun jednotlivych bitu, abychom dostali 10-bitove cislo*/ + } //konec for + return (AN10); +} + + +int main() +{ + _print("Snowforce for life ! ! !\n"); + init(); + /* TODO initialize rand accoring to voltage read from AD6. */ + + autonomni_pohyb(); // volani hlavni funkce autonomniho pohybu robota + //sleduj_caru(); // volani funkce na sledovani cary + for (;;); // nekonecna smycka, pro odchyceni nahodneho skoku zpet do main + return 0; +} diff --git a/navratil_ad/timer3.c b/navratil_ad/timer3.c new file mode 100644 index 0000000..a531deb --- /dev/null +++ b/navratil_ad/timer3.c @@ -0,0 +1,57 @@ +#include +#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) + *TPU_TSR3 &= ~TSR3_TCFVm & ~TSR3_TGFAm; //reset overflow and comare match flags +} + +//timer initialisation +/*free running counter*/ +int init_timer3(long long nsec) +{ +#define CPU_CYCLE_NSEC (1000000000/CPU_SYS_HZ) + long long timer_tick = CPU_CYCLE_NSEC; + int presc_ind = 0; + const signed char presc_tab[] = {0,1,2,3,6,5,7,-1}; + long long nsec_scaled = nsec >> 16; + while (presc_ind < 8 && + (timer_tick < nsec_scaled + || presc_tab[presc_ind] < 0)) { + presc_ind++; + timer_tick <<= 2; + } + if (timer_tick < nsec_scaled) + return -1; + + + *SYS_MSTPCRA &= ~MSTPCRA_TPUm; // power TPU unit + + *TPU_TCR3 = TCR3_CCLR0m | presc_tab[presc_ind]; //rising edge, cleared by TGA, prescaler is calculated + *TPU_TMDR3 =0x00; // normal mode + *TPU_TIOR3L = TIOR3L_IOC1m; // output 1 at compare match + *TPU_TSR3 &= ~TSR3_TCFVm & ~TSR3_TGFAm; //reset overflow and comare match flags + //*TPU_TIER3 |=TIER3_TCIEVm; //enable overflow interrupt + *TPU_TIER3 |=TIER3_TGIEAm; //enable compare match interrupt + *TPU_TGR3A = (unsigned)(nsec/timer_tick); + + *TPU_TSTR |=TSTR_CST3m; //start timer + + //excptvec_set(EXCPTVEC_TCI3V, timer_isr); /* handle overflow interrupt */ + excptvec_set(EXCPTVEC_TGI3A, timer_isr); /* handle TGRA match interrupt */ + return 0; +} + +timer_t get_timer() +{ + return timer; +} diff --git a/navratil_ad/timer3.h b/navratil_ad/timer3.h new file mode 100644 index 0000000..9bdf2d2 --- /dev/null +++ b/navratil_ad/timer3.h @@ -0,0 +1,7 @@ + + +typedef long long timer_t; + +int init_timer3(long long nsec); +timer_t get_timer(); + -- 2.39.2