From 3204d1f4bd5de7344b05402b707436a8c46c3a5e Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Mon, 28 Jul 2008 13:36:55 +0200 Subject: [PATCH] eb_carousel renamed to carousel and created symlink in build to smetak --- build/lpceurobot/smetak | 1 + src/{eb_carousel => carousel}/.gitignore | 0 src/carousel/Makefile.omk | 4 +- src/carousel/carousel.c | 1242 ++++++++++++++++++++++ src/eb_carousel/Makefile | 14 - src/eb_carousel/Makefile.omk | 6 - src/eb_carousel/carousel.c | 1242 ---------------------- 7 files changed, 1245 insertions(+), 1264 deletions(-) create mode 120000 build/lpceurobot/smetak rename src/{eb_carousel => carousel}/.gitignore (100%) delete mode 100644 src/eb_carousel/Makefile delete mode 100644 src/eb_carousel/Makefile.omk delete mode 100644 src/eb_carousel/carousel.c diff --git a/build/lpceurobot/smetak b/build/lpceurobot/smetak new file mode 120000 index 00000000..0e2ba17f --- /dev/null +++ b/build/lpceurobot/smetak @@ -0,0 +1 @@ +../../src/smetak \ No newline at end of file diff --git a/src/eb_carousel/.gitignore b/src/carousel/.gitignore similarity index 100% rename from src/eb_carousel/.gitignore rename to src/carousel/.gitignore diff --git a/src/carousel/Makefile.omk b/src/carousel/Makefile.omk index b4dd107d..2ece3e04 100644 --- a/src/carousel/Makefile.omk +++ b/src/carousel/Makefile.omk @@ -1,6 +1,6 @@ # -*- makefile -*- -#bin_PROGRAMS = carousel +bin_PROGRAMS = carousel carousel_SOURCES = carousel.c -carousel_LIBS = +carousel_LIBS = ebb can diff --git a/src/carousel/carousel.c b/src/carousel/carousel.c index e69de29b..4f730f39 100644 --- a/src/carousel/carousel.c +++ b/src/carousel/carousel.c @@ -0,0 +1,1242 @@ + +/** + * 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 +#include +#include +#include +#include +#include +#include +#include +#include + + +#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 SERVO_ISR 5 + +#define FULLSPEED 50 +#define MIDSPEED 20 +#define LOWSPEED 10 + +#define INIT_POSITION 0 + +#define GLOBALTIMEOUT 6000 + +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 + +//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 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 + +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 systemInit=0; // flag that show that carousel is just initialising its constants + +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 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 + */ +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 + +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 +void handle_CAN(void); +void blink_LED(void); +void updatePosition(void); + +// state space automat +typedef void (*state_fcn)(enum Event my_event); +state_fcn current_state; // current state of carousel, stop, fast_move, slow_move + +void state_stop(enum Event my_event); +void state_moveSlow(enum Event my_event); +void state_moveFast(enum Event my_event); +void state_returnToPosition(enum Event my_event); + + + + +// volatile int delay_done=0 ; +int main (void) +{ + systemInit= 1; // beginning of initialisation + init_perip(); + + char text[3]= {'s',0x0D,0x0A}; + int n; + for(n=0;n<3;n++) + { + uart0SendCh(text[n]); + } + + // 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); + + moveCarousel= INIT_POSITION; + carouselTimeOut= 0; + if (doInitCarousel){ + init_carousel(); // set the constants for positioning carousel + //moveCarousel= 0; + } + else { + validPosition= 4; // there is no reading about last valid position, it has to be setup + } + + uart0SendCh('j'); + + // start the stop state of carousel + current_state= &state_stop; // current state of carousel + state_fcn last_state= &state_stop; + current_state(EVENT_ENTRY); + + requestedPosition= 0; // the positon the carusel should move to + + + moveCarousel= 1; // request to move the carusel + moveDirection= FORWARD; + + systemInit= 0; // end of initialisation + + while (1){ + + last_state= current_state; // track changes in state + current_state(EVENT_DO); + + // check if it is after timeout + if ( (carouselTimeOut!=0)&&(carouselTimeOut=1){ + if ( repeatErrorCANtime=0) && (newPos<=4) ) + { + // keep track of previous requested position + prevRequestedPos= requestedPosition; + // 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)&&( getCurrentPosition()==requestedPosition) ) + { + // carousel is already in this position + // send message that i am in this position + sendCANmessage=1; + sendCANOKdata= requestedPosition; + wasError= 0; + current_state= &state_stop; + } + else if (prevRequestedPos!=newPos) + { + wasError= 0; + // task to move to new position + moveCarousel= 1; + requestedPosition= newPos; + current_state= &state_stop; + } + else + { + // the previosly requested position is the same as now requested position, + // but the sensor isn't located exactly + } + } + + } + } +} + +/** + * 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) + { + // used to set the position of carousel by keyboard + uart0SendCh('x'); + unsigned int next_pos= (unsigned int) uart0GetCh(); + next_pos= next_pos -48; // recived character is in ASCII + if (next_pos>4) next_pos=0; + requestedPosition= next_pos; + moveCarousel= 1; + + } + // preprogrammed sequence of positions + else if (useArray); + { + // !!! wait just for can + readyToGo= 1; + // wait a while before next turn + timeToGo = getTime() +10000; + } + } + if (readyToGo) + { + if (timeToGo< getTime()) + { + readyToGo= 0; + requestedPosition= sequens[sequensX]; + if (sequensX<12) sequensX++; + else sequensX=0; + moveCarousel= 1; + } + } + + prevMoveCarousel= moveCarousel; +} + + +/** + * blink with the LEDs located on eb_board + * green LED blink constantly + * red LED blinks when errorState is ERROR + */ +unsigned int okTimeToBlink= 0; +unsigned int okLEDstate= 0; + +unsigned int errTimeToBlink= 0; +unsigned int errLEDstate= 0; + +// unsigned int blueTimeToBlink= 0; +// unsigned int blueLEDstate= 0; + +unsigned int yBlinkTime=0; +unsigned int yLEDstate= 0; + +enum ErrorState lastErrorState= NO_ERROR; // used to keep information about error, used to cover the case, when NOTICE change errorState from ERROR + +void blink_LED(void) +{ + unsigned int curr_time= getTime(); + if (okTimeToBlink< curr_time) + { + okTimeToBlink= curr_time + 500; + okLEDstate= ~okLEDstate; + if (okLEDstate) deb_led_on(LEDG); + else deb_led_off(LEDG); + } + + // crucial fault error + if ( (errorState!=NO_ERROR)&&(errorState!=NOTICE) ){ + if (errTimeToBlink< curr_time){ + errTimeToBlink= curr_time + 200; + errLEDstate= ~errLEDstate; + if (errLEDstate) deb_led_on(LEDR); + else deb_led_off(LEDR); + } + } + // notice about some fault behaviour by red LED + else if (errorState==NOTICE) + { + // notice can't override error + if ( (lastErrorState=!NO_ERROR)&&(errorState!=NOTICE) ) { + errorState= lastErrorState; + } + else if(errLEDstate==0) + { + errLEDstate= 1; + errTimeToBlink= getTime() +1000; + deb_led_on(LEDR); + } + else if (errTimeToBlink< curr_time) + { + errLEDstate= 0; + deb_led_off(LEDR); + errorState= NO_ERROR; // clear the notice flag + } + + } + lastErrorState= errorState; + +/* // check if blue LED should flash + if(flashLEDB) + { + flashLEDB= 0; + blueLEDstate=1; + deb_led_on(LEDB); + blueTimeToBlink= getTime() +100; + } + // after specified interval turn off the blue LED + if(blueLEDstate==1) + { + if( blueTimeToBlink2) + { + if (dist==3) stepsToPos= 2; + else stepsToPos= 1; // dist==4 + moveDirection= BACKWARD; + } + else if ( (dist>0)&&(dist<3) ) + { + stepsToPos= dist; + moveDirection= FORWARD; + } + else if ( (dist>-3)&&(dist<0) ) + { + stepsToPos= -dist; + moveDirection= BACKWARD; + } + else if (dist<-2) + { + if (dist==-3) stepsToPos= 2; + else stepsToPos= 1; // dist==4 + moveDirection= FORWARD; + } + else + { + // when some nonsense distance, move forward and long time + moveDirection= FORWARD; + stepsToPos= 3; + } + + } + uart0SendCh(stepsToPos+48); + + + } + break; + + case EVENT_EXIT: + uart0SendCh('a'); + break; + } + +} +/** + * moving the carousel fast forward towards the right position + * + */ +unsigned int stopFaMoveTime=0; +void state_moveFast(enum Event my_event) +{ + switch (my_event ){ + case EVENT_ENTRY: + // start the motor + if (moveDirection==FORWARD) engine_A_dir(ENGINE_DIR_FW); + else engine_A_dir(ENGINE_DIR_BW); + + engine_A_pwm(FULLSPEED); + engine_A_en(ENGINE_EN_ON); + + // measure the time interval + // what if stepsToPos==0? !!! + if (stepsToPos==1) stopFaMoveTime= getTime() + timeNextPos; + else if (stepsToPos==2) stopFaMoveTime= getTime() + timeNext2Pos; + else stopFaMoveTime= getTime() + 10000; // if problems to reach the position just turn fast a while + + uart0SendCh('b'); + break; + + case EVENT_DO: + // if the carusel passed the set position go to the next state + if (validReading) + { + validReading= 0; + if(validPosition==requestedPosition) + { + // if the right position was passed return to this position + current_state= &state_returnToPosition; + } + } + + // after some delay change the speed of the motor + if (stopFaMoveTime< getTime()) + { + current_state= &state_moveSlow; + } + break; + + case EVENT_EXIT: + // should stop the motors + //engine_A_dir(ENGINE_DIR_FW); + engine_A_pwm(0); + //engine_A_en(ENGINE_EN_ON); + uart0SendCh('b'); + break; + } + +} +/** + * 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 ){ + case EVENT_ENTRY: + // start to move motor slowly + if (moveDirection==FORWARD) engine_A_dir(ENGINE_DIR_FW); + else engine_A_dir(ENGINE_DIR_BW); + + engine_A_pwm(MIDSPEED); + engine_A_en(ENGINE_EN_ON); + stopSlMoveTime= getTime() + moveSlowTimeOut; + + uart0SendCh('c'); + break; + case EVENT_DO: + // wait to reach the right position + if (validReading) + { + validReading= 0; + if( validPosition==requestedPosition) + { + // positon reached, go to the next state + current_state= &state_returnToPosition; + } + } + // almost error state, moving slow too long time, move fast to reach the position + if (getTime() > stopSlMoveTime) + { + // a bit error state, robot should reach the requested state, but its not there yet + current_state= &state_moveFast; + stepsToPos=3; // turn fast till reach the requested position + } + break; + + case EVENT_EXIT: + // stop the motor + engine_A_pwm(0); + + uart0SendCh('c'); + break; + } + +} + +/** + * 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: + // 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; + + case EVENT_DO: + // wait for reaching the right position + if ( requestedPosition==getCurrentPosition() ) + { + // when the carusel gets to the right position next state is stop + current_state= &state_stop; + moveCarousel= 0; + carouselTimeOut= 0; + sendCANmessage= 1; + sendCANOKdata= 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 + // if time out?? ?leave the direction changed and call the move fast?? + + // the requested position was reached, change the direction of move back + if (moveCarousel==0){ + carouselTimeOut= 0; + + uart0SendCh('d'); + uart0SendCh('1'); + + if (moveDirection==FORWARD) { + moveDirection=BACKWARD; + engine_A_dir(ENGINE_DIR_BW); + } + else { + moveDirection=FORWARD; + engine_A_dir(ENGINE_DIR_FW); + } + } + // requested position was not reached + else{ + // i should pass the carousel but didn't + // leave the direction and next state will be move fast + // i will go to the requested position from the other side + uart0SendCh('d'); + uart0SendCh('2'); + + // nothing to do + } + + // stop the carousel + engine_A_pwm(0); + uart0SendCh('d'); + break; + } + +} + +/** + * 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 + */ +// 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; + unsigned int pos; + input= ((IOPIN1>>16) & 0x07); + pos= signalToInt[input]; + if (pos!=11) deb_led_on(LEDB); + else deb_led_off(LEDB); + return pos; +} + +/** + * 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 + */ +signed int positionToDistance(unsigned int pos1, unsigned int pos2) +{ + signed int diff= pos1 -pos2; + + 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; + + return diff; +} + +/** + * count new position + * input position 0..4, distance -4..+4 + * return position 0..4 + */ +unsigned int distanceToPos(unsigned int position, signed int distance) +{ + unsigned absolutePos= position +distance; + if (absolutePos > 4) absolutePos= absolutePos - 5; + else if (position < 0) absolutePos= absolutePos +5; + + 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 +unsigned int prevValidPosition= 0; +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 lastSignaltime=0; +unsigned int errorPosRead[5]= {0,0,0,0,0}; // how many times positon was read wrong. once read right, reset this number + +void updatePosition(void) +{ + unsigned int readPosition= getCurrentPosition(); + unsigned int expectedPos= 0; // only initialisation of the expected pos, to ged rid of warning + + // case i am currently reading a position + if (stateReading==1) + { + // if signal is currently present update reading + if ( (readPosition>=0)&&(readPosition<=4) ) reading= reading | intToSignal[readPosition]; + + // if the signal got lost, position is valid + if ( readPosition==11) + { + stateReading= 0; // the state is over now, go to default state + 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= positionToDistance(validPosition, prevValidPosition); + + if(moveDirection!=prevMoveDirection){ + // direction is different, next valid position should be the same position + expectedPos= prevValidPosition; + if (diff!=0){ + errorState= NOTICE; + uart0SendCh('e'); + uart0SendCh('0'); + } + } + else if (diff==0){ + // the direction was not changed, but the last position was read again ->the same position was read twice by fault + // do nothing for this error + expectedPos= validPosition; + //errorState= NOTICE; + //uart0SendCh('e'); + //uart0SendCh('1'); + } + else if ( moveDirection== FORWARD){ + // moving forward the next diff should ve positive + expectedPos= distanceToPos(prevValidPosition,1); + if (diff!=1){ + // position was misread increment the array of misread positions + errorPosRead[expectedPos]= errorPosRead[expectedPos] +1; + + errorState= NOTICE; + uart0SendCh('e'); + uart0SendCh('2'); + } + } + else if ( moveDirection== BACKWARD){ + // moving backward the diff should be -1 + expectedPos= distanceToPos(prevValidPosition, -1); + if (diff!=-1){ + // position was misread increment the array of misread positions + errorPosRead[expectedPos]= errorPosRead[expectedPos] +1; + + errorState= NOTICE; + uart0SendCh('e'); + uart0SendCh('3'); + } + } + + // check if the wrongly read position isn't requested position and isn't >2 + if( (expectedPos== requestedPosition)&&(errorPosRead[expectedPos]>2) ){ + //!! this is crucial, read error of requested position + errorState= ERROR_POSITION; + sendCANERRdata= errorState; + wasError= 1; + sendCANmessage= 2; + + moveCarousel= 0; // stop moving with carousel + carouselTimeOut= 0; + current_state= &state_stop; + + uart0SendCh('!'); + uart0SendCh('2'); + // reset the counter, next time this error while reading will happed again, repeat the error + errorPosRead[expectedPos]= 0; + } + + // if the position was read correctly, reset the number of wrongly read positions + else if (errorPosRead[expectedPos]!=0){ + // reading of the requested position was correct, reset the counter + errorPosRead[expectedPos]= 0; + } + + }//if ( readPosition==11) + + } + // no signal about position + else + { + // waiting for signal + if ( (readPosition>=0)&&(readPosition<=4) ) + { + stateReading= 1; // start to read + reading= intToSignal[readPosition]; // keep the bits of the signal + signalTimeoutOn= 0; // some position was read + } + + } + // check for error !!, when carousel is moving and any position is read for long time, thow error + if ( (moveCarousel==1) && (readPosition==11) ) + { + // when carousel is moving fast, the timeout for geting some signal is shorter + unsigned int signalTimeout= 4000; + if ( current_state== &state_moveFast) signalTimeout= 2000; + + if (signalTimeoutOn==0){ + signalTimeoutOn= 1; + lastSignaltime= getTime(); + } + else if ( lastSignaltime +signalTimeout < getTime() ){ + // the carousel should move, but signal didn't come in timeout - ERROR !! + signalTimeoutOn=0; + errorState= ERROR_SENSOR; + wasError= 1; + sendCANERRdata= errorState; + sendCANmessage= 2; + + moveCarousel= 0; // stop moving with carousel + carouselTimeOut= 0; + current_state= &state_stop; + + uart0SendCh('!'); + uart0SendCh('1'); + } + } + + + prevMoveDirection= moveDirection; + // the error reading, this position is not on carousel + if (readPosition==10) { + uart0SendCh('e'); + uart0SendCh('4'); + } +} + +/** + * initialize constants for positioning carousel + * move n times by set speed and measure the time interval + */ +unsigned int initPos= 0; // initial position +unsigned int setConstants= 1; // should the robot initialize the constants for moving carousel? +void init_carousel(void) +{ + uart0SendCh('i'); + + // goto position initPos + engine_A_dir(ENGINE_DIR_FW); //FW); + engine_A_pwm(FULLSPEED); + engine_A_en(ENGINE_EN_ON); + do{ + updatePosition(); + if (validReading){ + validReading= 0; + //uart0SendCh( (char) validPosition +48 ); + if (validPosition==initPos) break; + } + } while (1); + + engine_A_pwm(0); + uart0SendCh('i'); + + // if the position is not exact, go back + unsigned int input; + input= ((IOPIN1>>16) & 0x07); + engine_A_dir(ENGINE_DIR_BW); //BW); + if (input!=intToSignal[initPos]) engine_A_pwm(10); + while (input!= intToSignal[initPos]) + { + input= ((IOPIN1>>16) & 0x07); + } + engine_A_dir(ENGINE_DIR_FW); //FW //nejdriv je potreba zmenit smer, pak teprve zmenit rychlost!!! + engine_A_pwm(0); // stop engine + + uart0SendCh('i'); + + if(setConstants) + { + // count the durations of move + unsigned int repeat= 0; + unsigned int cycles= 5; + unsigned int oneTurn= 0; // time to move to next postition + unsigned int twoTurns= 0; // time to move to second postition + unsigned int start= 0; + unsigned int nextPos= initPos +1; + unsigned int next2Pos= initPos +2; + + for (repeat=0; repeat<=cycles; repeat ++){ + start= getTime(); + engine_A_pwm(FULLSPEED); + do{ + updatePosition(); + if (validReading){ + validReading= 0; + //uart0SendCh( (char) validPosition +48 ); + if (validPosition==nextPos){ + oneTurn= oneTurn +getTime() -start; + } + if (validPosition==next2Pos){ + twoTurns= twoTurns +getTime() -start; + break; + } + + } + blink_LED(); + handle_CAN(); + } while (1); + engine_A_pwm(0); + + // if the position is not exact, go back + unsigned int input; + input= ((IOPIN1>>16) & 0x07); + engine_A_dir(ENGINE_DIR_BW); //BW); + if (input!=intToSignal[next2Pos]) engine_A_pwm(10); + 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 + + // count next positions + initPos= initPos +2; + nextPos= initPos +1; + next2Pos= initPos +2; + if (initPos>4) initPos= initPos - 5; + if (nextPos>4) nextPos= nextPos - 5; + if (next2Pos>4) next2Pos= next2Pos - 5; + + //uart0SendCh( (char) initPos+48 ); + uart0SendCh( ';'); + } + + // finally send constants to terminal + timeNextPos= oneTurn*5/(10*(cycles+1)); + timeNext2Pos= twoTurns*5/(10*cycles+1); + if (1) + { + uart0SendCh( '-'); + uart0SendCh( (char) ( (timeNext2Pos>>24)&0x0ff ) ); + uart0SendCh( (char) ( (timeNext2Pos>>16)&0x0ff ) ); + uart0SendCh( (char) ( (timeNext2Pos>>8)&0x0ff ) ); + uart0SendCh( (char) ( (timeNext2Pos)&0x0ff ) ); + uart0SendCh( '-'); + } + + timeNext2Pos= twoTurns*5/(10*cycles+1); + } + + uart0SendCh('i'); +} + +/** + * handels messages recieved by the CAN controller + * + */ +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_SERVO: + set_servo(0, rx_msg.data[2]); + set_servo(1, rx_msg.data[4]); + break; + + case CAN_DRIVES: + recieveCANmessage= 1; //flag to sent CAN data + recieveCANdata= rx_msg.data[3]; // data to send + break; + + case CAN_PWR_ALERT: + //if ( (rx_msg.data[0] == 2) | (rx_msg.data[0] == 3) ) go_to_sleep(); + break; + + default: + break; + } +} + +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 +} + + + +/** + * returns the number in T0TC + * + */ +unsigned int getTime(void) +{ + return T0TC; +} + + +/** + * wait time specified in the interaval + * int interval - interval in ms + * + * need to implement overflow when needed durations in days.. + */ +void delay(int interval) +{ + unsigned int myTime= getTime(); + while( getTime()< (myTime+interval) ); +} diff --git a/src/eb_carousel/Makefile b/src/eb_carousel/Makefile deleted file mode 100644 index 08cf5ff3..00000000 --- a/src/eb_carousel/Makefile +++ /dev/null @@ -1,14 +0,0 @@ -# Generic directory or leaf node makefile for OCERA make framework - -ifndef MAKERULES_DIR -MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) ) -endif - -ifeq ($(MAKERULES_DIR),) -all : default -.DEFAULT:: - @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n" -else -include $(MAKERULES_DIR)/Makefile.rules -endif - diff --git a/src/eb_carousel/Makefile.omk b/src/eb_carousel/Makefile.omk deleted file mode 100644 index 2ece3e04..00000000 --- a/src/eb_carousel/Makefile.omk +++ /dev/null @@ -1,6 +0,0 @@ -# -*- makefile -*- - -bin_PROGRAMS = carousel - -carousel_SOURCES = carousel.c -carousel_LIBS = ebb can diff --git a/src/eb_carousel/carousel.c b/src/eb_carousel/carousel.c deleted file mode 100644 index 4f730f39..00000000 --- a/src/eb_carousel/carousel.c +++ /dev/null @@ -1,1242 +0,0 @@ - -/** - * 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 -#include -#include -#include -#include -#include -#include -#include -#include - - -#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 SERVO_ISR 5 - -#define FULLSPEED 50 -#define MIDSPEED 20 -#define LOWSPEED 10 - -#define INIT_POSITION 0 - -#define GLOBALTIMEOUT 6000 - -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 - -//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 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 - -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 systemInit=0; // flag that show that carousel is just initialising its constants - -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 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 - */ -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 - -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 -void handle_CAN(void); -void blink_LED(void); -void updatePosition(void); - -// state space automat -typedef void (*state_fcn)(enum Event my_event); -state_fcn current_state; // current state of carousel, stop, fast_move, slow_move - -void state_stop(enum Event my_event); -void state_moveSlow(enum Event my_event); -void state_moveFast(enum Event my_event); -void state_returnToPosition(enum Event my_event); - - - - -// volatile int delay_done=0 ; -int main (void) -{ - systemInit= 1; // beginning of initialisation - init_perip(); - - char text[3]= {'s',0x0D,0x0A}; - int n; - for(n=0;n<3;n++) - { - uart0SendCh(text[n]); - } - - // 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); - - moveCarousel= INIT_POSITION; - carouselTimeOut= 0; - if (doInitCarousel){ - init_carousel(); // set the constants for positioning carousel - //moveCarousel= 0; - } - else { - validPosition= 4; // there is no reading about last valid position, it has to be setup - } - - uart0SendCh('j'); - - // start the stop state of carousel - current_state= &state_stop; // current state of carousel - state_fcn last_state= &state_stop; - current_state(EVENT_ENTRY); - - requestedPosition= 0; // the positon the carusel should move to - - - moveCarousel= 1; // request to move the carusel - moveDirection= FORWARD; - - systemInit= 0; // end of initialisation - - while (1){ - - last_state= current_state; // track changes in state - current_state(EVENT_DO); - - // check if it is after timeout - if ( (carouselTimeOut!=0)&&(carouselTimeOut=1){ - if ( repeatErrorCANtime=0) && (newPos<=4) ) - { - // keep track of previous requested position - prevRequestedPos= requestedPosition; - // 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)&&( getCurrentPosition()==requestedPosition) ) - { - // carousel is already in this position - // send message that i am in this position - sendCANmessage=1; - sendCANOKdata= requestedPosition; - wasError= 0; - current_state= &state_stop; - } - else if (prevRequestedPos!=newPos) - { - wasError= 0; - // task to move to new position - moveCarousel= 1; - requestedPosition= newPos; - current_state= &state_stop; - } - else - { - // the previosly requested position is the same as now requested position, - // but the sensor isn't located exactly - } - } - - } - } -} - -/** - * 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) - { - // used to set the position of carousel by keyboard - uart0SendCh('x'); - unsigned int next_pos= (unsigned int) uart0GetCh(); - next_pos= next_pos -48; // recived character is in ASCII - if (next_pos>4) next_pos=0; - requestedPosition= next_pos; - moveCarousel= 1; - - } - // preprogrammed sequence of positions - else if (useArray); - { - // !!! wait just for can - readyToGo= 1; - // wait a while before next turn - timeToGo = getTime() +10000; - } - } - if (readyToGo) - { - if (timeToGo< getTime()) - { - readyToGo= 0; - requestedPosition= sequens[sequensX]; - if (sequensX<12) sequensX++; - else sequensX=0; - moveCarousel= 1; - } - } - - prevMoveCarousel= moveCarousel; -} - - -/** - * blink with the LEDs located on eb_board - * green LED blink constantly - * red LED blinks when errorState is ERROR - */ -unsigned int okTimeToBlink= 0; -unsigned int okLEDstate= 0; - -unsigned int errTimeToBlink= 0; -unsigned int errLEDstate= 0; - -// unsigned int blueTimeToBlink= 0; -// unsigned int blueLEDstate= 0; - -unsigned int yBlinkTime=0; -unsigned int yLEDstate= 0; - -enum ErrorState lastErrorState= NO_ERROR; // used to keep information about error, used to cover the case, when NOTICE change errorState from ERROR - -void blink_LED(void) -{ - unsigned int curr_time= getTime(); - if (okTimeToBlink< curr_time) - { - okTimeToBlink= curr_time + 500; - okLEDstate= ~okLEDstate; - if (okLEDstate) deb_led_on(LEDG); - else deb_led_off(LEDG); - } - - // crucial fault error - if ( (errorState!=NO_ERROR)&&(errorState!=NOTICE) ){ - if (errTimeToBlink< curr_time){ - errTimeToBlink= curr_time + 200; - errLEDstate= ~errLEDstate; - if (errLEDstate) deb_led_on(LEDR); - else deb_led_off(LEDR); - } - } - // notice about some fault behaviour by red LED - else if (errorState==NOTICE) - { - // notice can't override error - if ( (lastErrorState=!NO_ERROR)&&(errorState!=NOTICE) ) { - errorState= lastErrorState; - } - else if(errLEDstate==0) - { - errLEDstate= 1; - errTimeToBlink= getTime() +1000; - deb_led_on(LEDR); - } - else if (errTimeToBlink< curr_time) - { - errLEDstate= 0; - deb_led_off(LEDR); - errorState= NO_ERROR; // clear the notice flag - } - - } - lastErrorState= errorState; - -/* // check if blue LED should flash - if(flashLEDB) - { - flashLEDB= 0; - blueLEDstate=1; - deb_led_on(LEDB); - blueTimeToBlink= getTime() +100; - } - // after specified interval turn off the blue LED - if(blueLEDstate==1) - { - if( blueTimeToBlink2) - { - if (dist==3) stepsToPos= 2; - else stepsToPos= 1; // dist==4 - moveDirection= BACKWARD; - } - else if ( (dist>0)&&(dist<3) ) - { - stepsToPos= dist; - moveDirection= FORWARD; - } - else if ( (dist>-3)&&(dist<0) ) - { - stepsToPos= -dist; - moveDirection= BACKWARD; - } - else if (dist<-2) - { - if (dist==-3) stepsToPos= 2; - else stepsToPos= 1; // dist==4 - moveDirection= FORWARD; - } - else - { - // when some nonsense distance, move forward and long time - moveDirection= FORWARD; - stepsToPos= 3; - } - - } - uart0SendCh(stepsToPos+48); - - - } - break; - - case EVENT_EXIT: - uart0SendCh('a'); - break; - } - -} -/** - * moving the carousel fast forward towards the right position - * - */ -unsigned int stopFaMoveTime=0; -void state_moveFast(enum Event my_event) -{ - switch (my_event ){ - case EVENT_ENTRY: - // start the motor - if (moveDirection==FORWARD) engine_A_dir(ENGINE_DIR_FW); - else engine_A_dir(ENGINE_DIR_BW); - - engine_A_pwm(FULLSPEED); - engine_A_en(ENGINE_EN_ON); - - // measure the time interval - // what if stepsToPos==0? !!! - if (stepsToPos==1) stopFaMoveTime= getTime() + timeNextPos; - else if (stepsToPos==2) stopFaMoveTime= getTime() + timeNext2Pos; - else stopFaMoveTime= getTime() + 10000; // if problems to reach the position just turn fast a while - - uart0SendCh('b'); - break; - - case EVENT_DO: - // if the carusel passed the set position go to the next state - if (validReading) - { - validReading= 0; - if(validPosition==requestedPosition) - { - // if the right position was passed return to this position - current_state= &state_returnToPosition; - } - } - - // after some delay change the speed of the motor - if (stopFaMoveTime< getTime()) - { - current_state= &state_moveSlow; - } - break; - - case EVENT_EXIT: - // should stop the motors - //engine_A_dir(ENGINE_DIR_FW); - engine_A_pwm(0); - //engine_A_en(ENGINE_EN_ON); - uart0SendCh('b'); - break; - } - -} -/** - * 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 ){ - case EVENT_ENTRY: - // start to move motor slowly - if (moveDirection==FORWARD) engine_A_dir(ENGINE_DIR_FW); - else engine_A_dir(ENGINE_DIR_BW); - - engine_A_pwm(MIDSPEED); - engine_A_en(ENGINE_EN_ON); - stopSlMoveTime= getTime() + moveSlowTimeOut; - - uart0SendCh('c'); - break; - case EVENT_DO: - // wait to reach the right position - if (validReading) - { - validReading= 0; - if( validPosition==requestedPosition) - { - // positon reached, go to the next state - current_state= &state_returnToPosition; - } - } - // almost error state, moving slow too long time, move fast to reach the position - if (getTime() > stopSlMoveTime) - { - // a bit error state, robot should reach the requested state, but its not there yet - current_state= &state_moveFast; - stepsToPos=3; // turn fast till reach the requested position - } - break; - - case EVENT_EXIT: - // stop the motor - engine_A_pwm(0); - - uart0SendCh('c'); - break; - } - -} - -/** - * 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: - // 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; - - case EVENT_DO: - // wait for reaching the right position - if ( requestedPosition==getCurrentPosition() ) - { - // when the carusel gets to the right position next state is stop - current_state= &state_stop; - moveCarousel= 0; - carouselTimeOut= 0; - sendCANmessage= 1; - sendCANOKdata= 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 - // if time out?? ?leave the direction changed and call the move fast?? - - // the requested position was reached, change the direction of move back - if (moveCarousel==0){ - carouselTimeOut= 0; - - uart0SendCh('d'); - uart0SendCh('1'); - - if (moveDirection==FORWARD) { - moveDirection=BACKWARD; - engine_A_dir(ENGINE_DIR_BW); - } - else { - moveDirection=FORWARD; - engine_A_dir(ENGINE_DIR_FW); - } - } - // requested position was not reached - else{ - // i should pass the carousel but didn't - // leave the direction and next state will be move fast - // i will go to the requested position from the other side - uart0SendCh('d'); - uart0SendCh('2'); - - // nothing to do - } - - // stop the carousel - engine_A_pwm(0); - uart0SendCh('d'); - break; - } - -} - -/** - * 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 - */ -// 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; - unsigned int pos; - input= ((IOPIN1>>16) & 0x07); - pos= signalToInt[input]; - if (pos!=11) deb_led_on(LEDB); - else deb_led_off(LEDB); - return pos; -} - -/** - * 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 - */ -signed int positionToDistance(unsigned int pos1, unsigned int pos2) -{ - signed int diff= pos1 -pos2; - - 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; - - return diff; -} - -/** - * count new position - * input position 0..4, distance -4..+4 - * return position 0..4 - */ -unsigned int distanceToPos(unsigned int position, signed int distance) -{ - unsigned absolutePos= position +distance; - if (absolutePos > 4) absolutePos= absolutePos - 5; - else if (position < 0) absolutePos= absolutePos +5; - - 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 -unsigned int prevValidPosition= 0; -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 lastSignaltime=0; -unsigned int errorPosRead[5]= {0,0,0,0,0}; // how many times positon was read wrong. once read right, reset this number - -void updatePosition(void) -{ - unsigned int readPosition= getCurrentPosition(); - unsigned int expectedPos= 0; // only initialisation of the expected pos, to ged rid of warning - - // case i am currently reading a position - if (stateReading==1) - { - // if signal is currently present update reading - if ( (readPosition>=0)&&(readPosition<=4) ) reading= reading | intToSignal[readPosition]; - - // if the signal got lost, position is valid - if ( readPosition==11) - { - stateReading= 0; // the state is over now, go to default state - 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= positionToDistance(validPosition, prevValidPosition); - - if(moveDirection!=prevMoveDirection){ - // direction is different, next valid position should be the same position - expectedPos= prevValidPosition; - if (diff!=0){ - errorState= NOTICE; - uart0SendCh('e'); - uart0SendCh('0'); - } - } - else if (diff==0){ - // the direction was not changed, but the last position was read again ->the same position was read twice by fault - // do nothing for this error - expectedPos= validPosition; - //errorState= NOTICE; - //uart0SendCh('e'); - //uart0SendCh('1'); - } - else if ( moveDirection== FORWARD){ - // moving forward the next diff should ve positive - expectedPos= distanceToPos(prevValidPosition,1); - if (diff!=1){ - // position was misread increment the array of misread positions - errorPosRead[expectedPos]= errorPosRead[expectedPos] +1; - - errorState= NOTICE; - uart0SendCh('e'); - uart0SendCh('2'); - } - } - else if ( moveDirection== BACKWARD){ - // moving backward the diff should be -1 - expectedPos= distanceToPos(prevValidPosition, -1); - if (diff!=-1){ - // position was misread increment the array of misread positions - errorPosRead[expectedPos]= errorPosRead[expectedPos] +1; - - errorState= NOTICE; - uart0SendCh('e'); - uart0SendCh('3'); - } - } - - // check if the wrongly read position isn't requested position and isn't >2 - if( (expectedPos== requestedPosition)&&(errorPosRead[expectedPos]>2) ){ - //!! this is crucial, read error of requested position - errorState= ERROR_POSITION; - sendCANERRdata= errorState; - wasError= 1; - sendCANmessage= 2; - - moveCarousel= 0; // stop moving with carousel - carouselTimeOut= 0; - current_state= &state_stop; - - uart0SendCh('!'); - uart0SendCh('2'); - // reset the counter, next time this error while reading will happed again, repeat the error - errorPosRead[expectedPos]= 0; - } - - // if the position was read correctly, reset the number of wrongly read positions - else if (errorPosRead[expectedPos]!=0){ - // reading of the requested position was correct, reset the counter - errorPosRead[expectedPos]= 0; - } - - }//if ( readPosition==11) - - } - // no signal about position - else - { - // waiting for signal - if ( (readPosition>=0)&&(readPosition<=4) ) - { - stateReading= 1; // start to read - reading= intToSignal[readPosition]; // keep the bits of the signal - signalTimeoutOn= 0; // some position was read - } - - } - // check for error !!, when carousel is moving and any position is read for long time, thow error - if ( (moveCarousel==1) && (readPosition==11) ) - { - // when carousel is moving fast, the timeout for geting some signal is shorter - unsigned int signalTimeout= 4000; - if ( current_state== &state_moveFast) signalTimeout= 2000; - - if (signalTimeoutOn==0){ - signalTimeoutOn= 1; - lastSignaltime= getTime(); - } - else if ( lastSignaltime +signalTimeout < getTime() ){ - // the carousel should move, but signal didn't come in timeout - ERROR !! - signalTimeoutOn=0; - errorState= ERROR_SENSOR; - wasError= 1; - sendCANERRdata= errorState; - sendCANmessage= 2; - - moveCarousel= 0; // stop moving with carousel - carouselTimeOut= 0; - current_state= &state_stop; - - uart0SendCh('!'); - uart0SendCh('1'); - } - } - - - prevMoveDirection= moveDirection; - // the error reading, this position is not on carousel - if (readPosition==10) { - uart0SendCh('e'); - uart0SendCh('4'); - } -} - -/** - * initialize constants for positioning carousel - * move n times by set speed and measure the time interval - */ -unsigned int initPos= 0; // initial position -unsigned int setConstants= 1; // should the robot initialize the constants for moving carousel? -void init_carousel(void) -{ - uart0SendCh('i'); - - // goto position initPos - engine_A_dir(ENGINE_DIR_FW); //FW); - engine_A_pwm(FULLSPEED); - engine_A_en(ENGINE_EN_ON); - do{ - updatePosition(); - if (validReading){ - validReading= 0; - //uart0SendCh( (char) validPosition +48 ); - if (validPosition==initPos) break; - } - } while (1); - - engine_A_pwm(0); - uart0SendCh('i'); - - // if the position is not exact, go back - unsigned int input; - input= ((IOPIN1>>16) & 0x07); - engine_A_dir(ENGINE_DIR_BW); //BW); - if (input!=intToSignal[initPos]) engine_A_pwm(10); - while (input!= intToSignal[initPos]) - { - input= ((IOPIN1>>16) & 0x07); - } - engine_A_dir(ENGINE_DIR_FW); //FW //nejdriv je potreba zmenit smer, pak teprve zmenit rychlost!!! - engine_A_pwm(0); // stop engine - - uart0SendCh('i'); - - if(setConstants) - { - // count the durations of move - unsigned int repeat= 0; - unsigned int cycles= 5; - unsigned int oneTurn= 0; // time to move to next postition - unsigned int twoTurns= 0; // time to move to second postition - unsigned int start= 0; - unsigned int nextPos= initPos +1; - unsigned int next2Pos= initPos +2; - - for (repeat=0; repeat<=cycles; repeat ++){ - start= getTime(); - engine_A_pwm(FULLSPEED); - do{ - updatePosition(); - if (validReading){ - validReading= 0; - //uart0SendCh( (char) validPosition +48 ); - if (validPosition==nextPos){ - oneTurn= oneTurn +getTime() -start; - } - if (validPosition==next2Pos){ - twoTurns= twoTurns +getTime() -start; - break; - } - - } - blink_LED(); - handle_CAN(); - } while (1); - engine_A_pwm(0); - - // if the position is not exact, go back - unsigned int input; - input= ((IOPIN1>>16) & 0x07); - engine_A_dir(ENGINE_DIR_BW); //BW); - if (input!=intToSignal[next2Pos]) engine_A_pwm(10); - 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 - - // count next positions - initPos= initPos +2; - nextPos= initPos +1; - next2Pos= initPos +2; - if (initPos>4) initPos= initPos - 5; - if (nextPos>4) nextPos= nextPos - 5; - if (next2Pos>4) next2Pos= next2Pos - 5; - - //uart0SendCh( (char) initPos+48 ); - uart0SendCh( ';'); - } - - // finally send constants to terminal - timeNextPos= oneTurn*5/(10*(cycles+1)); - timeNext2Pos= twoTurns*5/(10*cycles+1); - if (1) - { - uart0SendCh( '-'); - uart0SendCh( (char) ( (timeNext2Pos>>24)&0x0ff ) ); - uart0SendCh( (char) ( (timeNext2Pos>>16)&0x0ff ) ); - uart0SendCh( (char) ( (timeNext2Pos>>8)&0x0ff ) ); - uart0SendCh( (char) ( (timeNext2Pos)&0x0ff ) ); - uart0SendCh( '-'); - } - - timeNext2Pos= twoTurns*5/(10*cycles+1); - } - - uart0SendCh('i'); -} - -/** - * handels messages recieved by the CAN controller - * - */ -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_SERVO: - set_servo(0, rx_msg.data[2]); - set_servo(1, rx_msg.data[4]); - break; - - case CAN_DRIVES: - recieveCANmessage= 1; //flag to sent CAN data - recieveCANdata= rx_msg.data[3]; // data to send - break; - - case CAN_PWR_ALERT: - //if ( (rx_msg.data[0] == 2) | (rx_msg.data[0] == 3) ) go_to_sleep(); - break; - - default: - break; - } -} - -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 -} - - - -/** - * returns the number in T0TC - * - */ -unsigned int getTime(void) -{ - return T0TC; -} - - -/** - * wait time specified in the interaval - * int interval - interval in ms - * - * need to implement overflow when needed durations in days.. - */ -void delay(int interval) -{ - unsigned int myTime= getTime(); - while( getTime()< (myTime+interval) ); -} -- 2.39.2