-/**
- * Program carousel
- * controll the carousel wheel used in robot for eurobot 2008
- * program recieve commands telling the position carousel should be
- * positioned and send back this position
- *
- * created by Ondrej Vrzal, ondra.vrzal@gmail.com
- */
-
-/*
- need to implement to move a bit backward after error with not moving motor happes
-*/
-
-
#include <deb_led.h>
#include <uart.h>
#include <engine.h>
#define CAN_SPEED 1000000
#define CAN_ISR 0
-#define ENG_ISR 1
-// the interrupt for the timer is somehow set in this class perhaps 4
+#define ENG_ISR 1 ///< the interrupt for the timer is somehow set in this class perhaps 4
+
#define SERVO_ISR 5
-#define FULLSPEED 50
-#define MIDSPEED 20
-#define LOWSPEED 10
+#define FULLSPEED 50 ///< the speed of motors while turn at FULLSPEED
+#define MIDSPEED 20 ///< the speed of motors while turn at MIDSPEED
+#define LOWSPEED 10 ///< the speed of motors while turn a LOWSPEED - used to accurate move to possition
-#define INIT_POSITION 0
+#define INIT_POSITION 0 ///< the initial position of carousel, could be 0..3
-#define GLOBALTIMEOUT 6000
+#define GLOBALTIMEOUT 6000 ///< the timeout for carousel to reach requested position
+
+/**
+ * Program carousel
+ * - controll the carousel wheel used in robot for eurobot 2008
+ * - program recieves CAN commands about wanted carousel position
+ * - program moves the carousel
+ * - sends on CAN the carousel position
+ *
+ * created by Ondrej Vrzal, ondra.vrzal@gmail.com
+ */
-enum Event {EVENT_ENTRY, EVENT_DO, EVENT_EXIT};
-volatile unsigned int validReading= 0; // 0 - carusel position signal 0- not valid, 1- valid
-volatile unsigned int validPosition= 0; // 0..4 read position from carusel
+enum Event {EVENT_ENTRY, EVENT_DO, EVENT_EXIT}; ///< events of each state of state machine
-//unsigned int timeNextPos= 300; // default time to move to next postion, initialized on the beginning
-//unsigned int timeNext2Pos= 600; // default time to move to next postion, initialized on the beginning
-unsigned int timeNextPos= 232; // default time to move to next postion, initialized on the beginning
-unsigned int timeNext2Pos= 424; // default time to move to next postion, initialized on the beginning
+volatile unsigned int validReading= 0; ///< 0 - carusel position signal 0- not valid, 1- valid
+volatile unsigned int validPosition= 0; ///< 0..4 read position from carusel
+//unsigned int timeNextPos= 300;
+//unsigned int timeNext2Pos= 600;
+unsigned int timeNextPos= 232; ///< default time to move to next postion
+unsigned int timeNext2Pos= 424; ///< default time to move to next postion
-volatile unsigned int requestedPosition= 0; // the positon the carusel should move to
-volatile unsigned int moveCarousel= 0; // request to move the carusel
-enum Direction {FORWARD, BACKWARD}; // forward - 4,3,2,1,0, backward= 0,1,2,3,4
-enum Direction moveDirection= FORWARD; // the direction carousel should move
-unsigned int stepsToPos= 1; // distance to reguested position
-unsigned int flashLEDB= 0; // when set blue led will flash; the sensors sense new valid position (overshoot position)
-unsigned int flashLEDY= 0; // when set blue led will flash; the requested position was set
+volatile unsigned int requestedPosition= 0; ///< the positon the carousel should move to
+volatile unsigned int moveCarousel= 0; ///< request to move the carousel
+enum Direction {FORWARD, BACKWARD}; ///< FORWARD - 4,3,2,1,0, BACKWARD= 0,1,2,3,4
+enum Direction moveDirection= FORWARD; ///< the direction carousel should move
+unsigned int stepsToPos= 1; ///< distance to reguested position
-unsigned int sendCANmessage= 0; //flag to sent CAN data
-unsigned int sendCANOKdata= 0; // data to send
-unsigned int sendCANERRdata= 0; // data to send
-volatile unsigned int recieveCANmessage= 0; //flag to sent CAN data
-volatile unsigned int recieveCANdata= 0; // data to send
+unsigned int flashLEDB= 0; ///< when 1, blue led will flash when the sensors sense new valid position (overshoot position)
+unsigned int flashLEDY= 0; ///< when 1 Yellow led will flash when the requested position was set
-unsigned int systemInit=0; // flag that show that carousel is just initialising its constants
+unsigned int sendCANmessage= 0; ///< flag to sent CAN data
+unsigned int sendCANOKdata= 0; ///< flag data to send - OK
+unsigned int sendCANERRdata= 0; ///< flag data to send - ERROR
+volatile unsigned int recieveCANmessage= 0; ///< flag indicated CAN data was recieved
+volatile unsigned int recieveCANdata= 0; ///< recieved CAN data
-const unsigned int useKeyboard=0; // debuging - the next position of carousel is set by terminal
-const unsigned int useArray= 0; // debuging - next position is generated automaticly using the array of possition
-const unsigned int doInitCarousel= 0; // debuging - if set, program will count constants to move carousel
+unsigned int systemInit=0; ///< flag that show that carousel is just initialising
-unsigned int carouselTimeOut= 0;// global timeout, the requested position is not reachable
+const unsigned int useKeyboard=0; ///< debuging - the next position of carousel is set by terminal
+const unsigned int useArray= 0; ///< debuging - next position is generated automaticly using the array of possition
+const unsigned int doInitCarousel= 0; ///< debuging - if set, program will count constants to move carousel, not really the best ones..
+
+unsigned int carouselTimeOut= 0;///< global timeout, the requested position is not reachable
/**
* information about error states
- * ERROR_SENSOR - motor should move, so sensor should sense somethign, but doesn't sense anything
- * ERROR_POSITION - the position isn't read while rotating carousel
- * ERROR_TIMEOUT - the requested position wasn't reached in timeout = carouselTimeout
+ * - ERROR_SENSOR - motor should move, so sensor should sense somethign, but doesn't sense anything
+ * - ERROR_POSITION - the position isn't read correctly while rotating carousel
+ * - ERROR_TIMEOUT - the requested position wasn't reached in timeout = carouselTimeout
+ * - NOTICE - some small error
*/
enum ErrorState {NO_ERROR=0, ERROR_SENSOR=1, ERROR_POSITION=2, ERROR_TIMEOUT=3, NOTICE=5};
enum ErrorState errorState= NO_ERROR;
unsigned int errorNumber= 0;
-unsigned int wasError= 0; // flag that says, that there were an error, this flag is cancelled while new position is received
+unsigned int wasError= 0; ///< flag that says, that there were an error, this flag is cancelled while new position is received
-can_msg_t msg; // pointer to the recieved can message
+can_msg_t msg; ///< pointer to the recieved can message
void init_perip(void);
static void Init_timer(void);
void delay(int interval);
void init_carousel(void);
unsigned int getCurrentPosition(void);
-void gotoPos(unsigned int position); // !!! depricated
void generatePositions(void);
void go_to_sleep();
unsigned int getTime(void);
void can_rx(can_msg_t *msg);
-// main loop
+// declaration for main loop
void handle_CAN(void);
void blink_LED(void);
void updatePosition(void);
-// state space automat
+/**
+ * state space automat
+ * - used to change states
+ * - each state has three "events"
+ * -# EVENT_ENTRY
+ * -# EVENT_DO
+ * -# EVENT_EXIT
+ * - states are
+ * -# state_stop
+ * -# state_moveSlow
+ * -# state_moveFast
+ * -# state_returnToPosition
+ */
typedef void (*state_fcn)(enum Event my_event);
-state_fcn current_state; // current state of carousel, stop, fast_move, slow_move
+state_fcn current_state; ///< current state of carousel, stop, fast_move, slow_move, returnToPosition
+
void state_stop(enum Event my_event);
void state_moveSlow(enum Event my_event);
// volatile int delay_done=0 ;
+/**
+ *
+ * Main loop
+ *
+ * In main loop
+ * - At the beginning
+ * - ARM is initialised
+ * - Carousel is initialised and moved to INIT_POSITION
+ * - The first state is state_stop
+ * - Then in infinet loop
+ * -# The state space machine runs
+ * -# Check for carouselTimeOut
+ * -# Handle CAN
+ * -# Blink LEDs
+ * -# Update positions
+ * -# Check for state change
+ */
int main (void)
{
systemInit= 1; // beginning of initialisation
uart0SendCh(text[n]);
}
- // wait for motors to stop, needed after programming and reset
+ // wait for motors to stop, needed after programming and reset
unsigned int time_timeout = getTime() + 2000;
while(getTime() < time_timeout) blink_LED();
engine_A_en(ENGINE_EN_ON);
}
} // end of main function
+/**
+ * initialisation of ARM periphery
+ * - servo
+ * - power switch
+ * - motor that moves carousel
+ * - timer
+ * - UART0
+ * - CAN
+ */
void init_perip(void) // inicializace periferii mikroprocesoru
{
init_servo(SERVO_ISR);
}
-/**
+/*
void timer0_isr(void) __attribute__ ((interrupt));
// while interrupt do nothing
T0IR |= 0x00000001; // Clear match0 interrupt
VICVectAddr = 0x00000000;
}
-**/
+*/
/**
* initializes timer T0 to raise an interrupt and to reset T0counter when it matches T0MatchRegister0
- * @param
*/
static void Init_timer(void)
{
}
+
+// handle_CAN(void) variable declarations
+unsigned int repeatTXCANtime= 0;
+unsigned int repeatErrorCANtime= 0;
+unsigned int repeatErrorCAN=0;
+unsigned int prevRequestedPos= INIT_POSITION +1;///< at start robot should move therefore the initial position has to be different than this position
+
/**
* handle CAN bus
- * check if carousel position was reached. if so sends the reached (requested) position to CAN
- * if CAN message was received set the new requested position if in stop (ready state),
- * if positioning of the carousel is in progress and different position is requested
- * make next state stateStop and set new requested position
- * if debuging is set, the position of carousel is controlled by the the keyboard or
+ * - send messages
+ * - if carousel position was reached - send the position on CAN and repeat to send the position till the time new position is requested. CAN id is CAN_CAROUSEL
+ * - if there is error in moving carousel send message with CAN_ERROR id.
+ * - recieve message
+ * - if new position is recieved and is in state_stop, request new position
+ * - if positioning of the carousel is in progress and different position is requested
+ * make next state state_stop and set new requested position
+ * -if debuging is set, the position of carousel is controlled by the the keyboard or
* by array of positions
*/
-unsigned int repeatTXCANtime= 0;
-unsigned int repeatErrorCANtime= 0;
-unsigned int repeatErrorCAN=0;
-unsigned int prevRequestedPos= INIT_POSITION +1;// at start robot should move therefore the initial position has to be different than this position
void handle_CAN(void)
{
//-----------------------------------
msg.dlc = 1;
msg.data[0] = sendCANOKdata;
- // !! what it the message would not be sent??
+ // !! what if the message would not be sent??
while(can_tx_msg(&msg));
repeatTXCANtime= getTime() + 1000;
}
}
-
// repeated sending of the position, when in state, that requested position was set
if ( (moveCarousel==0)&&(wasError==0) )
{
}
}
-/**
- * used for debuging to generate new positions for the carousel
- */
+// generatePositions(void) declarations
int prevMoveCarousel= 0;
-const unsigned int sequens[14]= {0,1,2,3,4,3,2,1,0,2,4,3,1,4};
+const unsigned int sequens[14]= {0,1,2,3,4,3,2,1,0,2,4,3,1,4}; ///< sequence of carousel positions, used for debuging
unsigned int sequensX= 0;
unsigned int readyToGo=0;
unsigned int timeToGo=0;
+/**
+ * used for debuging to generate new positions for the carousel
+ * new position could be set by keyboard or read from array sequens[]
+ *
+ */
void generatePositions(void)
{
// when the carousel has achieved the final position
}
-/**
- * blink with the LEDs located on eb_board
- * green LED blink constantly
- * red LED blinks when errorState is ERROR
- */
+// void blink_LED(void) declarations
unsigned int okTimeToBlink= 0;
unsigned int okLEDstate= 0;
enum ErrorState lastErrorState= NO_ERROR; // used to keep information about error, used to cover the case, when NOTICE change errorState from ERROR
+/**
+ * Blink with the LEDs located on eb_board
+ * - green LED blink constantly
+ * - red LED blinks when errorState is ERROR, when erroeState is NOTICE, LED blinks for 1 sec
+ * - yellow LED blink when the requested position was reached
+ */
void blink_LED(void)
{
unsigned int curr_time= getTime();
/**
- * initial state where the motor is stoped
+ * State space machine state: state_stop
+ * The initial state where the motor is stoped and Carousel is waiting for commands to move to requested postion
+ * - on EVENT_ENTRY:
+ * - Stop the motors
+ * - on EVENT_DO:
+ * - Wait for moveCarousel==1 . If requested position is read on sensors - wait for new request. If requeset position is valid position (last requested) but not read from the sensors, new state is state_returnToPosition. If the requested position is different from the last one. then find out the direction to move and change state to state_moveFast.
+ * - on EVENT_EXIT:
+ * - nothing
*
*/
-
void state_stop(enum Event my_event)
{
switch (my_event ){
}
}
+
+unsigned int stopFaMoveTime=0;
/**
- * moving the carousel fast forward towards the right position
+ * State space machine state: state_moveFast
+ * - on EVENT_ENTRY:
+ * - Moving the carousel fast forward towards the right position - turn on the motors fast.
+ * - on EVENT_DO:
+ * - When the carousel overshoot the right position - stop, the next state is state_returnToPosition.
+ * - on EVENT_EXIT:
+ * - stop motors
*
*/
-unsigned int stopFaMoveTime=0;
void state_moveFast(enum Event my_event)
{
switch (my_event ){
}
}
-/**
- * moves the carusel slowly towards the righ position
- * when when the right position is passed go to next state
- */
+
+// state_moveSlow declarations
unsigned int stopSlMoveTime=0;
const unsigned int moveSlowTimeOut= 2000;
+/**
+ * State space machine state: state_moveSlow
+ * - on EVENT_ENTRY:
+ * - moves the carusel slowly towards the righ position
+ * - on EVENT_DO:
+ * - when when the right position is passed go to state_returnToPosition
+ * - when the position isn't reached in moveSlowTimeOut go to state state_moveFast
+ * - on EVENT_EXIT:
+ * - stop motors
+ *
+ */
void state_moveSlow(enum Event my_event)
{
switch (my_event ){
}
-/**
- * the requsted position was passed, move carusel a bit backwards
- *
- */
+// state_returnToPosition declarations
const unsigned int returnTimeOut= 2000; // max duration of the state
unsigned int returnStopTime; // time to of the time out
//unsigned int requestedPosOK= 0;
+/**
+ * State space machine state: state_returnToPosition
+ * - on EVENT_ENTRY:
+ * - moves the carusel slowly backwards to the righ position
+ * - on EVENT_DO:
+ * - when when the right position is reached go to state_stop and send the CAN message
+ * - when the position isn't reached in returnTimeOut go to state state_moveFast
+ * - on EVENT_EXIT:
+ * - stop motors
+ *
+ */
void state_returnToPosition(enum Event my_event)
{
switch (my_event ){
}
+// getCurrentPosition variables declarations
+const unsigned int signalToInt[7]= {11,0,3,4,1,2,10}; ///< used to decode signal from sensors to integer; 0..4 is position, 10 means error, 11 does'n sense any position
+const unsigned int intToSignal[5]={1,4,5,2,3};///< used to get signals from sensors for the position (0..4)
+
/**
* reads the sensors
- * returns: position 0..4 if sense sone data, 10 means error, 11 does'n sense any position
* !caution! read position can be errorneous
+ * @param
+ * @return position 0..4 if sense sone data, 10 means error, 11 does'n sense any position
*/
-// used to decode signal from sensors to integer; 0..4 is position, 10 means error, 11 does'n sense any position
-const unsigned int signalToInt[7]= {11,0,3,4,1,2,10};
-const unsigned int intToSignal[5]={1,4,5,2,3};
-
unsigned int getCurrentPosition(void)
{
unsigned int input;
}
/**
- * count the distance between 2 position
- * pos1 new position, pos2 old position - 0..4
- * return -2..+2 the distance, plus distace mean that pos 1 is forward to pos 2
+ * Count the distance between 2 position
+ * @param pos1 0..4, new position
+ * @param pos2 0..4, old position
+ * @return -2..+2 the distance, positive distace mean that pos 1 is forward to pos 2
*/
signed int positionToDistance(unsigned int pos1, unsigned int pos2)
{
}
/**
- * count new position
- * input position 0..4, distance -4..+4
- * return position 0..4
+ * Count new position
+ * @param position 0..4, the current position
+ * @param distance -4..+4, distance from the current position, + forward, - backward from current position
+ * @return 0..4, new position
*/
unsigned int distanceToPos(unsigned int position, signed int distance)
{
return absolutePos;
}
-/**
- * check the sensors
- * has three states - waiting for signal, signal was read, signal is valid
- * if signal is valid set the value int validPos=1, in the other code it has to be set to 0 in other code
- * it also set the value readPositon= n, where n(0,4) is the last read valid position
- *
- * here is also checked for an errors in reading signal
- */
-unsigned int stateReading=0; // 0- no signal, 1- some signal
-unsigned int reading= 0; // signal that is read
+// updatePosition variable declarations
+unsigned int stateReading=0; ///< 0- no signal, 1- some signal
+unsigned int reading= 0; ///< signal that is read
unsigned int prevValidPosition= 0;
-unsigned int prevMoveDirection= FORWARD; // init direction should be forward!!
+unsigned int prevMoveDirection= FORWARD; ///< init direction should be forward!!
-unsigned int signalTimeoutOn= 0; // flag that show if carousel is moving and is checked if new signal comes before timeout
+unsigned int signalTimeoutOn= 0; ///< flag that show if carousel is moving and is checked if new signal comes before timeout
unsigned int lastSignaltime=0;
-unsigned int errorPosRead[5]= {0,0,0,0,0}; // how many times positon was read wrong. once read right, reset this number
+unsigned int errorPosRead[5]= {0,0,0,0,0}; ///< how many times positon was read wrong. once read right, reset this number
+/**
+ * Check the sensors
+ * Has three states
+ * - waiting for signal
+ * - signal was read
+ * - signal is valid
+ * If signal is valid set the value int validPos=1, in the other code it has to be set to back 0.
+ * It also set the value readPositon= n, where n(0,4) is the last read valid position
+ *
+ * In this function it is also checked for an errors in reading signal
+ */
void updatePosition(void)
{
unsigned int readPosition= getCurrentPosition();
}
}
-/**
- * initialize constants for positioning carousel
- * move n times by set speed and measure the time interval
- */
+// init_carousel variable declarations
unsigned int initPos= 0; // initial position
unsigned int setConstants= 1; // should the robot initialize the constants for moving carousel?
+/**
+ * Initialize constants for positioning carousel.
+ * Move n times by set speed and measure the time interval
+ * Not very accurate, it is better to set constants manualy
+ *
+ */
void init_carousel(void)
{
uart0SendCh('i');
/**
* handels messages recieved by the CAN controller
- *
+ * @param msg the recieved CAN message pointer
*/
void can_rx(can_msg_t *msg) {
can_msg_t rx_msg;
}
}
+/**
+ * set carousel to sleep caused by CAN_PWR_ALERT CAN message
+ */
void go_to_sleep()
{
engine_A_en(ENGINE_EN_OFF); // engines off
}
-
/**
- * returns the number in T0TC
- *
+ * used as timer
+ * @param
+ * @return returns the number in timer T0TC
*/
unsigned int getTime(void)
{
projects / eurobot / public.git / commitdiff