--- /dev/null
- msg.dlc = 8;
- msg.data[0] = (data[0]>>8) & 0xFF;
- msg.data[1] = data[0] & 0xFF;
- msg.data[2] = (data[1]>>8) & 0xFF;
- msg.data[3] = data[1] & 0xFF;
- msg.data[4] = (data[2]>>8) & 0xFF;
- msg.data[5] = data[2] & 0xFF;
- msg.data[6] = (data[3]>>8) & 0xFF;
- msg.data[7] = data[3] & 0xFF;
+ ////////////////////////////////////////////////////////////////////////////////
+ //
+ // Eurobot BLINK TEST (with LPC2129)
+ //
+ // Description
+ // -----------
+ // This software blinks on debug leds. Use it with lpceurobot board
+ // Author : Jiri Kubias DCE CVUT
+ //
+ //
+ ////////////////////////////////////////////////////////////////////////////////
+
+ #include <lpc21xx.h> /* LPC21xx definitions */
+ #include <errno.h>
+ #include <periph/can.h>
+ #include <string.h>
+ #include <deb_led.h>
+ #include <can_ids.h>
+ #include <can_msg_def.h>
+ #include <servo.h>
+ #include <powswitch.h>
+ #include <engine.h>
+ #include <adc_filtr.h>
+ #include <uart_nozen.h>
+
+
+
+
+ #define CAN_SPEED 1000000
+ #define CAN_ISR 0
+ #define UART0_ISR 1
+ #define UART1_ISR 2
+ #define SERVO_ISR 4
+ #define ADC_ISR 5
+
+ #define CR 0x0d
+
+ #define PWM_CHAN_A 0
+ #define PWM_CHAN_B 1
+
+ #define ADC_TIME 60
+
+ #define SERVO_0_BOUND 0x10 // min for servo S0
+ #define SERVO_1_BOUND 0xFA // max for servo S1
+
+ #define TIMEOUT_DELAY 3000
+
+
+
+
+ /**
+ * @addtogroup leds
+ * @{
+ * @name Picker_09 (lpceurobot)
+ * @{
+ */
+ #define LED_GREEN /**< Blinks once per second when software is alive */
+ #define LED_BLUE /**< Blinks when a CAN message is received: */
+ #define LED_YELLOW /**< Light: when timeout occurs */
+ #define LED_RED /**< Not used */
+ /** @} */
+ /** @} */
+
+
+
+
+
+
+ can_msg_t msg;
+
+ unsigned int time_blink;
+ volatile unsigned int time_timeout;
+ volatile unsigned int sberac_timeout;
+ volatile unsigned sberac_timeout_en = 0;
+
+
+ void led_blik(unsigned char led)
+ {
+ if (time_blink == 0) time_blink = time_ms + 500;
+
+ if (time_ms > time_blink)
+ {
+ deb_led_change(led);
+ time_blink = time_ms + 500;
+ }
+
+ }
+
+
+ void atoim(unsigned int val)
+ {
+ write_UART_data( UART0, ' ' );
+ write_UART_data( UART0, (val / 1000 + 48) );
+ write_UART_data( UART0, ((val % 1000) / 100 + 48) );
+ write_UART_data( UART0, ((val % 1000 % 100) / 10 + 48) );
+ write_UART_data( UART0, ((val % 100) % 10 + 48) );
+
+ }
+
+ unsigned int time_adc =0;
+ void send_adc()
+ {
+ if (time_adc == 0) time_adc = time_ms + ADC_TIME;
+
+ if (time_ms > time_adc)
+ {
+ adc_filter(adc_update_adc);
+ unsigned int data[4];
+ data[0] = adc_val[0];
+ data[1] = adc_val[1];
+ data[2] = adc_val[2];
+ data[3] = adc_val[3];
+
+
+ atoim(data[0]);
+ atoim(data[1]);
+ atoim(data[2]);
+ atoim(data[3]);
+
+
+ msg.id = CAN_ADC_1;
+ msg.flags = 0;
- case CAN_SERVO:
++ msg.dlc = 2;
++ msg.data[0] = (data[2]>>8) & 0xFF;
++ msg.data[1] = data[2] & 0xFF;
+ while (can_tx_msg(&msg));
+
+
+ time_adc = time_ms + ADC_TIME;
+ }
+
+ }
+
+
+ void set_pwm(unsigned char chan , unsigned char pwm)
+ {
+ unsigned int dir = 0;
+ unsigned int speed = 0;
+
+ if ( pwm > 100 )
+ {
+ dir = 1;
+ speed = pwm - 100;
+ }
+ else
+ {
+ dir = 0;
+ speed = 100 - pwm;
+ }
+
+ if (chan == PWM_CHAN_A)
+ {
+ engine_A_pwm(speed);
+ if (dir)
+ engine_A_dir(ENGINE_DIR_FW);
+ else
+ engine_A_dir(ENGINE_DIR_BW);
+ }
+ else
+ {
+ engine_B_pwm(speed);
+ if (dir)
+ engine_B_dir(ENGINE_DIR_FW);
+ else
+ engine_B_dir(ENGINE_DIR_BW);
+ }
+ }
+
+ void go_to_sleep()
+ {
+ engine_A_en(ENGINE_EN_OFF); // engines off
+ engine_A_pwm(0);
+ engine_B_en(ENGINE_EN_OFF);
+ engine_B_pwm(0);
+
+ can_off();
+
+ deb_led_off(LEDY);
+ deb_led_off(LEDB);
+ deb_led_off(LEDG);
+ deb_led_on(LEDR);
+ PCON = 2; // bye bye
+ }
+
+
+ void can_rx(can_msg_t *msg) {
+ can_msg_t rx_msg;
+
+ memcpy(&rx_msg, msg, sizeof(can_msg_t));
+
+
+ switch (rx_msg.id) {
- case CAN_DRIVES:
++ case CAN_CHELAE:
+ deb_led_on(LEDB);
+ time_timeout = time_ms + TIMEOUT_DELAY;
+ deb_led_off(LEDY);
+
+ if(rx_msg.data[0] < SERVO_0_BOUND) rx_msg.data[0] = SERVO_0_BOUND;
+ if(rx_msg.data[1] > SERVO_1_BOUND) rx_msg.data[1] = SERVO_1_BOUND;
+
+ set_servo(0, rx_msg.data[0]);
+ set_servo(1, rx_msg.data[1]);
+ set_servo(2, rx_msg.data[2]);
+
+ break;
+
- case CAN_PICKER:
++ case CAN_BELTS:
+
+ deb_led_on(LEDB);
+ time_timeout = time_ms + TIMEOUT_DELAY;
+ deb_led_off(LEDY);
+ set_pwm(PWM_CHAN_B, rx_msg.data[0]);
+ set_pwm(PWM_CHAN_A, rx_msg.data[1]);
+ break;
+
+ case CAN_PWR_ALERT:
+ if ( (rx_msg.data[0] == 2) | (rx_msg.data[0] == 3) ) go_to_sleep();
+ break;
+
+
-
++/* case CAN_PICKER:
+ sberac_timeout_en = 1;
+ deb_led_on(LEDB);
+ deb_led_off(LEDY);
+ set_servo(0, rx_msg.data[0]);
+ set_servo(1, rx_msg.data[1]);
+ set_pwm(PWM_CHAN_B, rx_msg.data[2]);
+ set_pwm(PWM_CHAN_A, rx_msg.data[3]);
+ time_timeout = time_ms + TIMEOUT_DELAY;
+ sberac_timeout = time_ms + ((uint32_t)rx_msg.data[4])*100;
+ break;
++*/
+ default:
+ break;
+ }
+
+ deb_led_off(LEDB);
+ }
+
+
+
+ void check_sberac_timeout(void)
+ {
+ if ((sberac_timeout < time_ms) & sberac_timeout_en)
+ {
+ set_servo(0, SERVO_0_BOUND);
+ set_servo(1, SERVO_1_BOUND);
+ //set_servo(2, 0x80);
+ engine_A_pwm(0);
+ engine_B_pwm(0);
+ sberac_timeout_en = 0;
+ }
+
+ }
+
+ void init_perip(void) // inicializace periferii mikroprocesoru
+ {
+ init_servo(SERVO_ISR);
+ init_pow_switch();
+
+ init_engine_A();
+ init_engine_B();
+ engine_A_en(ENGINE_EN_OFF);
+ engine_B_en(ENGINE_EN_OFF);
+
+ init_adc_filter(ADC_ISR);
+
+ pow_switch_off();
+
+ UART_init( UART0,UART0_ISR,19200, UART_BITS_8, UART_STOP_BIT_1, UART_PARIT_OFF, 0);
+
+ can_init_baudrate(CAN_SPEED, CAN_ISR, can_rx);
+ }
+
+
+ int time_delay =0;
+
+
+ void timeout(void)
+ {
+ set_servo(0, SERVO_0_BOUND);
+ set_servo(1, SERVO_1_BOUND);
+ set_servo(2, 0x80);
+ engine_A_pwm(0);
+ engine_B_pwm(0);
+ }
+
+
+ int main (void) {
+
+ init_perip(); // sys init MCU
+
+ set_servo(0, 0x50);
+ set_servo(1, 0xB0);
+ set_servo(2, 0x80);
+
+ time_delay = time_ms + TIMEOUT_DELAY;
+
+ deb_led_on(LEDY);
+ while(time_ms < time_delay) led_blik(LEDG);
+ deb_led_off(LEDY);
+
+ engine_A_en(ENGINE_EN_ON);
+ engine_A_dir(ENGINE_DIR_FW);
+ engine_A_pwm(0);
+ engine_B_en(ENGINE_EN_ON);
+ engine_B_dir(ENGINE_DIR_FW);
+ engine_B_pwm(0);
+ sberac_timeout_en = 0;
+
+
+
+ time_timeout = time_ms + TIMEOUT_DELAY;
+
+
+ while(1)
+ {
+
+ if( time_ms > time_timeout)
+ {
+ timeout();
+ deb_led_on(LEDY);
+ }
+ led_blik(LEDG);
+ check_sberac_timeout();
+ send_adc();
+
+ if(adc_update_adc & 1) adc_filter(0);
+ if(adc_update_adc & 2) adc_filter(1);
+ if(adc_update_adc & 4) adc_filter(2);
+ if(adc_update_adc & 8) adc_filter(3);
+
+
+ }
+ }
+
+
- unsigned volatile char flags=0x00; // flags bit
-unsigned char flags=0x00; // flags bit
++volatile uint32_t flags=0x00; // flags bit
unsigned char lift_dir, pusher_dir;
unsigned int act_position_lift, last_position_lift, req_position_lift, last_time, max_time;
--unsigned int act_position_pusher, last_position_pusher, req_position_pusher;
- unsigned volatile int cnt_IRC_lift = 0, cnt_IRC_pusher = 0; //counters of EXTernal interrupts
- unsigned volatile int match_IRC_lift, match_IRC_pusher; //register which cnt_IRC will be compared with
-unsigned volatile int cnt_IRC_lift, cnt_IRC_pusher; //counters of EXTernal interrupts
-unsigned int match_IRC_lift, match_IRC_pusher; //register which cnt_IRC will be compared with
++volatile unsigned int act_position_pusher, last_position_pusher, req_position_pusher;
++unsigned volatile int cnt_IRC_lift , cnt_IRC_pusher; //counters of EXTernal interrupts
++unsigned volatile int match_IRC_lift, match_IRC_pusher; //register which cnt_IRC will be compared with
++volatile uint32_t can_req_position_lift, can_req_position_pusher;
++
++
error_messages error_state = NO_ERROR; //public variable content error massage for major unit
--state_fcn current_state; //pointer to current state function,( stop, fast_move, slow_move, returnToPosition)
--state_fcn last_state; //pointer to last state function
++struct fsm fsm_lift;
++struct fsm fsm_pusher;
++
can_msg_t msg, msg_tx; // CAN messge
+ //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ void dummy_wait(unsigned int wait){
+ //unsigned int wait = 50000000;
+ while(--wait)
+ ;
+ }
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~ Interrupt service rutines ~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void EINT2_rutine(void){
- EXTINT |= EXTINT_EINT2_m;
-
- deb_led_change(LEDG);
- dummy_wait(5000000);
- deb_led_change(LEDG);
- dummy_wait(5000000);
-
+ EXTINT |= EXTINT_EINT2_m;
deb_led_change(LEDR);
- if(++cnt_IRC_pusher >= match_IRC_pusher){
- sbit(flags, LIFT_ON_POS);//stop motors, set flag position OK
- engine_B_pwm(0); // STOP pwm is in percent, range 0~100~200
- deb_led_change(LEDG);
-
- }// pozooor druha jednotka
++ ++cnt_IRC_pusher;
VICVectAddr = 0;
}
void EINT3_rutine(void){
EXTINT |= EXTINT_EINT3_m;
deb_led_change(LEDY);
- if(++cnt_IRC_lift >= match_IRC_pusher){
- sbit(flags, PUSHER_ON_POS);//stop motors, set flag position OK
- while(1);
++ if(++cnt_IRC_lift >= match_IRC_lift){
++ sbit(flags, LIFT_ON_POS);//stop motors, set flag position OK
+ engine_B_pwm(0); // STOP pwm is in percent, range 0~100~200
+ }
VICVectAddr = 0;
}
}
engine_A_pwm(0); // STOP pwm is in percent, range 0~100~200
engine_B_pwm(0); // STOP pwm is in percent, range 0~100~200
-
+
- /* dummy_wait(5000000);
- send_rs_int(cnt_IRC_pusher);
- uart_send_char('\n');
- send_rs_int(match_IRC_pusher);
- uart_send_char('\n');*/
-
+ cnt_IRC_pusher = 0; //clear counter of IRC for pusher
cbit(flags,TIME_ERR);
last_time = get_TMR0();//save time when motore start
- while((tbit(flags,TIME_ERR) == 0)){//move pusher until bit is zero/ bit is switch input
- if((get_TMR0() >= last_time + TIMEOUT/3))//check timeout
+ while((tbit(flags,TIME_ERR) == 0) && (cnt_IRC_pusher < match_IRC_pusher)){//move pusher until bit is zero/ bit is switch input
+ if((get_TMR0() >= last_time + PUSHER_TIMEOUT/5))//check timeout
sbit(flags, TIME_ERR);
- }
+ }
cbit(flags,TIME_ERR);
last_time = get_TMR0();//save time when motore start
- move_lift(ENGINE_FW, 50);// start moving to back bound width max velocity
- while((tbit(flags,TIME_ERR) == 0)&&(tbit(IO0PIN, BACK_SW) == 0)){//move pusher until bit is zero/ bit is switch input
- if((get_TMR0() >= last_time + MAX_TIMEOUT))//check timeout
+ cnt_IRC_pusher = 0; //clear counter of IRC for pusher
+ move_pusher(ENGINE_BW, 50);// start moving to back bound width max velocity
+ while((tbit(flags,TIME_ERR) == 0) && (cnt_IRC_pusher < match_IRC_pusher)){//move pusher until bit is zero/ bit is switch input
+ if((get_TMR0() >= last_time + PUSHER_TIMEOUT))//check timeout
sbit(flags, TIME_ERR);
}
engine_A_pwm(0); // STOP pwm is in percent, range 0~100~200
}
}
++
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- void init_TMR0(void){
- T0MR0 = 0xffffffff; // interrupt interval when timer0 overload and trigger some action
+ static void init_TMR0(void){
+ T0MR0= 0xffffffff; // interrupt interval when timer0 overload and trigger some action
T0PR = CPU_APB_HZ/1000; // Load prescaler for 1 Msec tick
T0MCR = 0x07; //resets timer0, stop timer 0 and generates interrupt when timer0 counter match MR0 register
T0TCR = 0x02; //Reset timer 0
unsigned int count_pusher_IRC(unsigned int req_pos, unsigned int act_pos){
unsigned int irc;
if(req_pos > act_pos){
-- lift_dir = ENGINE_BW; //set the direction of pusher moving
++ pusher_dir = ENGINE_FW; //set the direction of pusher moving
irc = req_pos - act_pos;
}
else{
-- lift_dir = ENGINE_FW;
++ pusher_dir = ENGINE_BW;
irc = act_pos - req_pos;
}
return irc;
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void CAN_rx(can_msg_t *msg) {
can_msg_t rx_msg;
--
memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
--// switch (rx_msg.id)
--// {
--// case CAN_DOORS:
--// //set_servo(0, rx_msg.data[2]);
-- //set_servo(1, rx_msg.data[4]);
--// break;
--
--// case CAN_LIFT:
-- //recieveCANmessage= 1; //flag to sent CAN data
-- //recieveCANdata= rx_msg.data[3]; // data to send
--// break;
++
++ switch (rx_msg.id)
++ {
++ case CAN_HOLDER:
++ set_servo(rx_msg.data[0]); //set servo position
++ uart_send_char('R'); //Receive Can Message
++ uart_send_char('H');
++ uart_send_char('M');
++ uart_send_char('\n');
++ break;
++ case CAN_LIFT:
++ can_req_position_lift = (rx_msg.data[0]<<8) | (rx_msg.data[1]); // save new req position of lift
++ sbit(flags, UPDATE_FSM_LIFT);
++ uart_send_char('R'); //Receive Can Message
++ uart_send_char('L');
++ uart_send_char('M');
++ uart_send_char('\n');
++ break;
++ case CAN_PUSHER:
++ can_req_position_pusher = (rx_msg.data[0]<<8) | (rx_msg.data[1]);//save new req position of pusher
++ sbit(flags, UPDATE_FSM_PUSHER);
++ uart_send_char('R'); //Receive Can Message
++ uart_send_char('P');
++ uart_send_char('M');
++ uart_send_char('\n');
++ break;
--// case CAN_PUSHER:
-- //if ( (rx_msg.data[0] == 2) | (rx_msg.data[0] == 3) ) go_to_sleep();
--// break;
--
--// default:
--// break;
--// }
++ default:break;
++ }
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~ Possible states definition ~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
--void stop(events my_event){
++void stop_lift(struct fsm *fsm, events my_event){
switch(my_event){
case EVENT_ENTRY:
engine_A_pwm(0);//stop booth of motors
-- engine_B_pwm(0);
set_holder(60);//close holder
-- break;
++ uart_send_char('S'); //Stop Lift Do
++ uart_send_char('L');
++ uart_send_char('E');
++ uart_send_char('\n');
++ break;
case EVENT_DO:
-- //wait for commands
++ if(tbit(flags, UPDATE_FSM_LIFT))
++ {
++ fsm->current_state = &move_lift_pos;
++ req_position_lift = can_req_position_lift;
++ cbit(flags, UPDATE_FSM_LIFT);
++ }
break;
case EVENT_EXIT:
break;
}
}
- void move_lift_pos(events my_event){
++
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void move_lift_pos(events my_event){
++void stop_pusher(struct fsm *fsm, events my_event){
+ switch(my_event){
+ case EVENT_ENTRY:
++ engine_B_pwm(0);
++ set_holder(60);//close holder
++ uart_send_char('S'); //Stop Lift Do
++ uart_send_char('P');
++ uart_send_char('E');
++ send_rs_int(act_position_pusher);
++ uart_send_char('\n');
++ break;
++ case EVENT_DO:
++ //wait for commands
++ if(tbit(flags, UPDATE_FSM_PUSHER)) //test flag indicating new CAN message
++ {
++ fsm->current_state = &move_pusher_pos;
++ req_position_pusher = can_req_position_pusher;
++ cbit(flags, UPDATE_FSM_PUSHER);
++ }
++ break;
++ case EVENT_EXIT:
++ break;
++ default:break;
++ }
++}
++
++
+ //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
++void move_lift_pos(struct fsm *fsm, events my_event){
+ switch(my_event){
+ case EVENT_ENTRY:
++ cnt_IRC_lift = 0;
match_IRC_lift = count_lift_IRC(req_position_lift, act_position_lift); //compute number of pulses
move_lift(lift_dir,100); //start moving
last_time = get_TMR0();//save time when moving start
break;
case EVENT_DO:
//wait for commands
- if((get_TMR0() >= last_time + TIMEOUT))
- current_state = &stop;
+ if((get_TMR0() >= last_time + LIFT_TIMEOUT))
- current_state = &stop;
++ fsm->current_state = &stop_lift;
++ uart_send_char('M'); //Move Lift
++ uart_send_char('L');
++ uart_send_char('\n');
break;
case EVENT_EXIT:
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
--void move_pusher_pos(events my_event){
++void move_pusher_pos(struct fsm *fsm, events my_event){
switch(my_event){
case EVENT_ENTRY:
++ cnt_IRC_pusher = 0;
match_IRC_pusher = count_pusher_IRC(req_position_pusher, act_position_pusher);
-- move_pusher(pusher_dir, 100);
++ move_pusher(pusher_dir, 40);
last_time = get_TMR0();//save time when moving start
break;
case EVENT_DO:
//wait for commands
- if((get_TMR0() >= last_time+TIMEOUT))
- current_state = &stop;
+ if((get_TMR0() >= last_time + PUSHER_TIMEOUT))
- current_state = &stop;
++ fsm->current_state = &stop_pusher; //stop pusher if timeout
++
++ if(cnt_IRC_pusher >= match_IRC_pusher){
++ sbit(flags, PUSHER_ON_POS);//stop motors, set flag position OK
++ engine_B_pwm(0); // STOP pwm is in percent, range 0~100~200
++ act_position_pusher = cnt_IRC_pusher;
++ fsm->current_state = &stop_pusher;//set new state
++ }// pozooor druha jednotka
++
++
++ uart_send_char('M');
++ uart_send_char('P'); //Move Pusher
++ uart_send_char('\n');
break;
case EVENT_EXIT:
}
}
++//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
++void run_fsm(struct fsm *fsm){
++ fsm->last_state = fsm->current_state;// set actual state
++ fsm->current_state(fsm, EVENT_DO);// change parameter
++
++ if(fsm->last_state != fsm->current_state){ // if state was changed
++ fsm->last_state(fsm, EVENT_EXIT); // finish the old state
++ fsm->current_state(fsm, EVENT_ENTRY); // initialize the new state
++ }
++}
++
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
int main(void){
-- unsigned int cnt_irc;
- match_IRC_lift = 80;
- match_IRC_pusher = 80;
++ unsigned int lst_CAN_time;
++ act_position_lift = 0;
++ act_position_pusher = 0;
error_messages bug;
// ~~~~~ initialization of peryphery ~~~~~
-// init_periphery();
-// init_motors();//initialization of direction and velocity
-// init_servo(SERVO_PRIORITY);//SERVO_PRIORITY is level of intrrupt, is defined in lift header file
-// init_TMR0();
+ init_periphery();
+ init_motors();//initialization of direction and velocity
+ init_servo(SERVO_PRIORITY);//SERVO_PRIORITY is level of intrrupt, is defined in lift header file
+ init_TMR0();
init_ports();
init_interrupts(IRC_PRIORITY);
- // init_uart();
-
-
++ init_uart();
++//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
++//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
--
//// if(init_engine(LIFT_ENGINE)!=NO_ERROR)
;//current_state=err_init(EVEN_ENTRY);//goto error state and send message to major unit
-// set_holder(200);//open holder
-// sbit(flags, HOLDER_OPEN);//set flag bit that holder is open
-// bug = init_engine(PULL_ENGINE);//PULL_ENGINE);
+ set_holder(200);//open holder
+ sbit(flags, HOLDER_OPEN);//set flag bit that holder is open
+ //bug = pusher_init();// initialization of push unit
//;//current_state=err_init(EVEN_ENTRY);//goto error state and send message to major unit
- front_back_test();
++ //front_back_test();
-
-
- while(1)
- {
- deb_led_change(LEDY);
- dummy_wait(5000000);
- deb_led_change(LEDY);
- dummy_wait(5000000);
- }
-
- {//move pusher until bit is zero/ bit is switch input
- //if(tbit(IOPIN0,BACK_SW)){//check pin value
- if(IO0PIN & (1<<BACK_SW)){
-// engine_A_pwm(0); // STOP pwm is in percent, range 0~100~200
- // engine_B_pwm(0); // STOP pwm is in percent, range 0~100~200
- deb_led_on(LEDB);
- }
- else{
- //engine_A_pwm(50); // START, pwm is in percent, range 0~100~200
- //engine_B_pwm(50); // START, pwm is in percent, range 0~100~200
- deb_led_off(LEDB);
- }
- }
-
-
-
-
+
- while(1){//move pusher until bit is zero/ bit is switch input
- dummy_wait(3000000);
- if(tbit(flags,LIFT_ON_POS) == 0)
- deb_led_change(LEDB);
- }
-
-
-
-
//// ~~~~~ no error >> initialization successfull completed
-- last_position_lift = HOME;
-- last_position_pusher = HOME;
-- current_state = &stop;// move_lift_pos;//stop;
-- current_state(EVENT_ENTRY);// go stop state and wait for any commands from major unit
-//// ~~~~~ state machine ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- for(;;){
- last_state = current_state;// set actual state
- current_state(EVENT_DO);// change parameter
--
++ fsm_lift.current_state = &stop_lift;// move_lift_pos;//stop;
++ fsm_pusher.current_state = &stop_pusher;// move_lift_pos;//stop;
++ fsm_lift.current_state(&fsm_lift, EVENT_ENTRY);// go stop state and wait for any commands from major unit
++ fsm_pusher.current_state(&fsm_pusher, EVENT_ENTRY);
- while(1){
- last_state = current_state;// set actual state
- current_state(EVENT_DO);// change parameter
-
-
+//// ~~~~~ state machine ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
++ send_rs_str("E\n");
++
++ cbit(flags,TIME_ERR);
++ last_time = get_TMR0();//save time when motore start
++ move_pusher(ENGINE_BW, 100);// start moving to back bound width max velocity
++ while(tbit(flags,TIME_ERR) == 0){//move pusher until bit is zero/ bit is switch input
++ if((get_TMR0() >= last_time + PUSHER_TIMEOUT))//check timeout
++ sbit(flags, TIME_ERR);
++ }
++ engine_A_pwm(0); // STOP pwm is in percent, range 0~100~200
++ engine_B_pwm(0); // STOP pwm is in percent, range 0~100~200
-- set_holder(60);
-- if(last_state != current_state)
-- { // if state was changed
-- last_state(EVENT_EXIT); // finish the old state
-- current_state(EVENT_ENTRY); // initialize the new state
++
++
++ act_position_pusher = 0;
++ lst_CAN_time = 0; // clear timer for CAN messages
++ while(1){
++ run_fsm(&fsm_lift);
++ run_fsm(&fsm_pusher);
++ if(get_TMR0() >= lst_CAN_time + 100){ //repeat sending message every 100 ms
++ lst_CAN_time = get_TMR0(); //save new time, when message was sent
++ //send CAN msg
}
++ dummy_wait(3000000);
++ deb_led_change(LEDG);
}
}
projects / eurobot / public.git / commitdiff