-#define FULLSPEED 40
+#define FULLSPEED 50
#define MIDSPEED 20
#define LOWSPEED 10
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;
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);
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
// 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
*
* 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;
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)
{
timeToGo = getTime() +2000;
}
}
-
if (readyToGo)
{
if (timeToGo< getTime())
}
}
- 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
*/
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
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);
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);
else if(errLEDstate==0)
{
errLEDstate= 1;
- errTimeToBlink= getTime() +noticeLEDinterval;
+ errTimeToBlink= getTime() +1000;
deb_led_on(LEDR);
}
else if (errTimeToBlink< curr_time)
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)
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)
*
*/
-unsigned int stepsToPos= 1; // distance to reguested position
void state_stop(enum Event my_event)
{
switch (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;
* 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 ){
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;
}
// after some delay change the speed of the motor
- if (stopMoveTime< getTime())
+ if (stopFaMoveTime< getTime())
{
current_state= &state_moveSlow;
}
* 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 ){
engine_A_pwm(MIDSPEED);
engine_A_en(ENGINE_EN_ON);
+ stopSlMoveTime= getTime() + moveSlowTimeOut;
+
uart0SendCh('c');
break;
case EVENT_DO:
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:
* 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;
{
// 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);
*/
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)
{
// 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');
+ }
}
}
}
}
+ blink_LED();
+ handle_CAN();
} while (1);
engine_A_pwm(0);
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
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');
*/
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:
projects / eurobot / public.git / commitdiff