From ea7c51021482ab1fe2262dd05b953111faf55741 Mon Sep 17 00:00:00 2001 From: Ondra Vrzal Date: Sun, 27 Apr 2008 16:34:45 +0200 Subject: [PATCH] implement timeouts for state machine, during initialisation LED is blinking, changed constants of carousel for faster positioning Ondra --- src/eb_carousel/carousel.c | 333 ++++++++++++++++++++++++------------- 1 file changed, 217 insertions(+), 116 deletions(-) diff --git a/src/eb_carousel/carousel.c b/src/eb_carousel/carousel.c index e9ac0bfa..dd681664 100644 --- a/src/eb_carousel/carousel.c +++ b/src/eb_carousel/carousel.c @@ -21,7 +21,7 @@ -#define FULLSPEED 40 +#define FULLSPEED 50 #define MIDSPEED 20 #define LOWSPEED 10 @@ -39,12 +39,20 @@ volatile unsigned int requestedPosition= 0; // the positon the carusel should mo 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 unsigned int sendCANmessage= 0; //flag to sent CAN data unsigned int sendCANdata= 0; // data to send +volatile unsigned int recieveCANmessage= 0; //flag to sent CAN data +volatile unsigned int recieveCANdata= 0; // data to send + +unsigned int systemInit=0; // flag that show that carousel is just initialising its constants + +const unsigned int useKeyboard=0; // debugging - the next position of carousel is set by terminal +const unsigned int useArray= 1; // debugging - next position is generated automaticly using the array of possition enum ErrorState errorState= NO_ERROR; @@ -55,13 +63,14 @@ static void Init_timer(void); void delay(int interval); void init_carousel(void); unsigned int getCurrentPosition(void); -void gotoPos(unsigned int position); +void gotoPos(unsigned int position); // !!! depricated +void generatePositions(void); + unsigned int getTime(void); void can_rx(can_msg_t *msg); // main loop void handle_CAN(void); -void send_data(void); void blink_LED(void); void updatePosition(void); @@ -79,10 +88,11 @@ void state_returnToPosition(enum Event my_event); void init_perip(void) // inicializace periferii mikroprocesoru { - //init_servo(SERVO_ISR); + init_servo(SERVO_ISR); init_pow_switch(); init_engine_A(); + engine_A_en(ENGINE_EN_OFF); //init_engine_B(); // init timer 0 @@ -97,54 +107,55 @@ void init_perip(void) // inicializace periferii mikroprocesoru // volatile int delay_done=0 ; int main (void) { - + systemInit= 1; // beginning of initialisation init_perip(); - moveCarousel= 0; - + char text[3]= {'s',0x0D,0x0A}; + int n; + for(n=0;n<3;n++) + { + uart0SendCh(text[n]); + } + + // wait for motors to stop + unsigned int time_timeout = getTime() + 2000; + while(getTime() < time_timeout) blink_LED(); + engine_A_en(ENGINE_EN_ON); + + moveCarousel= 0; + init_carousel(); // set the constants for positioning carousel + // send information on the CAN, that carusel is now at the right position msg.id = CAN_CAROUSEL; msg.flags = 0; msg.dlc = 1; - msg.data[0] = 0x0ff; - - // !! what it the message would not be sent?? + msg.data[0] = validPosition; + // !! what it the message would not be sent?? while(can_tx_msg(&msg)); - - - uart0SendCh(0x0D); - uart0SendCh(0x0A); - - init_carousel(); // set the constants for positioning carousel - char text[6]= {'i','n','i','t',0x0D,0x0A}; - int n; - for(n=0;n<6;n++) - { - uart0SendCh(text[n]); - } - // start the stop state of carousel current_state= &state_stop;; // current state of carousel state_fcn last_state= &state_stop;; - current_state(EVENT_ENTRY); + current_state(EVENT_ENTRY); - requestedPosition= 3; // the positon the carusel should move to + requestedPosition= 0; // the positon the carusel should move to moveCarousel= 1; // request to move the carusel moveDirection= FORWARD; + systemInit= 0; // end of initialisation //errorState= ERROR; unsigned int waitAwhile= getTime()+ 5000; while (1){ + last_state= current_state; // track changes in state + current_state(EVENT_DO); + handle_CAN(); - send_data(); + blink_LED(); updatePosition(); - last_state= current_state; // track changes in state - current_state(EVENT_DO); if(last_state!=current_state) { // in case of changing state last_state(EVENT_EXIT); // finish the old state @@ -201,22 +212,14 @@ static void Init_timer(void) * * CAN temporary simulated by pressing the keys, unfortunately stops the state machine */ -int prevMoveCarousel=0; -const unsigned int useKeyboard=0; -const unsigned int useArray=1; -const unsigned int sequens[14]= {0,1,2,3,4,3,2,1,0,2,4,3,1,4}; -unsigned int sequensX= 0; - -unsigned int readyToGo=0; -unsigned int timeToGo=0; void handle_CAN(void) { - // !! need to be implemented - unsigned int implement=1; - implement++; + //----------------------------------- + // THIS PART HANDLE SENDING MESSAGE // send information on the CAN, that carusel is now at the right position - if ( (sendCANmessage)&&(useKeyboard==0)&&(useArray==0) )// real mode should used rx, tx CAN data + + if (sendCANmessage)// real mode should used rx, tx CAN data { sendCANmessage=0; msg.id = CAN_CAROUSEL; @@ -228,8 +231,71 @@ void handle_CAN(void) while(can_tx_msg(&msg)); } - // auto setting new the positions, used for debugging - else if( (moveCarousel==0)&&(prevMoveCarousel!=0) ) + //----------------------------------- + // THIS PART HANDLE RECIEVED MESSAGE + + // checked if message was recieved + if ( (useKeyboard==1)||(useArray==1) ) generatePositions(); // debuging insted of can keyboard or array of positions is used + else if ( (recieveCANmessage) && (systemInit==0) ) // process the CAN message only if everything in initialized + { + recieveCANmessage= 0; + unsigned int newPos= recieveCANdata; // recieved postion + // check for valid position + if ( (newPos>=0) && (newPos<=4) ) + { + // when carousel is already moving + if(moveCarousel==1) + { + if (newPos== requestedPosition) + { + // BOA is repeating sending the position + // moving to this position is already in progress + // IGNORED + } + else + { + // while carousel was moving new position was requested + current_state= &state_stop; + requestedPosition= newPos; + } + } + // carousel is in stop state, carousel oriented in requested position + else + { + if( newPos==requestedPosition) + { + // carousel is already in this position + // send message that i am in this position + // !!! could be valid position and requested position different?? + sendCANmessage=1; + sendCANdata= requestedPosition; + } + { + // task to move to new position + moveCarousel= 1; + requestedPosition= newPos; + current_state= &state_stop; + } + } + + } + } +} + +/** + * used for debuging to generate new positions for the carousel + */ +int prevMoveCarousel= 0; +const unsigned int sequens[14]= {0,1,2,3,4,3,2,1,0,2,4,3,1,4}; +unsigned int sequensX= 0; + +unsigned int readyToGo=0; +unsigned int timeToGo=0; + +void generatePositions(void) +{ + // when the carousel has achieved the final position + if ( (moveCarousel==0)&&(prevMoveCarousel!=0) ) { if (useKeyboard) { @@ -251,7 +317,6 @@ void handle_CAN(void) timeToGo = getTime() +2000; } } - if (readyToGo) { if (timeToGo< getTime()) @@ -263,18 +328,9 @@ void handle_CAN(void) } } - prevMoveCarousel= moveCarousel; - + prevMoveCarousel= moveCarousel; } -/** - * send data, should send data?? perhaps will be done already in handle_CAN - */ -void send_data(void) -{ - unsigned int implement=1; - implement++; -} /** * blink with the LEDs located on eb_board @@ -283,20 +339,15 @@ void send_data(void) */ unsigned int okTimeToBlink= 0; unsigned int okLEDstate= 0; -const unsigned int okLEDinterval= 500; unsigned int errTimeToBlink= 0; unsigned int errLEDstate= 0; -const unsigned int errLEDinterval= 200; -const unsigned int noticeLEDinterval= 1000; unsigned int blueTimeToBlink= 0; unsigned int blueLEDstate= 0; -const unsigned int blueLEDinterval= 100; unsigned int yBlinkTime=0; unsigned int yLEDstate= 0; -const unsigned int yLEDinterval= 1000; enum ErrorState lastErrorState= NO_ERROR; // used to keep information about error, used to cover the case, when NOTICE change errorState from ERROR @@ -305,7 +356,7 @@ void blink_LED(void) unsigned int curr_time= getTime(); if (okTimeToBlink< curr_time) { - okTimeToBlink= curr_time + okLEDinterval; + okTimeToBlink= curr_time + 500; okLEDstate= ~okLEDstate; if (okLEDstate) deb_led_on(LEDG); else deb_led_off(LEDG); @@ -317,7 +368,7 @@ void blink_LED(void) if (errTimeToBlink< curr_time) { - errTimeToBlink= curr_time + errLEDinterval; + errTimeToBlink= curr_time + 200; errLEDstate= ~errLEDstate; if (errLEDstate) deb_led_on(LEDR); else deb_led_off(LEDR); @@ -334,7 +385,7 @@ void blink_LED(void) else if(errLEDstate==0) { errLEDstate= 1; - errTimeToBlink= getTime() +noticeLEDinterval; + errTimeToBlink= getTime() +1000; deb_led_on(LEDR); } else if (errTimeToBlink< curr_time) @@ -353,7 +404,7 @@ void blink_LED(void) flashLEDB= 0; blueLEDstate=1; deb_led_on(LEDB); - blueTimeToBlink= getTime() +blueLEDinterval; + blueTimeToBlink= getTime() +100; } // after specified interval turn off the blue LED if(blueLEDstate==1) @@ -371,7 +422,7 @@ void blink_LED(void) flashLEDY= 0; yLEDstate=1; deb_led_on(LEDY); - yBlinkTime= getTime() +yLEDinterval; + yBlinkTime= getTime() +1000; } // after specified interval turn off the blue LED if(yLEDstate==1) @@ -392,7 +443,6 @@ void blink_LED(void) * */ -unsigned int stepsToPos= 1; // distance to reguested position void state_stop(enum Event my_event) { switch (my_event ){ @@ -410,6 +460,9 @@ void state_stop(enum Event my_event) { // if the position is already set, don't do anything + // !!! this solved also in handle_CAN !!! + // !! also valid position could not be valit, current_position is important + // also, when the motors are turning, the current_position could be mistaken.. if (requestedPosition==validPosition){ moveCarousel=0; stepsToPos= 0; @@ -460,7 +513,7 @@ void state_stop(enum Event my_event) * moving the carousel fast forward towards the right position * */ -unsigned int stopMoveTime=0; +unsigned int stopFaMoveTime=0; void state_moveFast(enum Event my_event) { switch (my_event ){ @@ -471,8 +524,13 @@ void state_moveFast(enum Event my_event) engine_A_pwm(FULLSPEED); engine_A_en(ENGINE_EN_ON); - //measure the time interval - stopMoveTime= getTime() + timeNextPos; + + // measure the time interval + // what if stepsToPos==0? !!! + if (stepsToPos==1) stopFaMoveTime= getTime() + timeNextPos; + else if (stepsToPos==2) stopFaMoveTime= getTime() + timeNext2Pos; + else stopFaMoveTime= getTime() + 4*timeNext2Pos; // if problems to reach the position just turn fast a while + uart0SendCh('b'); break; @@ -489,7 +547,7 @@ void state_moveFast(enum Event my_event) } // after some delay change the speed of the motor - if (stopMoveTime< getTime()) + if (stopFaMoveTime< getTime()) { current_state= &state_moveSlow; } @@ -509,6 +567,8 @@ void state_moveFast(enum Event my_event) * moves the carusel slowly towards the righ position * when when the right position is passed go to next state */ +unsigned int stopSlMoveTime=0; +const unsigned int moveSlowTimeOut= 2000; void state_moveSlow(enum Event my_event) { switch (my_event ){ @@ -519,6 +579,8 @@ void state_moveSlow(enum Event my_event) engine_A_pwm(MIDSPEED); engine_A_en(ENGINE_EN_ON); + stopSlMoveTime= getTime() + moveSlowTimeOut; + uart0SendCh('c'); break; case EVENT_DO: @@ -532,6 +594,11 @@ void state_moveSlow(enum Event my_event) current_state= &state_returnToPosition; } } + // almost error state, moving slow too long time, move fast to reach the position + if (getTime() > stopSlMoveTime) + { + current_state= &state_moveFast; + } break; case EVENT_EXIT: @@ -548,16 +615,27 @@ void state_moveSlow(enum Event my_event) * the requsted position was passed, move carusel a bit backwards * */ +const unsigned int returnTimeOut= 2000; // max duration of the state +unsigned int returnStopTime; // time to of the time out +//unsigned int requestedPosOK= 0; void state_returnToPosition(enum Event my_event) { switch (my_event ){ case EVENT_ENTRY: - if (moveDirection==FORWARD) engine_A_dir(ENGINE_DIR_BW); - else engine_A_dir(ENGINE_DIR_FW); + // change the direction + if (moveDirection==FORWARD) { + moveDirection=BACKWARD; + engine_A_dir(ENGINE_DIR_BW); + } + else { + moveDirection=FORWARD; + engine_A_dir(ENGINE_DIR_FW); + } engine_A_pwm(LOWSPEED); engine_A_en(ENGINE_EN_ON); + returnStopTime= getTime() + returnTimeOut; uart0SendCh('d'); break; @@ -567,17 +645,41 @@ void state_returnToPosition(enum Event my_event) { // when the carusel gets to the right position next state is stop current_state= &state_stop; + moveCarousel= 0; + sendCANmessage= 1; + sendCANdata= requestedPosition; + + flashLEDY= 1; + //requestedPosOK= 1; + } + // check for timeout + else if (getTime()> returnStopTime) + + { + // when error in setting the exact position move fast again + current_state= &state_moveFast; + stepsToPos= 3; // don't slow down, turn fast as long as not pass the requested position } break; case EVENT_EXIT: // the task was achieved, carousel is at the right position - moveCarousel= 0; + // if time out?? ?leave the direction changed and call the move fast?? - sendCANmessage= 1; - sendCANdata= 1; + // change back the direction of moving? + if (moveDirection==FORWARD) { + moveDirection=BACKWARD; + engine_A_dir(ENGINE_DIR_BW); + } + else { + moveDirection=FORWARD; + engine_A_dir(ENGINE_DIR_FW); + } + //if (requestedPosOK==1){ + // requestedPosOK=0; + // moveCarousel= 0; + //} - flashLEDY= 1; // stop the carousel engine_A_pwm(0); @@ -611,6 +713,7 @@ unsigned int getCurrentPosition(void) */ unsigned int stateReading=0; // 0- no signal, 1- some signal unsigned int reading= 0; // signal that is read +unsigned int prevValidPosition= 0; void updatePosition(void) { @@ -622,15 +725,36 @@ void updatePosition(void) // if signal is currently present update reading if ( (readPosition>=0)&&(readPosition<=4) ) reading= reading | intToSignal[readPosition]; - // if the signal got lost + // if the signal got lost, position is valid if ( readPosition==11) { stateReading= 0; // the state is over now, go to default state - validPosition= signalToInt[reading]; //decode the position; + prevValidPosition= validPosition; // keep the previous valid position + + validPosition= signalToInt[reading]; // decode the position; validReading= 1; // reading is valid flashLEDB= 1; uart0SendCh(validPosition+48); + + // check if there are no mistakes in reading + signed int diff= validPosition -prevValidPosition; + if (diff==-4) diff= 1; // count real distances + else if (diff==4) diff= -1; + else if (diff==3) diff= -2; + else if (diff==3) diff= 2; + + if(diff==0) + { + // the read valid position could generate error + // or direction of move was changed + } + else if( (diff<-1) || (diff>1) ) + { + // some position was not read correctly, or this position was not read correctly + errorState= NOTICE; + uart0SendCh('e'); + } } } @@ -717,6 +841,8 @@ void init_carousel(void) } } + blink_LED(); + handle_CAN(); } while (1); engine_A_pwm(0); @@ -728,6 +854,8 @@ void init_carousel(void) while (input!= intToSignal[next2Pos]) { input= ((IOPIN1>>16) & 0x07); + blink_LED(); + handle_CAN(); } engine_A_dir(ENGINE_DIR_FW); //FW //nejdriv je potreba zmenit smer, pak teprve zmenit rychlost!!! engine_A_pwm(0); // stop engine @@ -744,17 +872,16 @@ void init_carousel(void) uart0SendCh( ';'); } - // finally compture the constants - timeNextPos= oneTurn*7/(10*(cycles+1)); - uart0SendCh( '-'); - uart0SendCh( (char) ( (timeNextPos>>24)&0x0ff ) ); - uart0SendCh( (char) ( (timeNextPos>>16)&0x0ff ) ); - uart0SendCh( (char) ( (timeNextPos>>8)&0x0ff ) ); - uart0SendCh( (char) ( (timeNextPos)&0x0ff ) ); - - uart0SendCh( '-'); + // finally send constants to terminal + timeNextPos= oneTurn*5/(10*(cycles+1)); + //uart0SendCh( '-'); + //uart0SendCh( (char) ( (timeNextPos>>24)&0x0ff ) ); + //uart0SendCh( (char) ( (timeNextPos>>16)&0x0ff ) ); + //uart0SendCh( (char) ( (timeNextPos>>8)&0x0ff ) ); + //uart0SendCh( (char) ( (timeNextPos)&0x0ff ) ); + //uart0SendCh( '-'); - timeNext2Pos= twoTurns/cycles; + timeNext2Pos= twoTurns*5/(10*cycles+1); } uart0SendCh('i'); @@ -766,45 +893,19 @@ void init_carousel(void) */ void can_rx(can_msg_t *msg) { can_msg_t rx_msg; - unsigned int newPos; memcpy(&rx_msg, msg, sizeof(can_msg_t)); switch (rx_msg.id) { case CAN_SERVO: - //set_servo(0, rx_msg.data[0]); - //set_servo(2, rx_msg.data[2]); + set_servo(0, rx_msg.data[2]); + set_servo(1, rx_msg.data[4]); break; case CAN_DRIVES: - - newPos= rx_msg.data[3]; - // check for valid position - if ( (newPos>=0) && (newPos<=4) ) - { - // when carousel is already moving - if(moveCarousel==1) - { - if (newPos== requestedPosition) - { - // BOA is repatly sending the position - } - else - { - // while carousel was moving new position was requested - current_state= &state_stop; - requestedPosition= newPos; - } - } - // carousel is in stop position - else - { - moveCarousel= 1; - requestedPosition= newPos; - } - - } + recieveCANmessage= 1; //flag to sent CAN data + recieveCANdata= rx_msg.data[3]; // data to send break; default: -- 2.39.2