6 * @author Bc. Jiri Kubias, jiri.kubias@gmail.com
7 * @author Michal Sojka <sojkam1@fel.cvut.cz>
14 * @defgroup fork Vidle (fork) application
22 #include <lpc21xx.h> /* LPC21xx definitions */
25 #include <system_def.h>
27 #include <periph/can.h>
32 #include <can_msg_def.h>
37 #define CAN_SPEED 1000000 //< CAN bus speed
42 #define TIMER_IRQ_PRIORITY 5
47 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
48 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
49 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
51 #define END_SWITCH_UP_PIN 9 //pin 4, exp. port on board
52 #define END_SWITCH_DOWN_PIN 8 //pin 3, exp. port on board
53 #define SWITCH_HOME_PIN 19 //pin 6, exp. port on board
54 #define START_PIN 15 //pin 7, exp. port on board
55 #define COLOR_PIN 18 //pin 5, exp. port on board
56 #define SWITCH_STRATEGY_PIN 17 //pin 8, exp. port on board
58 #define START_SEND_PRIOD_FAST 50 /* [miliseconds] */
59 #define START_SEND_PRIOD_SLOW 300 /* [miliseconds] */
60 #define START_SEND_FAST_COUNT 10 /* How many times to send start with small period (after a change) */
62 #define LIFT_IRC_VAL_MAX 0x19C
63 #define LIFT_IRC_VAL_MIN 0x0
65 #define IRC_A_PIN 2 //pin 1, exp. port on board
66 #define IRC_B_PIN 3 //pin 2, exp. port on board
68 #define IRC_A_MASK 0x04 //(1<<IRC_A)
69 #define IRC_B_MASK 0x08 //(1<<IRC_B)
70 #define IRC_AB_MASK 0x0C //((1<<IRC_A)&(1<<IRC_B))
72 void lift_switches_handler(void);
74 void lift_switches_handler()
76 if (IO0PIN & (1<<END_SWITCH_UP_PIN)){
77 fsm_lift.flags &= ~CAN_LIFT_SWITCH_UP;
80 fsm_lift.flags |= CAN_LIFT_SWITCH_UP;
84 if (IO0PIN & (1<<END_SWITCH_DOWN_PIN)){
85 fsm_lift.flags &= ~CAN_LIFT_SWITCH_DOWN;
88 fsm_lift.flags |= CAN_LIFT_SWITCH_DOWN;
92 if (IO0PIN & (1<<SWITCH_HOME_PIN)){
93 fsm_lift.flags &= ~CAN_LIFT_SWITCH_HOME;
96 fsm_lift.flags |= CAN_LIFT_SWITCH_HOME;
99 // if (IO0PIN & (1<<IRC_A_PIN)){
100 // deb_led_off(LEDR);
104 // if (IO0PIN & (1<<IRC_B_PIN)){
105 // deb_led_off(LEDR);
111 //source code from http://www.vosrk.cz/robotika/Stavba/Enkoder.pdf
112 int32_t irc_read_tick(){
114 static uint16_t cnt_up = 0;
115 static uint16_t cnt_down = 0;
116 static uint16_t last_irc = 0;
117 static int32_t position = 0;
120 irc = IO0PIN & IRC_AB_MASK;
121 if ((irc & IRC_B_MASK) != 0){
125 temp = (irc - last_irc) & IRC_AB_MASK;
129 if (temp == IRC_A_MASK){
130 /* count 100times slower - we do not need 250 ticks per milimeter*/
131 if (++cnt_down >= 100) {
134 deb_led_change(LEDB);
137 } else if (temp == IRC_AB_MASK){
138 /* count 100times slower - we do not need 250 ticks per milimeter*/
139 if (++cnt_up >= 100) {
142 deb_led_change(LEDB);
149 void init_motors(void){
151 init_engine_A(); // initialization of PWM unit
152 engine_A_en(ENGINE_EN_ON); //enable motor A
153 engine_A_dir(ENGINE_DIR_FW); //set direction
154 engine_A_pwm(0); // STOP pwm is in percent, range 0~100~200
158 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
159 void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
160 /* set interrupt vector */
161 ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
162 ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
163 /* enable interrupt */
164 VICIntEnable = 1<<source;
167 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
170 void timer0_irq() __attribute__((interrupt));
171 volatile uint32_t timer_msec = 0, timer_usec = 0;
173 void init_timer0(uint32_t prescale, uint32_t period) {
174 T0TCR = 0x2; /* reset */
177 T0MCR = 0x3; /* match0: reset & irq */
178 T0EMR = 0; /* no ext. match */
179 T0CCR = 0x0; /* no capture */
180 T0TCR = 0x1; /* go! */
184 static unsigned cnt1khz = 0;
186 /* reset timer irq */
189 /* increment timer_usec */
191 /* increment msec @1kHz */
192 if (++cnt1khz == 100) {
197 /* int acknowledge */
202 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
203 void start_button(void)
206 bool start_condition;
207 static bool last_start_condition = 0;
209 static int count = 0;
210 static uint32_t next_send = 0;
213 start_condition = (IO0PIN & (1<<START_PIN)) == 0;
215 if (start_condition != last_start_condition) {
218 last_start_condition = start_condition;
220 next_send = timer_msec; /* Send now */
222 fsm_lift.flags |= CAN_LIFT_START;
225 if (timer_msec >= next_send) {
226 msg.id = CAN_ROBOT_CMD;
229 msg.data[0] = start_condition;
231 // send_rs_str("start\n");
233 /*while*/ (can_tx_msg(&msg));
235 if (count < START_SEND_FAST_COUNT) {
237 next_send = timer_msec + START_SEND_PRIOD_FAST;
239 next_send = timer_msec + START_SEND_PRIOD_SLOW;
246 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
247 void CAN_rx(can_msg_t *msg) {
251 memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
260 req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
261 spd = rx_msg.data[2];
262 fsm_lift.can_req_homing=rx_msg.data[3];
264 if (req >= LIFT_IRC_VAL_MIN && req <= LIFT_IRC_VAL_MAX) {
265 fsm_lift.flags &= ~CAN_LIFT_OUT_OF_BOUNDS;
266 fsm_lift.can_req_position = req;// save new req position of lift
267 fsm_lift.can_req_spd = spd;// save new req spd of lift
269 fsm_lift.flags |= CAN_LIFT_OUT_OF_BOUNDS;
278 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
279 void init_periphery(void){
281 can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
285 init_timer0(1, CPU_APB_HZ/100000);
286 set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
289 // init_adc(ADC_ISR);
291 /*********************************************************/
292 void can_send_status(void)
295 tx_msg.id = CAN_LIFT_STATUS;
298 tx_msg.data[0] = (fsm_lift.act_pos >> 8) & 0xFF;
299 tx_msg.data[1] = fsm_lift.act_pos & 0xFF;
300 tx_msg.data[2] = (fsm_lift.can_response >> 8) & 0xFF;
301 tx_msg.data[3] = fsm_lift.can_response & 0xFF;
302 tx_msg.data[4] = fsm_lift.flags;
303 /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
306 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
307 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
309 void dbg_print_time()
312 unsigned t = timer_usec, i;
313 memset(str, ' ', sizeof(str));
316 for (i=7; t > 0; i--) {
323 void fsm_lift_init(struct fsm *fsm, enum event event);
328 static uint32_t led_time = 0;
330 if(timer_msec >= led_time + 500)
332 led_time = timer_msec;
335 // fsm_lift.can_req_position = 0x380;
337 // fsm_lift.can_req_position = 0x1e0;
340 deb_led_change(LEDG);
342 send_rs_str("ACT_POS\t");
343 send_rs_int(fsm_lift.act_pos);
345 send_rs_str("CAN_FLAGS\t");
346 send_rs_int(fsm_lift.flags);
351 void robot_switches_handler()
353 static uint32_t color_time = 0;
356 if (timer_msec >= color_time + 100)
360 color_time = timer_msec;
362 if (IO0PIN & (1<<COLOR_PIN))
363 sw |= CAN_SWITCH_COLOR;
365 sw &= ~CAN_SWITCH_COLOR;
367 if (IO0PIN & (1<<SWITCH_STRATEGY_PIN))
368 sw &= ~CAN_SWITCH_STRATEGY;
370 sw |= CAN_SWITCH_STRATEGY;
372 if (sw & CAN_SWITCH_COLOR){
375 send_rs_str("color\n");}
379 if (sw & CAN_SWITCH_STRATEGY){
382 send_rs_str("strategy\n");
387 // send_rs_int(IO1PIN);
389 // send_rs_str("\n");
391 /* TODO: Put color to the second bit */
393 tx_msg.id = CAN_ROBOT_SWITCHES;
402 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
403 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
404 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
407 uint32_t main_time = timer_usec;
408 uint32_t status_time = timer_usec;
410 //lift motor is motor A, MOTA connctor on board
413 SET_PIN(PINSEL0, IRC_A_PIN, PINSEL_0);
414 SET_PIN(PINSEL0, IRC_B_PIN, PINSEL_0);
416 SET_PIN(PINSEL0, END_SWITCH_UP_PIN, PINSEL_0);
417 SET_PIN(PINSEL0, END_SWITCH_DOWN_PIN, PINSEL_0);
419 SET_PIN(PINSEL0, START_PIN, PINSEL_0); //init of start pin
420 SET_PIN(PINSEL1, (COLOR_PIN - 16), PINSEL_0); //init of color pin
421 SET_PIN(PINSEL1, (SWITCH_STRATEGY_PIN - 16), PINSEL_0); //init of strategy pin
422 SET_PIN(PINSEL1, (SWITCH_HOME_PIN - 16), PINSEL_0); //init of home pin
424 IO0DIR &= ~((1<<START_PIN) | (1<<SWITCH_HOME_PIN) | (1 << COLOR_PIN) | (1 << SWITCH_STRATEGY_PIN));
425 IO0DIR &= ~((1<<END_SWITCH_UP_PIN) | (1<<END_SWITCH_DOWN_PIN));
426 IO0DIR &= ~((1<<IRC_A_PIN) | (1<<IRC_B_PIN));
427 send_rs_str("Lift started\n");
429 fsm_lift.act_pos = 0;
430 init_fsm(&fsm_lift, &fsm_lift_init);
435 if(timer_usec >= main_time + 1000)
437 main_time = timer_usec;
441 // fsm_lift.act_pos = adc_val[0];
446 if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms
447 fsm_lift.trigger_can_send) { //or when something important happen
448 fsm_lift.trigger_can_send = false;
449 status_time = timer_msec; //save new time, when message was sent
453 fsm_lift.act_pos = irc_read_tick();
456 robot_switches_handler();
457 lift_switches_handler();