2 * Copyright (C) 2006-2011 Department of Control Engineering, FEE,
3 * Czech Technical University in Prague, Czech Republic,
4 * http://dce.fel.cvut.cz/
6 * Copyright (C) 2006-2011 Michal Sojka <sojkam1@fel.cvut.cz>
8 * This program is free software; you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License as published by
10 * the Free Software Foundation; either version 2 of the License, or
11 * (at your option) any later version.
13 * This program is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU General Public License for more details.
19 /* procesor H8S/2638 ver 1.1 */
23 #include <system_def.h>
27 //#include <pxmc_internal.h>
33 #include <periph/sci_rs232.h>
34 #include <periph/sci_channels.h>
43 #include <candriver.h>
44 #include <canOpenDriver.h>
47 #define INLINE /* Empty */
52 * @name Odometry (IRC2010)
55 /* #define LED_MAIN_LOOP 0 */
56 /* #define LED_CORR_TRIG 2 */
57 /* #define LED_ODO_SEND 3 */
58 #define LED_LIVE 0 /**< D1: Blinks once per second when software is alive */
59 #define LED_CAN_REC 1 /**< D2: Blinks when a CAN message is received:
60 * - 50ms for motion message
61 * - 5ms for other message (corr_trig) */
62 #define LED_ERROR 2 /**< D3: Light = Unhandled exception, Slow blink (4s) = motor error */
63 #define LED_RESET 3 /**< D4: Blinks for 1 s after reset */
67 #define MAX(a,b) ((a)>(b)?(a):(b))
69 bool odometry_triggered = false;
70 unsigned char trig_seq;
72 int cmd_do_quit(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
74 printf("Good bye!\n");
75 *HCAN1_MCR=1; // software reset
80 int cmd_do_canst(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
86 int cmd_do_setflags(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
90 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
92 pxmc_set_flag(mcs,PXMS_PTI_b); // start to updating index pointing to the phase table (ptindx)
93 pxmc_set_flag(mcs,PXMS_PHA_b); // FIXME: is it really need here
94 printf("Flags were set!\n");
98 * Rotates motor slowly four turns forward and than two turns backward
99 * (in a feedforward manner). During movement it displays values read
100 * from HAL sensors and tries to detect correct index mark position.
102 int cmd_do_haltest(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
105 int i, vang, ptindx, ang_deg, dir, rot;
107 int ptmark_errors[10];
108 int err_idx=-1; /* Ignore the first change because */
109 short ptofs = 1; /* it is unlikely that initial offset is 1 (after reset it will be zero) */
110 unsigned last_time = 0;
112 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
114 vang = mcs->pxms_ptvang; /* Remember vang */
117 mcs->pxms_ene=mcs->pxms_me/3;
118 printf("ptindx hal ap ptofs\n");
120 /* Move motor to the initial position */
121 mcs->pxms_flg = PXMS_PHA_m | PXMS_PTI_m; /* Do not update ptindex acording to HALs */
122 mcs->pxms_ptindx=0; /* Set fixed index to phase tables*/
123 motor_do_output(mcs); /* Apply mag. field accoring to our index */
124 last_time = pxmc_msec_counter;
125 while (pxmc_msec_counter < last_time + 1000) /* Wait for motor to move */
129 for (dir=0; dir<2; dir++) {
130 for (rot=0; rot<4; rot++) {
131 pxmc_initialize(); // reset index mark detection
132 if ((dir&1) == 0) ang_deg=0;
134 while (((dir&1) == 0 && ang_deg < 360) ||
135 ((dir&1) != 0 && ang_deg >= 0)) {
137 ptindx = ((long)ang_deg*mcs->pxms_ptirc/360) % mcs->pxms_ptirc;
140 pxmc_call(mcs, mcs->pxms_do_inp); /* Read IRCs */
141 mcs->pxms_ptindx=ptindx;/* Set fixed index to phase tables*/
142 mcs->pxms_flg = PXMS_PHA_m | PXMS_PTI_m; /* Do not update ptindex acording to HALs */
143 motor_do_output(mcs); /* Apply mag. field accoring to our index */
144 last_time = pxmc_msec_counter;
145 while (pxmc_msec_counter < last_time + 4); /* Wait for motor to move */
146 hal = hal_read(mcs); /* read HAL sensors */
147 printf("%d %d %ld %d\n", mcs->pxms_ptindx, hal,
148 mcs->pxms_ap>>PXMC_SUBDIV(mcs), mcs->pxms_ptofs);
149 if (mcs->pxms_ptofs != ptofs) { /* index mark was detected */
150 ptofs = mcs->pxms_ptofs;
151 if ((int)err_idx < (int)(sizeof(ptmark_errors)/sizeof(*ptmark_errors))) {
153 ptmark_errors[err_idx] =
154 mcs->pxms_ptofs + mcs->pxms_ptindx - (mcs->pxms_ap>>PXMC_SUBDIV(mcs));
159 if ((dir&1) == 0) ang_deg++;
165 mcs->pxms_ptvang = vang;
166 motor_do_output(mcs);
167 mcs->pxms_flg = PXMS_ENI_m;
170 for (i=0; i<err_idx; i++) {
172 printf("ptmark error %d\n", ptmark_errors[i]);
173 avg+=ptmark_errors[i];
175 mcs->pxms_ptmark = (mcs->pxms_ptmark + avg/err_idx) % mcs->pxms_ptirc;
176 printf("pxms_ptmark increased by %ld to %d\n", avg/err_idx, mcs->pxms_ptmark);
181 int cmd_do_setindex(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
184 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
185 int index=atol(param[2]);
186 printf("index=%d\n", index);
187 //printf("No errors detected.\n", index);
188 mcs->pxms_ptindx=index;
192 int cmd_do_setvang(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
195 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
196 int index=atol(param[2]);
197 printf("ptvang=%d\n", index);
198 //printf("No errors detected.\n", index);
199 mcs->pxms_ptvang=index;
204 int cmd_do_setshift(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
207 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
208 int index=atol(param[2]);
209 printf("ptshift=%d\n", index);
210 //printf("No errors detected.\n", index);
211 mcs->pxms_ptshift=index;
215 int cmd_do_showene(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
218 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
219 // if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
220 printf("Current ene is: %d\n",mcs->pxms_ene);
221 printf("Current ptindx is: %d\n",mcs->pxms_ptindx);
222 printf("Current ptvang is: %d\n",mcs->pxms_ptvang);
224 printf("Current ptshift is: %d\n",mcs->pxms_ptshift);
225 printf("Current ptofs is: %d\n",mcs->pxms_ptofs);
228 printf("Actual pos is: %ld\n",mcs->pxms_ap>>PXMC_SUBDIV(mcs));
229 printf("Request pos is: %ld\n",mcs->pxms_rp>>PXMC_SUBDIV(mcs));
231 printf("inp_info is: %d\n",*(volatile short*)mcs->pxms_inp_info);
233 //printf("hal is: %d\n", hal_read(mcs) );
235 if((mcs->pxms_flg&PXMS_PTI_m)) printf("PXMS_PTI_m is: enabled\n"); else printf("PXMS_PTI_m is: disabled\n");
236 if((mcs->pxms_flg&PXMS_PHA_m)) printf("PXMS_PHA_m is: enabled\n"); else printf("PXMS_PHA_m is: disabled\n");
240 int cmd_do_nopxmc(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
243 if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
244 pxmc_clear_flag(mcs,PXMS_ENR_b);
245 pxmc_clear_flag(mcs,PXMS_ENO_b);
252 int cmd_do_mypwm(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
256 #define _PXMC_MAX_OUT_ 0x6f
257 const unsigned long ph1[6]={2, 1, 1, 0, 1, 1};
258 const unsigned long ph2[6]={1, 0, 1, 1, 2, 1};
259 const unsigned long ph3[6]={1, 1, 2, 1, 1, 0};
260 #undef _PMXC_MAX_OUT_
263 val=atol(param[2])/2;
265 printf("r=%d val=%d\n", r, val);
267 unsigned pwm1=val*ph1[r];
268 unsigned pwm2=val*ph2[r];
269 unsigned pwm3=val*ph3[r];
270 *PWM_PWBFR1A=(pwm1&0x3ff);
271 *PWM_PWBFR1C=(pwm2&0x3ff);
272 *PWM_PWBFR1E=(pwm3&0x3ff);
277 * Implementation of help command with watchdog
279 static INLINE char *skip_white(char *p)
281 while(isspace((__u8)*p)) p++;
286 #define CMD_ARR_STACK_SIZE 4
287 int cmd_do_help_wd(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
291 const cmd_des_t **des_arr=*(const cmd_des_t ***)des->info[0];
292 cmd_des_t const **arr_stack[CMD_ARR_STACK_SIZE];
295 if (cmd_io->priv.ed_line.io_stack)
296 cmd_io = cmd_io->priv.ed_line.io_stack;
299 filt=skip_white(filt);
300 if(!*filt) filt=NULL;
302 cmd_io_puts(cmd_io,"Help for commands\n");
304 wdg_clear(); /* Added to avoid reseting while listing commands */
307 if(!arr_stack_sp) break;
308 des_arr=arr_stack[--arr_stack_sp];
311 if(des==CMD_DES_CONTINUE_AT_ID){
312 /* list continues at new address */
313 des_arr=(const cmd_des_t **)*des_arr;
316 if(des==CMD_DES_INCLUDE_SUBLIST_ID){
317 /* list includes commands from sublists */
318 if(arr_stack_sp>=CMD_ARR_STACK_SIZE){
321 arr_stack[arr_stack_sp++]=des_arr+1;
322 des_arr=(const cmd_des_t **)*des_arr;
327 if(!filt || !strncmp(des->name,filt,strlen(filt))) {
330 cmd_io_puts(cmd_io,des->name);
331 cmd_io_puts(cmd_io," - ");
332 cmd_io_puts(cmd_io,help);
333 cmd_io_puts(cmd_io, "\r\n");
340 int runtime_display = 0;
343 cmd_des_t const **cmd_list;
345 /* command descriptions */
346 cmd_des_t const cmd_des_help={0, 0,"HELP","prints help for commands",
347 cmd_do_help_wd,{(char*)&cmd_list}};
348 cmd_des_t const cmd_des_showene={0, 0,"SHOWENE?","shows ene and couple others parameters",
351 cmd_des_t const cmd_des_mypwm={0, 0,"x#","test",
353 cmd_des_t const cmd_des_nopxmc={0, 0,"NOPXMC?","switch of pxmc controller and output",
355 cmd_des_t const cmd_des_disp={0, CDESM_OPCHR|CDESM_RW,"DISP","switch on/off runtime display of variables",
356 cmd_do_rw_int, {(char*)&runtime_display}};
357 cmd_des_t const cmd_des_slowgo={0, CDESM_OPCHR|CDESM_RW,"SLOWGO","program controlled movement",
358 cmd_do_rw_int, {(char*)&slowgo}};
359 cmd_des_t const cmd_des_setindex={0, 0,"SETINDEX?","changes pxms_ptindx",
361 cmd_des_t const cmd_des_setvang={0, 0,"SETVANG?","changes pxms_ptvang",
363 cmd_des_t const cmd_des_halindex={0, 0,"HALTEST?","show hal state with respect to ptindex",
365 cmd_des_t const cmd_des_setshift={0, 0,"SETSHIFT?","changes pxms_ptshift",
367 cmd_des_t const cmd_des_quit={0, 0,"QUIT","exit",
369 cmd_des_t const cmd_des_canst={0, 0,"CANST","Print CAN controller status",
372 cmd_des_t const cmd_des_setflags={0, 0,"setflags","set some flags",
376 extern cmd_io_t cmd_io_rs232_line;
377 cmd_des_t const *cmd_list_default[]={
393 (cmd_des_t*)cmd_stm_default,
397 cmd_des_t const **cmd_list=cmd_list_default; /*cmd prikazy pro PC*/
400 void unhandled_exception(void) __attribute__ ((interrupt_handler));
401 void unhandled_exception(void)
404 DEB_LED_ON(LED_ERROR);
406 DEB_LED_OFF(LED_ERROR);
411 void print_mcs_err(pxmc_state_t *mcs, char *name)
413 const char *errmsg[] = {
414 [PXMS_E_COMM&0xff] = "COMM",
415 [PXMS_E_MAXPD&0xff] = "MAXPD",
416 [PXMS_E_OVERL&0xff] = "OVERL",
417 [PXMS_E_HAL&0xff] = "HAL",
418 [PXMS_E_POWER_STAGE&0xff] = "POWER"};
419 printf("PXMC ERROR: %s - %s\n", name, errmsg[mcs->pxms_errno&0xff]);
424 #define ODOMETRY_TABLE_SIZE 10
425 struct odometryData_t
429 } oddata[ODOMETRY_TABLE_SIZE];
432 static INLINE short sendOdometry()
434 static short idxSend=0;
435 static short prevIdxRecive=0xff;
437 if(prevIdxRecive==idxRecive)
439 // Re-send can frames.
443 prevIdxRecive=idxRecive;
444 // Here should be function to send.
452 static INLINE void blink_odo_send(void)
456 if (led) DEB_LED_ON(LED_ODO_SEND);
457 else DEB_LED_OFF(LED_ODO_SEND);
463 * Sends only pxms_ap. Odometry is calculated elsewhere and since we
464 * don't send differences, sequence numbers are not necessary.
466 static INLINE void sendOdometrySimple()
468 /* If the variable is static, it is initialized to zero, so we don't have to initialize it all the time again. */
472 lap = mcs_left.pxms_ap;
473 rap = mcs_right.pxms_ap;
477 m.cob_id.w = CAN_ODO_DATA;
479 /* Data in big-endian, the least significant byte is always zero -
480 * we don't send it */
481 memcpy(&m.data[0], &lap, sizeof(lap)-1);
482 memcpy(&m.data[3], &rap, sizeof(rap)-1);
483 canMsgTransmit(0, m);
487 static INLINE short storeOdometryInTable()
489 /*FIXME: add detection of error when idxStore>idxRecive.*/
490 oddata[idxStore].dx=x;
491 oddata[idxStore].dy=y;
492 oddata[idxStore].angle=alfa;
493 // Start to count from zero ;)
496 idxStore%=ODOMETRY_TABLE_SIZE;
503 static INLINE void blink_err_led(void)
506 static bool err_led = false;
508 if (err_led) DEB_LED_ON(LED_ERROR);
509 else DEB_LED_OFF(LED_ERROR);
513 static INLINE void handle_motor_errors() {
514 static unsigned last_msg_time=0;
518 if (mcs_left.pxms_flg&PXMS_ERR_m||mcs_right.pxms_flg&PXMS_ERR_m) {
519 pxmc_stop(&mcs_left, 0);
520 pxmc_stop(&mcs_right, 0);
521 if (pxmc_msec_counter - last_msg_time >= 2000) {
523 last_msg_time = pxmc_msec_counter;
524 if (mcs_left.pxms_flg&PXMS_ERR_m) print_mcs_err(&mcs_left, "L");
525 if (mcs_right.pxms_flg&PXMS_ERR_m) print_mcs_err(&mcs_right, "R");
528 /* In case of PXMS_E_MAXPD, we wait 3 seconds and then purge the error. */
529 if (( mcs_left.pxms_flg&PXMS_ERR_m && mcs_left.pxms_errno == PXMS_E_MAXPD) ||
530 (mcs_right.pxms_flg&PXMS_ERR_m && mcs_right.pxms_errno == PXMS_E_MAXPD)) {
531 static unsigned last_err_time=0;
532 if (last_err_time == 0)
533 last_err_time = pxmc_msec_counter;
534 if (last_err_time != 0 && (pxmc_msec_counter - last_err_time > 3000)) {
535 pxmc_for_each_mcs(i, mcs) {
536 if (mcs->pxms_flg&PXMS_ERR_m && mcs->pxms_errno == PXMS_E_MAXPD) {
537 pxmc_set_const_out(mcs,0);
538 pxmc_clear_flag(mcs,PXMS_ERR_b);
546 void led_can_rec(unsigned duration_msec)
549 static unsigned last_rec;
550 static unsigned duration;
552 last_rec = pxmc_msec_counter;
553 duration = MAX(duration_msec, duration);
554 DEB_LED_ON(LED_CAN_REC);
556 if (pxmc_msec_counter - last_rec > duration) {
558 DEB_LED_OFF(LED_CAN_REC);
564 static INLINE void handle_can_receive(void)
568 while (f_can_receive(0, &msg_rcv)) {
569 switch (msg_rcv.cob_id.w) {
570 case CAN_ODO_RESET: {
571 unsigned now = pxmc_msec_counter;
572 wdg_enable(0); /* 25 us */
573 while (pxmc_msec_counter - now < 10);
574 /* Hopefully, we are reset now. */
582 /* printf("Received message: id=%lx\n", msg_rcv.cob_id.w); */
583 /* for (i = 0 ; i < 8; i++) printf("%02x ", msg_rcv.data[i]); */
589 static INLINE void handle_odometry_send()
591 #define ODOMETRY_PERIOD 50
592 #define ODOMETRY_TIMEOUT (3*ODOMETRY_PERIOD)
594 timeout = ODOMETRY_TIMEOUT,
597 if (odometry_triggered) {
598 odometry_triggered = false;
599 last_send_time = pxmc_msec_counter;
600 sendOdometrySimple();
601 timeout = ODOMETRY_TIMEOUT;
604 if (pxmc_msec_counter - last_send_time >= timeout) {
605 last_send_time = pxmc_msec_counter;
606 sendOdometrySimple();
607 timeout = ODOMETRY_PERIOD;
611 static INLINE void blink_main_loop()
614 static bool led = false;
616 if (led) DEB_LED_ON(LED_MAIN_LOOP);
617 else DEB_LED_OFF(LED_MAIN_LOOP);
622 static INLINE void handle_leds()
625 static unsigned last=0;
626 static bool on=false;
627 if (pxmc_msec_counter-last >= PERIOD/2) {
632 DEB_LED_ON(LED_LIVE);
636 DEB_LED_OFF(LED_LIVE);
639 DEB_LED_OFF(LED_RESET);
649 void _print(char *ptr);
653 excptvec_initfill(unhandled_exception, 0);
654 sci_rs232_setmode(19200, 0, 0, sci_rs232_chan_default); //PC
655 DEB_LED_ON(LED_RESET);
656 wdg_enable(6); /* 420 ms */
658 _print("CPU initialized\r\n\r\n");
660 printf("Eurobot odometry application.\n"
661 "Copyright (C) 2005-2011 PiKRON s.r.o., P. Pisa, M. Sojka and others.\n"
662 "This is free software; see the source code for copying conditions.\n"
663 "There is ABSOLUTELY NO WARRANTY; not even for MERCHANTABILITY or\n"
664 "FITNESS FOR A PARTICULAR PURPOSE.\n\n");
667 printf("PXMC odometry initialized (motor: %s)", pxmc_variant);
670 int32_t receive_id[] = { CAN_ODO_RESET, -1 };
671 canInit(0, 1000000, receive_id);
672 printf("CAN initialized\n");
676 handle_can_receive();
677 handle_odometry_send();
678 handle_motor_errors();
680 cmd_processor_run(&cmd_io_rs232_line, cmd_list_default); // run command processor on serial line
684 if (runtime_display) {
685 //printf("c=%d idx=%d\n", test_counter, test_index);
686 //printf("ene=%d\n", mcs_left.pxms_ene);
687 //printf("rs=%ld as=%ld\n", mcs_left.pxms_rs, mcs_left.pxms_as);