2 * C Implementation: main_test
8 * Author: Petr Kovacik <kovacp1@feld.cvut.cz>, (C) 2006
13 #define _USE_EXR_LEVELS 1
17 #include <periph/sci_rs232.h>
18 #include <system_def.h>
23 #include <pxmc_h2638.h>
25 #include <bth_fce_out.h>
26 #include <bth_inface.h>
31 #include <bth_h8s2638.h>
34 /*struktury prikazu cmd*/
35 cmd_des_t const cmd_des_help={0, 0,"HELP","prints help for commands",
36 cmd_do_help,{(char*)&cmd_bth}};
38 cmd_des_t const *cmd_rs232_default[]={
42 (cmd_des_t*)cmd_stm_default,
45 cmd_des_t const **cmd_bth=cmd_rs232_default; /*cmd prikazy pro bth*/
46 cmd_des_t const **cmd_rs232=cmd_rs232_default; /*cmd prikazy pro PC*/
49 /*struktury charakterizujici motor 0*/
59 pxms_rs:0, //pxms_subdiv:8,
60 pxms_md:8000l<<8, pxms_ms:5000, pxms_ma:10,
61 pxms_inp_info:(long)TPU_TCNT1,//TPU_TCNT1 /*chanel TPU A,B*/
62 pxms_out_info:(long)PWM_PWBFR1A, /*chanel PWM A,B*/
63 pxms_ene:0, pxms_erc:0,
64 pxms_p:40, pxms_i:0, pxms_d:1, pxms_s1:0, pxms_s2:0,
65 pxms_me:0x1800, //6144
66 pxms_ptirc:40, // 2000 irc per rev, 200/4 steps /
70 pxms_cfg:PXMS_CFG_MD2E_m|PXMS_CFG_HLS_m|
71 PXMS_CFG_HPS_m|PXMS_CFG_HDIR_m|0x1
75 /*struktury charakterizujici motor 1*/
85 pxms_rs:0, //pxms_subdiv:8,
86 pxms_md:8000l<<8, pxms_ms:5000, pxms_ma:10,
87 pxms_inp_info:(long)TPU_TCNT2, /*chanel TPU C,D*/
88 pxms_out_info:(long)PWM_PWBFR1C, /*chanel PWM C,D*/
89 pxms_ene:0, pxms_erc:0,
90 pxms_p:40, pxms_i:0, pxms_d:1, pxms_s1:0, pxms_s2:0,
91 pxms_me:0x1800, //6144
92 pxms_ptirc:40, // 2000 irc per rev, 200/4 steps /
96 pxms_cfg:PXMS_CFG_MD2E_m|PXMS_CFG_HLS_m| //FIXME: nastavit spravne priznaky pro dalsi motorove struktur
97 PXMS_CFG_HPS_m|PXMS_CFG_HDIR_m|0x1
100 pxmc_state_t *pxmc_main_arr[] = {&mcsX0,&mcsX1};
102 #define SUMMOTORS (sizeof(pxmc_main_arr)/sizeof(pxmc_main_arr[0]))
104 pxmc_state_list_t pxmc_main_list = {
105 pxml_arr:pxmc_main_arr,
111 //*******************************************************
113 void unhandled_exception(void) __attribute__ ((interrupt_handler));
115 * init - shaddow registers, outputs..
117 * Initializes P1 and P3 shaddow registers,
118 * sets PJ.1, PJ.2, PJ.3 LED as outputs,
119 * initialises interrupt vector.
123 /* initialises interrupt vector */
124 excptvec_initfill(unhandled_exception, 0);
127 /*Interrupt routines*/
128 void unhandled_exception(void)
131 //********************************************************
134 int cmd_rs232_processor_run(void)
139 cmd_io=&cmd_io_rs232;
140 if(cmd_rs232_line_out(cmd_io))
143 if(cmd_rs232_line_in(cmd_io)<=0)
147 val=proc_cmd_line(cmd_io, cmd_rs232, cmd_io->priv.ed_line.in->buf);
152 if(cmd_io->priv.ed_line.out->inbuf){
153 cmd_io_putc(cmd_io,'\r');
154 cmd_io_putc(cmd_io,'\n');
158 cmd_io_write(&cmd_io_rs232,"ERROR ",6);
160 cmd_io_write(cmd_io,s,strlen(s));
161 cmd_io_putc(cmd_io,'\r');
162 cmd_io_putc(cmd_io,'\n');
168 int cmd_bth_processor_run(void)
174 if(cmd_bth_line_out(cmd_io))
177 if(cmd_bth_line_in(cmd_io)<=0)
181 val=proc_cmd_line(cmd_io, cmd_bth, cmd_io->priv.ed_line.in->buf);
186 if(cmd_io->priv.ed_line.out->inbuf){
187 cmd_io_putc(cmd_io,'\r');
188 cmd_io_putc(cmd_io,'\n');
192 cmd_io_write(&cmd_io_bth,"ERROR ",6);
194 cmd_io_write(cmd_io,s,strlen(s));
195 cmd_io_putc(cmd_io,'\r');
196 cmd_io_putc(cmd_io,'\n');
207 /********************************************************************************/
208 *DIO_PJDDR=0xff; /*output gate*/
209 *DIO_PEDDR=0xff; /*output gate*/
210 *DIO_PEDR=0x60; /*0x0-LED - light all; 0x6 -ENA,ENB=1, LE33CD=0*/
211 *DIO_PJDR=0x00; //rozsviceni vsech diod na */
214 /*priority preruseni - SCI > TPU*/
215 *SYS_SYSCR|=SYSCR_INTM1m;
216 *INT_IPRA=0x22; *INT_IPRB=0x22; *INT_IPRC=0x04;
217 *INT_IPRD=0x40; *INT_IPRE=0x44; *INT_IPRF=0x55;
218 *INT_IPRG=0x55; *INT_IPRH=0x55; *INT_IPRJ=0x06;
219 *INT_IPRK=0x67; *INT_IPRM=0x66;
221 /*povoleni vsech preruseni atd...*/
225 /*nastaveni seriovych linek - Bth, PC*/
226 sci_rs232_setmode(115200, 0, 0, 2); //bth
227 sci_rs232_setmode(19200, 0, 0, sci_rs232_chan_default); //PC
230 /*inicializace bluetooth*/
233 /*inicializace komunikacnich datovych I/O bufferu */
236 /*TPU kanal 1 - inicialize (kontrola paketu + pocatecni zpozdeni)*/
237 bth_init_pkt_controll();
239 /*nastaveni zarizeni Bth do slave role*/
240 bth_parametr_slave();
242 /*kratka cas. pouza pro bth zarizeni - mazani zasilanych dat*/
244 // bth_start_TPU_counter();
247 zn_bth=sci_rs232_recch(2);
248 }while(bth_get_timer()<11);
251 /*nastaveni HW (TPU, PWM)*/
252 pxmc_set_pwm_tpu(SUMMOTORS);
254 /*nastaveni DC motoru*/
255 pxmc_add_pservice_and_mode(4); /*Macro - mod=4 tj. all motors are DC*/
257 /*nekonecna smycka obsluhujici bth, pc ...*/
259 zn_bth=sci_rs232_recch(2);
262 bth_recieve_packet(zn_bth);
265 l2cap_send_data(0, 0);
266 bth_send_queue(); /*odesli sestavene pakety, pokod jsou*/
267 cmd_bth_processor_run(); /*sber + odesilani cmd prikazu bth*/
268 cmd_rs232_processor_run(); /*sber + odesilani cmd prikazu PC*/
292 #if 0 //male pohybove demo - pohyb robota
297 // long int before = -1;
300 /* long int now = get_timer();
305 pxmc_spd(&mcsX0,a*400,0);
306 pxmc_spd(&mcsX1,-a*700,0);
309 pxmc_spd(&mcsX0,a*700,0);
310 pxmc_spd(&mcsX1,-a*400,0);
313 pxmc_spd(&mcsX0,a*2000,0);
314 pxmc_spd(&mcsX1,-a*1200,0);
317 pxmc_spd(&mcsX0,a*1200,0);
318 pxmc_spd(&mcsX1,-a*2000,0);
321 pxmc_spd(&mcsX0,a*1300,0);
322 pxmc_spd(&mcsX1,-a*1000,0);
325 pxmc_spd(&mcsX0,a*1300,0);
326 pxmc_spd(&mcsX1,-a*1000,0);
329 pxmc_spd(&mcsX0,a*100,0);
330 pxmc_spd(&mcsX1,-a*300,0);
333 pxmc_spd(&mcsX0,a*300,0);
334 pxmc_spd(&mcsX1,-a*100,0);
337 pxmc_spd(&mcsX0,a*2000,0);
338 pxmc_spd(&mcsX1,-a*100,0);
341 pxmc_spd(&mcsX0,a*4000,0);
342 pxmc_spd(&mcsX1,-a*4000,0);