-
-
+// need to implement checking of the position of the carousel out of range 0..4
#include <deb_led.h>
#include <uart.h>
-#define FULLSPEED 30
+#define FULLSPEED 40
+#define MIDSPEED 20
#define LOWSPEED 10
-
enum Event {EVENT_ENTRY, EVENT_DO, EVENT_EXIT};
-enum ErrorState {ERROR,NO_ERROR};
+enum ErrorState {ERROR, NOTICE, NO_ERROR};
unsigned int validReading= 0; // 0 - carusel position signal 0- not valid, 1- valid
unsigned int requestedPosition= 0; // the positon the carusel should move to
unsigned int moveCarousel= 0; // request to move the carusel
-enum Direction {FORWARD, BACKWARD};
+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 flashLEDB= 0; // when set blue led will flash
enum ErrorState errorState= NO_ERROR;
{
init_perip();
-
+
+ uart0SendCh(0x0D);
+ uart0SendCh(0x0A);
+
init_carousel(); // set the constants for positioning carousel
char text[6]= {'i','n','i','t',0x0D,0x0A};
state_fcn last_state= &state_stop;;
current_state(EVENT_ENTRY);
- requestedPosition= 2; // the positon the carusel should move to
+ requestedPosition= 3; // the positon the carusel should move to
moveCarousel= 1; // request to move the carusel
+ moveDirection= FORWARD;
//errorState= ERROR;
send_data();
blink_LED();
updatePosition();
+
last_state= current_state; // track changes in state
current_state(EVENT_DO);
if(last_state!=current_state)
current_state(EVENT_ENTRY); // initialize the new state
}
- if (waitAwhile< getTime()) errorState= ERROR;
-
+ //if ( waitAwhile<getTime() ) errorState= NOTICE;
}
/**
* handle CAN bus
+ *
+ * CAN temporary simulated by pressing the keys, unfortunately stops the state machine
*/
+int prevMoveCarousel=0;
void handle_CAN(void)
{
// !! need to be implemented
unsigned int implement=1;
implement++;
+
+ if( (moveCarousel==0)&&(prevMoveCarousel!=0) )
+ {
+ 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;
+
+ }
+ prevMoveCarousel= moveCarousel;
+
}
/**
unsigned int okTimeToBlink= 0;
unsigned int okLEDstate= 0;
unsigned int okLEDinterval= 500;
+
unsigned int errTimeToBlink= 0;
unsigned int errLEDstate= 0;
unsigned int errLEDinterval= 200;
+unsigned int noticeLEDinterval= 1000;
+
+unsigned int blueTimeToBlink= 0;
+unsigned int blueLEDstate= 0;
+unsigned int blueLEDinterval= 500;
+
+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)
{
if (okLEDstate) deb_led_on(LEDG);
else deb_led_off(LEDG);
}
+
+ // crucial fault error
if (errorState==ERROR)
{
if (errTimeToBlink< curr_time)
else deb_led_off(LEDR);
}
}
+ // notice about some fault behaviour by red LED
+ else if (errorState==NOTICE)
+ {
+ // notice can't override error
+ if(lastErrorState==ERROR)
+ {
+ errorState= ERROR;
+ }
+ else if(errLEDstate==0)
+ {
+ errLEDstate= 1;
+ errTimeToBlink= getTime() +noticeLEDinterval;
+ 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() +blueLEDinterval;
+ }
+ // after specified interval turn off the blue LED
+ if(blueLEDstate==1)
+ {
+ if( blueTimeToBlink<getTime() )
+ {
+ deb_led_off(LEDB);
+ blueLEDstate=0;
+ }
+ }
+
}
* initial state where the motor is stoped
*
*/
+
+unsigned int stepsToPos= 1; // distance to reguested position
void state_stop(enum Event my_event)
{
switch (my_event ){
// nothing to do, wait for command to start the carousel
if (moveCarousel)
{
+
+ // if the position is already set, don't do anything
+ if (requestedPosition==validPosition){
+ moveCarousel=0;
+ stepsToPos= 0;
+ current_state= &state_stop;
+ break;
+ }
+
current_state= &state_moveFast;
+ // else set the direction of move to minimalize the move distance
+ signed int dist= requestedPosition -validPosition;
+ if (dist>2)
+ {
+ 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;
+ }
+
+ uart0SendCh(stepsToPos+48);
+
+
}
break;
case EVENT_EXIT:
- // set the direction of move to minimalize the move distance
-
- // !! need to implement direction check
- moveDirection= FORWARD;
-
uart0SendCh('a');
break;
}
if (moveDirection==FORWARD) engine_A_dir(ENGINE_DIR_FW);
else engine_A_dir(ENGINE_DIR_BW);
- engine_A_pwm(LOWSPEED);
+ engine_A_pwm(MIDSPEED);
engine_A_en(ENGINE_EN_ON);
uart0SendCh('c');
break;
if (moveDirection==FORWARD) engine_A_dir(ENGINE_DIR_BW);
else engine_A_dir(ENGINE_DIR_FW);
- engine_A_pwm(LOWSPEED);
+ engine_A_pwm(LOWSPEED);
engine_A_en(ENGINE_EN_ON);
uart0SendCh('d');
stateReading= 0; // the state is over now, go to default state
validPosition= signalToInt[reading]; //decode the position;
validReading= 1; // reading is valid
-
+
+ flashLEDB= 1;
uart0SendCh(validPosition+48);
}
} while (1);
engine_A_pwm(0);
+ uart0SendCh('i');
// if the position is not exact, go back
unsigned int input;
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);
//uart0SendCh( (char) initPos+48 );
uart0SendCh( ';');
}
- timeNextPos= oneTurn/cycles;
+
+ // 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( '-');
+
timeNext2Pos= twoTurns/cycles;
}
* wait time specified in the interaval
* int interval - interval in ms
*
- * !!! need to implement overflow !!!!
+ * need to implement overflow when needed durations in days..
*/
void delay(int interval)
{
projects / eurobot / public.git / commitdiff