--- /dev/null
+/*******************************************************************
+ 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 <types.h>
+#include <ctype.h>
+#include <string.h>
+#include <stdlib.h>
+#include <cmd_proc.h>
+#include <cpu_def.h>
+#include <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\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;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_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
+};
--- /dev/null
+/*
+*
+* Author funkci na pohyb motoru: Petr Kovacik <kovacp1@feld.cvut.cz>, (C) 2006
+*
+* Autor funkci na autonomni pohyb: Milan Navratil <navram1@fel.cvut.cz> 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 <types.h>
+#include <cpu_def.h>
+#include <h8s2638h.h>
+#include <periph/sci_rs232.h>
+#include <system_def.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <pxmc.h>
+#include <pxmcbsp.h>
+
+#include <cmd_proc.h>
+#include "cmd_pxmc.h"
+#include "timer3.h"
+#include <stdio.h>
+
+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;
+}
projects / mirosot.git / commitdiff