bin_PROGRAMS = eb_vytah_09
-eb_vytah_09_SOURCES = lift.c serv.c uar.c
+eb_vytah_09_SOURCES = main.c pusher.c lift.c serv.c uar.c
eb_vytah_09_LIBS = can ebb
-////////////////////////////////////////////////////////////////////////////////
-//
-// Eurobot BLINK TEST (with LPC2129)
-//
-// Description
-// -----------
-// This software control mechanisms in eurobot. Use it with lpceurobot board
-// Author : Jarda Sach DCE CVUT
-//
-//
-////////////////////////////////////////////////////////////////////////////////
-
+
#include <lpc21xx.h> /* LPC21xx definitions */
#include <deb_led.h>
#include <system_def.h>
#include <periph/can.h>
#include <string.h>
#include <deb_led.h>
-#include "serv.h"
#include "engine.h"
#include "lift.h"
#include "uar.h"
+#include "def.h"
+
-
-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;
-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
-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(LEDR);
- ++cnt_IRC_pusher;
- VICVectAddr = 0;
-}
+
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void EINT3_rutine(void){
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void init_ports(void){
- //PINSEL0 = 0x800c0000; // P0.9 => EINT (alt. fce 3), P0.15 => EINT (alt. fce 4)
- SET_PIN(PINSEL0, 15 , PINSEL_2);
- //IO0DIR = 0xffff7fff;//set PIN0.15 as input => clr bit
-}
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void init_periphery(void){
- init_engine_A();// initialization of PWM unit
- init_engine_B();
- can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
-}
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void init_motors(void){
- engine_A_en(ENGINE_EN_ON);//enable motor A
- engine_B_en(ENGINE_EN_ON);// ----//----- B
- engine_A_dir(ENGINE_BW);//set direction
- engine_B_dir(ENGINE_UP);
- 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
-}
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void init_interrupts(unsigned char IRC_priority){
- unsigned char store = VPBDIV;
- EXTINT = EXTINT_EINT2_m | EXTINT_EINT3_m;
- EXTMODE = 0x06; //sensitivity for edge
- EXTPOLAR = 0x00; //falling-edge
- VPBDIV = store; // restoring register
- ((uint32_t*)&VICVectAddr0)[IRC_PRIORITY] = (unsigned long)EINT2_rutine; // Nastaveni adresy vektotu preruseni
- ((uint32_t*)&VICVectCntl0)[IRC_PRIORITY] = VIC_ENABLE | VIC_EINT2; // choice EINT2 as source of interrupt
- VICIntEnable |= (1 << VIC_EINT2);//0x15 // enable interrupt from EINT2 source
-}
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-error_messages pusher_init(void){
- error_messages err = NO_ERROR;
- //if(tbit(flags,PUCK_LOADED))
- // return(ERROR_PUCK_LOADED);// no initialization is possible if puck is loaded in holder
-// if(!(tbit(flags,HOLDER_OPEN)))
-// return(ERROR_HOLDER_OPEN);// no initialization is possible if puck is loaded in holder
-
- cbit(flags,TIME_ERR);
- last_time = get_TMR0();//save time when motore start
- move_pusher(ENGINE_FW, 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
- 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 + PUSHER_TIMEOUT/3))//check timeout
- sbit(flags, TIME_ERR);
- }
- 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
- cbit(flags, TIME_ERR);
- return(err);
-}
-
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
error_messages lift_init(void){
error_messages err = NO_ERROR;
return(err);
}
-void front_back_test(void){
- unsigned char i;
- match_IRC_pusher = 160; //cca 2cm
- cbit(flags,TIME_ERR);
- last_time = get_TMR0();//save time when motore start
- move_pusher(ENGINE_FW, 50);// 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
- for(i=0; i<10; i++){
- cbit(flags,TIME_ERR);
- last_time = get_TMR0();//save time when motore start
- cnt_IRC_pusher = 0; //clear counter of IRC for pusher
- move_pusher(ENGINE_FW, 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
- engine_B_pwm(0); // STOP pwm is in percent, range 0~100~200
-
- 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) && (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
- 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
- engine_B_pwm(0); // STOP pwm is in percent, range 0~100~200
- cbit(flags, TIME_ERR);
- }
-}
+
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void move_lift(unsigned char direct,unsigned char velocity){//how many pulzes
engine_A_en(ENGINE_EN_ON);//enable motor of lift
}
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void move_pusher(unsigned char direct,unsigned char velocity){//how many pulzes
- engine_B_dir(direct);//set direction for lift
- engine_B_pwm(velocity); // set velocity, range 0~100~200
- engine_B_en(ENGINE_EN_ON);//enable motor of pusher
-}
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void set_holder(unsigned char range){
- if((range > HOLDER_MIN)&&(range < HOLDER_MAX)){ // servo can be moved in limitary range (lift palate)
- set_servo(range);
- }
- else{
- if(range < HOLDER_MIN)
- set_servo(HOLDER_MIN);
- else
- set_servo(HOLDER_MAX);
- }
-}
-
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-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
- T0TCR = 0x01; // start timer 0
-}
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-unsigned int get_TMR0(void){
- return T0TC;// return actual value of timer
-}
-
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
unsigned int count_lift_IRC(unsigned int req_pos, unsigned int act_pos){
unsigned int irc;
return irc;
}
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-unsigned int count_pusher_IRC(unsigned int req_pos, unsigned int act_pos){
- unsigned int irc;
- if(req_pos > act_pos){
- pusher_dir = ENGINE_FW; //set the direction of pusher moving
- irc = req_pos - act_pos;
- }
- else{
- 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_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;
-
- default:break;
- }
-}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
}
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-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;
- }
-}
+
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
default:break;
}
}
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-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, 40);
- last_time = get_TMR0();//save time when moving start
- break;
- case EVENT_DO:
- //wait for commands
- if((get_TMR0() >= last_time + PUSHER_TIMEOUT))
- 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:
-
- break;
- default:break;
- }
-}
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-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 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_ports();
- init_interrupts(IRC_PRIORITY);
- 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 = pusher_init();// initialization of push unit
- //;//current_state=err_init(EVEN_ENTRY);//goto error state and send message to major unit
- //front_back_test();
-
-
-//// ~~~~~ no error >> initialization successfull completed
- 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);
-
-//// ~~~~~ 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
-
-
-
- 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);
- }
-}
-
-
-
-#ifndef DEFH
-#define DEFH
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- //~~~~~~~~~~ definition of interrupt priority ~~~~~~~~~~~
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- #define SERVO_PRIORITY 7 // 0 is the biggest priority
- #define IRC_PRIORITY 5 // higher priority than servo
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- //~~~~~~~ definition of macros for bit operations ~~~~~~~
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- #define sbit(WORD,BIT) (WORD|=(1<<BIT)) //set bit
- #define cbit(WORD,BIT) (WORD&=~(1<<BIT))//clear bit
- #define tbit(WORD,BIT) (WORD&(1<<BIT)) //test bit
-
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- //~~~~~~~~~~~ definition of moving directions ~~~~~~~~~~~
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- #define ENGINE_BW 1 ///< Backward direction
- #define ENGINE_FW 0 ///< Forward direction
- #define ENGINE_UP 1 ///< Up direction
- #define ENGINE_DWN 0 ///< Down direction
-
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- //~~~~~~~~~~~ definition of terminal switches ~~~~~~~~~~~
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- #define BACK_SW 15
- #define FORWARD_SW 16
- #define DOWN_SW 18
- #define UP_SW 19
-
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- //~~~~~~~~~~~~ definition of lift positions ~~~~~~~~~~~~~
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- #define HOME 0
- #define FLOOR1 1
- #define FLOOR2 2
- #define FLOOR3 3
- #define FLOOR4 4
-
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- //~~~~~~~~~~ definition of flag register bits ~~~~~~~~~~~
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- #define HOLDER_OPEN 0
- #define PUCK_LOADED 1
- #define NEW_CAN_MSG 2
- #define TIME_ERR 3
- #define SWITCH 4
- #define LIFT_ON_POS 5
- #define PUSHER_ON_POS 6
- #define UPDATE_FSM_LIFT 7
- #define UPDATE_FSM_PUSHER 8
-
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- //~~~~~~~~~ definition of CAN bus cominication ~~~~~~~~~~
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- #define CAN_SPEED 1000000
- #define CAN_ISR 0
-
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- //~~~~~~~~~~~~ definition of some constants ~~~~~~~~~~~~~
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- #define HOLDER_MIN 60
- #define HOLDER_MAX 255
- #define LIFT_TIMEOUT 5000
- #define PUSHER_TIMEOUT 5000
- #define MAX_TIMEOUT 10000
-
-
- typedef enum {//possible errors
- NO_ERROR,
- ERROR_LIFT_IRC,
- ERROR_LIFT_POSITION,
- ERROR_LIFT_TIMEOUT,
- ERROR_PUSHER_IRC,
- ERROR_PUSHER_POSITION,
- ERROR_PUSHER_TIMEOUT,
- ERROR_PUCK_LOADED,
- ERROR_HOLDER_OPEN
- } error_messages;
-
-
- typedef enum {//mark which unit we want controlled
- LIFT_ENGINE,
- PULL_ENGINE
- } units;
-
- typedef enum {//mark which unit we want controlled
- DOWN,
- UP,
- BACK,
- FORWARD
- } direction;
-
- typedef enum {// events of each state of state machine
- EVENT_ENTRY,
- EVENT_DO,
- EVENT_EXIT
- } events;
-
- struct fsm;
-
- typedef void (*state_fcn)(struct fsm *fsm, events my_event);//pointer to function returning void and one input parametr
-
- struct fsm {
- volatile state_fcn current_state;
- state_fcn last_state;
- };
+#ifndef __LIFT_H
+#define __LIFT_H
+#include "def.h"
//************************************************************************************
//******************** declaration of interrupt service rutines **********************
//************************************************************************************
- void EINT2_rutine(void) __attribute__ ((interrupt));
- void EINT3_rutine(void) __attribute__ ((interrupt));
- void init_interrupts(unsigned char IRC_priority);
+ void EINT3_rutine(void) __attribute__ ((interrupt));
+
+
//************************************************************************************
//******************** declaration of users methods **********************************
//************************************************************************************
error_messages lift_init(void);// calibration and move to start position
- error_messages pusher_init(void);// calibration and move to start position
void move_lift(unsigned char direct,unsigned char velocity);
- void move_pusher(unsigned char direct,unsigned char velocity);
- void front_back_test(void);
- void set_holder(unsigned char);
- void init_motors(void);//initialization PWM duty, direction
- void init_ports(void);
- void init_switches(void);
unsigned int count_lift_IRC(unsigned int req_pos, unsigned int act_pos);
- unsigned int count_pusher_IRC(unsigned int req_pos, unsigned int act_pos);
- static void init_TMR0(void);
- unsigned int get_TMR0(void);
- void CAN_rx(can_msg_t *msg);//CAN bus receive rutine
+
//************************************************************************************
//****************** declaration of possible states **********************************
//************************************************************************************
- void stop_pusher(struct fsm *fsm, events my_event);
+
void stop_lift(struct fsm *fsm, events my_event);
- void error(struct fsm *fsm, events my_event);
void move_lift_pos(struct fsm *fsm, events my_event);
- void move_pusher_pos(struct fsm *fsm, events my_event);
-#endif
+
+
+unsigned char lift_dir;
+volatile unsigned int act_position_lift, last_position_lift, req_position_lift;
+unsigned volatile int cnt_IRC_lift ; //counters of EXTernal interrupts
+unsigned volatile int match_IRC_lift; //register which cnt_IRC will be compared with
+volatile uint32_t can_req_position_lift;
+
+
+
+#endif /*__PUSHER_H*/
// $ProjectName$
-/**
- * @file main.c
- *
-* @brief Demo application demonstrating use of eb_ebb library.
- * This file provides simply how-to use eb_ebb library.
- * From main function is called init_perip function
- * where is initialized servos, engines, power switch,
- * CAN,ADC and serial port. After this initialization is shown
- * how to control each devices. This sample also include simply
- * sample of sending and receiving CAN message.
- *
- */
-
+////////////////////////////////////////////////////////////////////////////////
+//
+// Eurobot BLINK TEST (with LPC2129)
+//
+// Description
+// -----------
+// This software control mechanisms in eurobot. Use it with lpceurobot board
+// Author : Jarda Sach DCE CVUT
+//
+//
+////////////////////////////////////////////////////////////////////////////////
+
#include <lpc21xx.h> /* LPC21xx definitions */
-#include <errno.h>
+#include <deb_led.h>
+#include <system_def.h>
+#include <can_ids.h>
#include <periph/can.h>
#include <string.h>
#include <deb_led.h>
-#include "uart.h"
-#include "servo.h"
-#include "powswitch.h"
-#include "engine.h"
-#include "adc.h" // header ADC with averaging filter
-//#include "adc_filtr.h" // header ADC with mean filter
-#include <can_ids.h>
+#include "serv.h"
+#include "engine.h"
+#include "lift.h"
+#include "pusher.h"
+#include "uar.h"
+#include "def.h"
+
-#define CAN_SPEED 1000000 ///< CAN speed
-//#define CAN_CHELAE 0x32 ///< CAN ID for servo message
+error_messages error_state = NO_ERROR; //public variable content error massage for major unit
+struct fsm fsm_lift;
+struct fsm fsm_pusher;
-/**
- * Variables ISR values
- */
-/*@{*/
-#define CAN_ISR 0
-#define ENG_ISR 1
-#define ADC_ISR 3
-#define SERVO_ISR 5
-/*@}*/
+can_msg_t msg, msg_tx; // CAN messge
-#define SPEED 30 ///< Motor speed
+void CAN_rx(can_msg_t *msg);
-/**
- * Dummy wait (busy wait)
- * This function shoul be removed in future
- */
-void dummy_wait(void)
-{
- int i = 1000000;
-
- while(--i);
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void dummy_wait(unsigned int wait){
+ //unsigned int wait = 50000000;
+ while(--wait)
+ ;
}
-/**
- * This function is called when we recieve CAN message.
- * Its called from CAN ISR
- *
- * @param *msg structure of can message (can_msg_t)
- *
- */
-void can_rx(can_msg_t *msg) {
- can_msg_t rx_msg;
-
- memcpy(&rx_msg, msg, sizeof(can_msg_t));
-
- switch (rx_msg.id) { // demo sample parsing recieved message by ID
- case CAN_CHELAE:
- deb_led_change(LEDB);
-
- set_servo(0, rx_msg.data[0]);
- set_servo(1, rx_msg.data[1]);
- set_servo(2, rx_msg.data[2]);
- break;
- default:
- break;
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void init_ports(void){
+ //PINSEL0 = 0x800c0000; // P0.9 => EINT (alt. fce 3), P0.15 => EINT (alt. fce 4)
+ SET_PIN(PINSEL0, 15 , PINSEL_2);
+ //IO0DIR = 0xffff7fff;//set PIN0.15 as input => clr bit
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void init_periphery(void){
+ init_engine_A();// initialization of PWM unit
+ init_engine_B();
+ can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void init_motors(void){
+ engine_A_en(ENGINE_EN_ON);//enable motor A
+ engine_B_en(ENGINE_EN_ON);// ----//----- B
+ engine_A_dir(ENGINE_BW);//set direction
+ engine_B_dir(ENGINE_UP);
+ 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
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void init_interrupts(unsigned char IRC_priority){
+ unsigned char store = VPBDIV;
+ EXTINT = EXTINT_EINT2_m | EXTINT_EINT3_m;
+ EXTMODE = 0x06; //sensitivity for edge
+ EXTPOLAR = 0x00; //falling-edge
+ VPBDIV = store; // restoring register
+ ((uint32_t*)&VICVectAddr0)[IRC_PRIORITY] = (unsigned long)EINT2_rutine; // Nastaveni adresy vektotu preruseni
+ ((uint32_t*)&VICVectCntl0)[IRC_PRIORITY] = VIC_ENABLE | VIC_EINT2; // choice EINT2 as source of interrupt
+ VICIntEnable |= (1 << VIC_EINT2);//0x15 // enable interrupt from EINT2 source
+}
+
+
+
+void front_back_test(void){
+ unsigned char i;
+ match_IRC_pusher = 160; //cca 2cm
+ cbit(flags,TIME_ERR);
+ last_time = get_TMR0();//save time when motore start
+ move_pusher(ENGINE_FW, 50);// 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
+ for(i=0; i<10; i++){
+ cbit(flags,TIME_ERR);
+ last_time = get_TMR0();//save time when motore start
+ cnt_IRC_pusher = 0; //clear counter of IRC for pusher
+ move_pusher(ENGINE_FW, 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
+ engine_B_pwm(0); // STOP pwm is in percent, range 0~100~200
+
+ 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) && (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
+ 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
+ engine_B_pwm(0); // STOP pwm is in percent, range 0~100~200
+ cbit(flags, TIME_ERR);
}
+}
-/**
- * Inicializes pepherials used in ebb library - sample use
- *
- */
-void init_perip(void)
-{
- init_servo(SERVO_ISR);
- init_pow_switch();
- init_engine_A();
- init_engine_B();
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void set_holder(unsigned char range){
+ if((range > HOLDER_MIN)&&(range < HOLDER_MAX)){ // servo can be moved in limitary range (lift palate)
+ set_servo(range);
+ }
+ else{
+ if(range < HOLDER_MIN)
+ set_servo(HOLDER_MIN);
+ else
+ set_servo(HOLDER_MAX);
+ }
+}
+
- init_uart0((int)9600 ,UART_BITS_8, UART_STOP_BIT_1, UART_PARIT_OFF, 0 );
- init_adc(ADC_ISR); // inicialization ADC with averaging filter
- //init_adc_filter(ADC_ISR); // inicialization ADC with mean filter
- can_init_baudrate(CAN_SPEED, CAN_ISR, can_rx);
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+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
+ T0TCR = 0x01; // start timer 0
}
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+inline unsigned int get_TMR0(void){
+ return T0TC;// return actual value of timer
+}
-/**
- * Main function contains a small sample how to use ebb library.
- *
- */
-int main (void) {
- init_perip(); // sys init MCU
- set_servo(0, 0x80); // set servo 1 to center position
- set_servo(1, 0x80); // set servo 2 to center position
- set_servo(2, 0x80); // set servo 3 to center position
- engine_A_dir(ENGINE_DIR_FW); // set engine A to direction forward
- engine_B_dir(ENGINE_DIR_FW); // set engine B to direction backward
- engine_A_en(ENGINE_EN_ON); // enables engine A
- engine_B_en(ENGINE_EN_ON); // enables engine B
- engine_A_pwm(SPEED); // sets speed on Engine A
- engine_B_pwm(SPEED); // sets speed on Engine B
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void CAN_rx(can_msg_t *msg) {
+ can_msg_t rx_msg;
+ memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
- can_msg_t msg; // This part is sending CAN messge
- msg.id = 0xAA; // CAN message ID
- msg.flags = 0;
- msg.dlc = 1; // number of transmited data bytes
- msg.data[0] = 0x55; // data byte
- while(can_tx_msg(&msg)); // sending function
+ 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;
+
+ default:break;
+ }
+}
+
- while(1) // this will blink for ever
- {
- deb_led_on(LEDG);
- dummy_wait();
- deb_led_off(LEDG);
- dummy_wait();
- }
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+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 lst_CAN_time;
+ act_position_lift = 0;
+ act_position_pusher = 0;
+ flags = 0; // reset all flags
+ 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_ports();
+ init_interrupts(IRC_PRIORITY);
+ 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 = pusher_init();// initialization of push unit
+ //;//current_state=err_init(EVEN_ENTRY);//goto error state and send message to major unit
+ //front_back_test();
+
+
+//// ~~~~~ no error >> initialization successfull completed
+ 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);
+
+//// ~~~~~ 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
+
+
+
+ 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);
+ }
}
--- /dev/null
+#include <lpc21xx.h> /* LPC21xx definitions */
+#include <deb_led.h>
+#include <system_def.h>
+#include <can_ids.h>
+#include <periph/can.h>
+#include <string.h>
+#include <deb_led.h>
+#include "serv.h"
+#include "engine.h"
+#include "lift.h"
+#include "pusher.h"
+#include "uar.h"
+#include "def.h"
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~ Interrupt service rutines ~~~~~~~~~~~~~~~
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void EINT2_rutine(void){
+ EXTINT |= EXTINT_EINT2_m;
+ deb_led_change(LEDR);
+ ++cnt_IRC_pusher;
+ VICVectAddr = 0;
+}
+
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+error_messages pusher_init(void){
+ error_messages err = NO_ERROR;
+ //if(tbit(flags,PUCK_LOADED))
+ // return(ERROR_PUCK_LOADED);// no initialization is possible if puck is loaded in holder
+// if(!(tbit(flags,HOLDER_OPEN)))
+// return(ERROR_HOLDER_OPEN);// no initialization is possible if puck is loaded in holder
+
+ cbit(flags,TIME_ERR);
+ last_time = get_TMR0();//save time when motore start
+ move_pusher(ENGINE_FW, 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
+ 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 + PUSHER_TIMEOUT/3))//check timeout
+ sbit(flags, TIME_ERR);
+ }
+ 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
+ cbit(flags, TIME_ERR);
+ return(err);
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+void move_pusher(unsigned char direct,unsigned char velocity){//how many pulzes
+ engine_B_dir(direct);//set direction for lift
+ engine_B_pwm(velocity); // set velocity, range 0~100~200
+ engine_B_en(ENGINE_EN_ON);//enable motor of pusher
+}
+
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+unsigned int count_pusher_IRC(unsigned int req_pos, unsigned int act_pos){
+ unsigned int irc;
+ if(req_pos > act_pos){
+ pusher_dir = ENGINE_FW; //set the direction of pusher moving
+ irc = req_pos - act_pos;
+ }
+ else{
+ pusher_dir = ENGINE_BW;
+ irc = act_pos - req_pos;
+ }
+ return irc;
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+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_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, 40);
+ last_time = get_TMR0();//save time when moving start
+ break;
+ case EVENT_DO:
+ //wait for commands
+ if((get_TMR0() >= last_time + PUSHER_TIMEOUT))
+ 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:
+
+ break;
+ default:break;
+ }
+}
--- /dev/null
+#ifndef __PUSHER_H
+#define __PUSHER_H
+
+#include "def.h"
+
+//************************************************************************************
+//******************** declaration of interrupt service rutines **********************
+//************************************************************************************
+ void EINT2_rutine(void) __attribute__ ((interrupt));
+
+//************************************************************************************
+//******************** declaration of users methods **********************************
+//************************************************************************************
+ error_messages pusher_init(void);// calibration and move to start position
+ void move_pusher(unsigned char direct,unsigned char velocity);
+ unsigned int count_pusher_IRC(unsigned int req_pos, unsigned int act_pos);
+
+
+//************************************************************************************
+//****************** declaration of possible states **********************************
+//************************************************************************************
+ void stop_pusher(struct fsm *fsm, events my_event);
+ void move_pusher_pos(struct fsm *fsm, events my_event);
+
+
+
+
+unsigned char pusher_dir;
+volatile unsigned int act_position_pusher, last_position_pusher, req_position_pusher;
+unsigned volatile int match_IRC_pusher, cnt_IRC_pusher; //register which cnt_IRC will be compared with
+volatile uint32_t can_req_position_pusher;
+
+
+#endif /*__PUSHER_H*/
projects / eurobot / public.git / commitdiff