/**
* @Function RunTemplateSubHSM(ES_Event ThisEvent)
* @param ThisEvent - the event (type and param) to be responded.
* @return Event - return event (type and param), in general should be ES_NO_EVENT
* @brief This function is where you implement the whole of the heirarchical state
* machine, as this is called any time a new event is passed to the event
* queue. This function will be called recursively to implement the correct
* order for a state transition to be: exit current state -> enter next state
* using the ES_EXIT and ES_ENTRY events.
* @note Remember to rename to something appropriate.
* The lower level state machines are run first, to see if the event is dealt
* with there rather than at the current level. ES_EXIT and ES_ENTRY events are
* not consumed as these need to pass pack to the higher level state machine.
* @author J. Edward Carryer, 2011.10.23 19:25
* @author Gabriel H Elkaim, 2011.10.23 19:25 */
ES_Event RunAlignCollect_SubHSM(ES_Event ThisEvent) {
uint8_t makeTransition = FALSE; // use to flag transition
SubAlignCollect_SubHSMState_t nextState; // <- change type to correct enum
ES_Tattle(); // trace call stack
ES_Event Tcontrol;
static int aligning_start_time, aligning_end_time;
static int aligning_difference_time = 0;
static int back_tape_flag = 0;
switch (CurrentState) {
case InitPSubState: // If current state is initial Psedudo State
if (ThisEvent.EventType == ES_INIT)// only respond to ES_Init
{
nextState = Buffer_State;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case Buffer_State:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
//PivotRight(newspeed);
break;
case ES_EXIT:
break;
case FORCED:
nextState = Checking;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
// ThisEvent.EventParam = ThisEvent.EventParam;
break;
case ES_TIMEOUT:
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
break;
default:
break;
}
}
break;
case Checking:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
PivotRight(speed*1.2);//(speed*.5);
// TurnRight(speed);
// StraightForward(speed);
// ES_Timer_InitTimer(replungetimer, 300);
break;
case ES_EXIT:
break;
case TAPE:
if ((ThisEvent.EventParam & 0x01) == 0x01) { // B
printf("\nback on\n");
aligning_start_time = ES_Timer_GetTime();
back_tape_flag = 1;
int i;
for(i = 0; i < 100000; i++)
{
asm("nop");
}
// nextState = Pivoting_Right_AC;
// makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if (((ThisEvent.EventParam & 0b000001) == 0b000000) && back_tape_flag) { // Back tape off T
aligning_end_time = ES_Timer_GetTime();
FullStop();
printf("\nback tape off\n");
int i;
for(i = 0; i < 100000; i++)
{
asm("nop");
}
aligning_difference_time = aligning_end_time - aligning_start_time;
// printf("\n\naligning_diff_time %d\n", aligning_difference_time);
nextState = Pivoting_Left_AC;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}// if ((ThisEvent.EventParam & 0b110000) == 0b110000) { // LL, L: if miss most of the stem/land on T top
// nextState = Pivoting_Right_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
// else if ((ThisEvent.EventParam & 0b111100) == 0b111100) { // LL, L,F, R: if hit T stem head-on
// nextState = Turning_Right_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// } else if ((ThisEvent.EventParam & 0b100000) == 0b100000) { // LL entry
// nextState = Turning_Left_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
else {
ThisEvent.EventType = ES_NO_EVENT;
}
// else if ((ThisEvent.EventParam & T_Left) == T_Left) {
// nextState = Pivoting_Right;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// } else if ((ThisEvent.EventParam & T_Right) == T_Right) {
// nextState = Pivoting_Left;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
break;
case ES_TIMEOUT:
// nextState = Pivoting_Right_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
break;
default:
break;
}
}
break;
case Pivoting_Right_AC:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
PivotRight(tspeed * 1);
ES_Timer_InitTimer(replungetimer, 900);
break;
// case TAPE:
// if ((ThisEvent.EventParam & 0x01) == 0x01) { // B
// nextState = Reverse_Adjusting_Right;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
// else {
// ThisEvent.EventType = ES_NO_EVENT;
// }
// break;
case ES_EXIT:
FullStop();
break;
case ES_TIMEOUT:
// nextState = Lost;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
if (ThisEvent.EventParam == replungetimer) {
nextState = Reverse_Adjusting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default:
break;
}
}
break;
case Pivoting_Left_AC:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
PivotLeft_WH(speed * .7);
int spintime = aligning_difference_time / 2;
// printf("\n\n%dcheck spin time\n", spintime);
ES_Timer_InitTimer(replungetimer, spintime);
break;
case ES_EXIT:
FullStop();
break;
case ES_TIMEOUT:
// nextState = Lost;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
if (ThisEvent.EventParam == replungetimer) {
FullStop();
// printf("\n\ncheck\n");
nextState = Reverse_Adjusting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default:
break;
}
}
break;
case Turning_Left_AC:
//ThisEvent = Run_AA_LineFollowing_SubHSM(ThisEvent); // run sub-state machine for this state
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
TurnLeft(speed);
break;
case TAPE:
if ((ThisEvent.EventParam & 0b000100) == 0b000100) { // R
nextState = Turning_Right_AC; //TODO: straight on
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
FullStop();
break;
case ES_TIMEOUT:
// ThisEvent.EventType = ES_NO_EVENT;
default: break;
}
}
break;
case Turning_Right_AC:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
TurnRight(speed);
break;
// case BUMPED:
// nextState = Wall_Hugging;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// break;
case TAPE:
if ((ThisEvent.EventParam & 0x01) == 0x01) { // B
nextState = Reverse_Adjusting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
FullStop();
break;
case ES_TIMEOUT:
// nextState = Lost;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
break;
default: break;
}
}
break;
case Stopping_AC:
FullStop();
break;
case Reversing_AC:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
StraightReverse(speed * 1.3);
ES_Timer_InitTimer(ammofindtimer, ammofindtimervalue);
break;
case ES_EXIT:
ES_Timer_StopTimer(ammofindtimer);
ES_Timer_StopTimer(portalsearchingtimer);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == ammofindtimer) {
ES_Timer_InitTimer(portalsearchingtimer, 600);
StraightForward(speed);
ThisEvent.EventType = ES_NO_EVENT;
} else if (ThisEvent.EventParam == portalsearchingtimer) {
FullStop();
Tcontrol.EventType = AMMO;
Tcontrol.EventParam = 0;
ThisEvent.EventType = ES_NO_EVENT;
PostDagobotHSM(Tcontrol);
}
break;
default:
break;
}
}
break;
case Forwarding_AC:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
StraightForward(speed);
break;
case TAPE:
if ((ThisEvent.EventParam & 0b001000) == 0b000000) { // Front off the T
nextState = Reversing_AC;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
break;
case ES_TIMEOUT:
break;
default:
break;
}
}
break;
case Reverse_Adjusting_Right:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
// GentleLeft(-6 - .3); // backwards left = front of robot pivoting right
GentleRight(-6 - .3);
ES_Timer_InitTimer(replungetimer, replungetimervalue);
break;
// case TAPE:
// if ((ThisEvent.EventParam & 0b111110) == 0b000000) { // F
// nextState = Stopping_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// } else {
// ThisEvent.EventType = ES_NO_EVENT;
// }
// break;
case ES_EXIT:
ES_Timer_StopTimer(replungetimer);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == replungetimer) {
nextState = Forwarding_AC;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default:
break;
}
}
break;
case Adjusting_Right_AC:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
GentleRight(speed);
break;
case TAPE:
if ((ThisEvent.EventParam & 0b011100) == 0b011100) { // L, C, R
nextState = Reverse_Adjusting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
break;
}
}
break;
case Reverse_Turning_Left:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
//ES_Timer_InitTimer(pivotingtimer, pivotingtimervalue);
ReverseLeft(speed); // fiz thinks this means reverse right // previously 5
ES_Timer_InitTimer(ammofindtimer, ammofindtimervalue);
break;
// case TAPE:
// if ((ThisEvent.EventParam & 0b011100) == 0b011100){ // L, C, R
// nextState = Reverse_Adjusting_Right;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
// break;
//
case ES_TIMEOUT:
if (ThisEvent.EventParam == ammofindtimer) {
Tcontrol.EventType = MISSED_T;
Tcontrol.EventParam = 0;
PostDagobotHSM(Tcontrol);
ThisEvent.EventType = ES_NO_EVENT;
}
// else if (plungecount == 4){
// debug("Oh, shit!");
// nextState = Stopping_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// //TODO: Deal with later!
// // post event to pop into higher state
// }
break;
case ES_EXIT:
ES_Timer_StopTimer(ammofindtimer);
break;
}
}
break;
default: // all unhandled states fall into here
break;
} // end switch on Current State
if (makeTransition == TRUE) {
RunAlignCollect_SubHSM(EXIT_EVENT); // <- rename to your own Run function
CurrentState = nextState;
RunAlignCollect_SubHSM(ENTRY_EVENT); // <- rename to your own Run function
}
ES_Tail(); // trace call stack end
return ThisEvent;
}
/**
* @Function RunEngage_DestroySubHSM(ES_Event ThisEvent)
* @param ThisEvent - the event (type and param) to be responded.
* @return Event - return event (type and param), in general should be ES_NO_EVENT
* @brief This function is where you implement the whole of the heirarchical state
* machine, as this is called any time a new event is passed to the event
* queue. This function will be called recursively to implement the correct
* order for a state transition to be: exit current state -> enter next state
* using the ES_EXIT and ES_ENTRY events.
* @note Remember to rename to something appropriate.
* The lower level state machines are run first, to see if the event is dealt
* with there rather than at the current level. ES_EXIT and ES_ENTRY events are
* not consumed as these need to pass pack to the higher level state machine.
* @author J. Edward Carryer, 2011.10.23 19:25
* @author Gabriel H Elkaim, 2011.10.23 19:25 */
ES_Event RunEngage_DestroySubHSM(ES_Event ThisEvent) {
uint8_t makeTransition = FALSE; // use to flag transition
SubEngage_DestroyState_t nextState;
ES_Event eventtopost, donefiring;
ES_Tattle(); // trace call stack
switch (CurrentState) {
case Init_ED: // If current state is initial Psedudo State
if (ThisEvent.EventType == ES_INIT) {// only respond to ES_Init
//call all substate run functions here
InitEngage_Destroy_Wall_Hugging_SubHSM();
nextState = Engage_Destroy_Searching;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case Engage_Destroy_Searching:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
// ES_Timer_InitTimer(15, 2500); //TODO: Move to exit from acquiring_ammo
// PivotRight(speed);
// PivotRight(speed);
StraightForward(speed); //speed // move forwards away from the ammo dump
//pivot timer
// start pivoting
Dagobot_SetLoaderServo(1200);
ThisEvent.EventType = ES_NO_EVENT;
// ThisEvent.EventParam = 0;
// PostDagobotHSM(ThisEvent);
//TODO: Add flag to only go forwards if just from Acquiring_Ammo?
break;
// case AMMO:
// ThisEvent.EventType = ES_NO_EVENT;
// break;
case ES_EXIT:
FullStop(); // aimed at the enemy: stop and fire
ES_Timer_StopTimer(15);
// ES_Timer_StopTimer(forwardtimer);
break;
case SNIPED:
// debug("check event");
FullStop();
nextState = Firing;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
break;
case BUMPED:
nextState = Engage_Destroy_Wall_Hugging;
makeTransition = TRUE;
eventtopost.EventType = BUMP_CHECK;
eventtopost.EventParam = ThisEvent.EventParam;
// ThisEvent.EventType = BUMP_CHECK;
// ThisEvent.EventParam = ThisEvent.EventParam;
PostDagobotHSM(eventtopost);
break;
//
// case TAPE:
// nextState = Redirecting;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// break;
case ES_TIMEOUT:
//if (ThisEvent.EventParam == forwardtimer) {
// ES_Timer_InitTimer(pivotingtimer, 800);
// PivotRight(newspeed);
//}
// if (ThisEvent.EventParam == 15) {
// FullStop(); // stop after spinning
// StraightForward(speed*1.25); // didn't find enemy, head towards obstacle (or at least forwards)
// }
// ThisEvent.EventType = ES_NO_EVENT;
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Firing:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is active
switch (ThisEvent.EventType) {
case ES_ENTRY:
// int AmmoState = Dagobot_ReadLoaderTape();
// if (AmmoState == 1) { // have ammo!
// debug("check");
FullStop();
ES_Timer_InitTimer(ammofindtimer, 2000); // head towards deadbot
StraightForward(speed);
// }
break;
// case SNIPED:
// if(ThisEvent.EventParam == 0)
// {
//
// }
// case BUMPED:
// if ((ThisEvent.EventParam & FRONT_LEFT_BUMP) == FRONT_LEFT_BUMP) { // front left bump
//
// ThisEvent.EventType = ES_NO_EVENT;
// } else if ((ThisEvent.EventParam & FRONT_RIGHT_BUMP) == FRONT_RIGHT_BUMP) { // front right bump
// nextState = Reversing_WH;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// } else if ((ThisEvent.EventParam & FRONT_BUMPS) == FRONT_BUMPS) { // both front bumpers
// nextState = Reversing_WH;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
// else {
// ThisEvent.EventType = ES_NO_EVENT;
// }
// break;
case ES_EXIT:
// ES_TIMER_StopTimer(motortimer);
Dagobot_SetLoaderServo(1200);
ES_Timer_StopTimer(shootingtimer);
ES_Timer_StopTimer(ammofindtimer);
// ThisEvent.EventType = ES_NO_EVENT; //TODO: remove??
break;
case ES_TIMEOUT:
if(ThisEvent.EventParam == ammofindtimer){
FullStop();
int i;
for(i = 0; i < 30000; i++)
{
asm("nop");
}
Dagobot_SetLoaderServo(2000); // start running up motor
ES_Timer_InitTimer(shootingtimer, shootingtimervalue);
ThisEvent.EventType = ES_NO_EVENT;
}
// if (ThisEvent.EventParam == motortimervalue) {//done spinning up motor
// Dagobot_SetLoaderServo(servorelease); // drop balls into shooter
// ES_Timer_InitTimer(shootingtimer, shootingtimervalue);
// }
if (ThisEvent.EventParam == shootingtimer) {
donefiring.EventType = DONE_FIRING; // upper level deal with getting to portal
donefiring.EventParam = 0;
PostDagobotHSM(donefiring);
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Engage_Destroy_Wall_Hugging:
ThisEvent = RunEngage_Destroy_Wall_Hugging_SubHSM(ThisEvent); // run sub-state machine for this state
//TODO: make new wall_hugging file/fn?
// and/or deal with entry from differet events
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is active
switch (ThisEvent.EventType) {
case ES_ENTRY:
// this is where you would put any actions associated with the
// entry to this state
// ES_Timer_InitTimer(wallhuggingtimer, 8000);
break;
case SNIPED:
FullStop();
nextState = Firing;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
break;
// case TAPE:
// if((ThisEvent.EventParam & 0x000010) != 0x000010) {
// nextState = Redirecting;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
// break;
case ES_EXIT:
// this is where you would put any actions associated with the
// exit from this state
// ES_Timer_StopTimer(wallhuggingtimer);
FullStop();
break;
case ES_TIMEOUT:
// if (ThisEvent.EventParam == wallhuggingtimer) { // timer to check for enemy every once in a while while bumping around
// //TODO: make new checkin timer?
// nextState = Engage_Destroy_Searching; // search
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
//
// }
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Redirecting:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
PivotRight(speed);
ES_Timer_InitTimer(pivotingtimer, pivotingtimervalue);
break;
case ES_EXIT:
ES_Timer_StopTimer(pivotingtimer);
// ThisEvent.EventType = ES_NO_EVENT;
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam = pivotingtimer) {
nextState = Engage_Destroy_Searching; // search
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT; // head for whatever's in the future
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
default: // all unhandled states fall into here
break;
} // end switch on Current State
if (makeTransition == TRUE) { // making a state transition, send EXIT and ENTRY
// recursively call the current state with an exit event
RunEngage_DestroySubHSM(EXIT_EVENT); // <- rename to your own Run function
CurrentState = nextState;
RunEngage_DestroySubHSM(ENTRY_EVENT); // <- rename to your own Run function
}
ES_Tail(); // trace call stack end
return ThisEvent;
}
int GoToCoords(double newra, double newdec, int pmodel)
{
char sendstr[] = { 0x50, 0x04, 0x10, 0x17, 0x00, 0x00, 0x00, 0x00 };
char returnstr[32];
int azcount, azcount0, azcount1, azcount2;
int altcount, altcount0, altcount1, altcount2;
int numread;
double newha, newalt, newaz;
double newha0, newalt0, newaz0;
double newra0, newdec0;
double newra1, newdec1;
double nowha0, nowra0, nowdec0;
double encoderalt = 0.;
double encoderaz = 0.;
/* Select fast slew command if needed */
/* Note: may place large inertial load on the gear train */
if( SLEWFAST )
{
sendstr[3] = 0x02;
}
newha = LSTNow() - newra;
newha = Map12(newha);
/* Convert HA and Dec to Alt and Az */
/* Test for visibility */
EquatorialToHorizontal(newha, newdec, &newaz, &newalt);
/* Check altitude limit */
if (newalt < MINTARGETALT)
{
fprintf(stderr,"Target is below the telescope horizon\n");
slewphase = 0;
return(0);
}
/* Target request is valid */
/* Find the best path to target */
/* Mount coordinates for the target */
newra1 = newra;
newdec1 = newdec;
PointingToTel(&newra0,&newdec0,newra1,newdec1,pmodel);
newha0 = LSTNow() - newra0;
newha0 = Map12(newha0);
EquatorialToHorizontal(newha0, newdec0, &newaz0, &newalt0);
/* Stop all mount motion in preparation for a slew */
FullStop();
/* Get current mount coordinates */
GetTel(&nowra0, &nowdec0, RAW);
nowha0 = LSTNow() - nowra0;
nowha0 = Map12(nowha0);
/* Prepare encoder counts for a new slew */
/* German equatorial */
if (telmount == GEM)
{
/* Flip signs for the southern sky */
if (SiteLatitude < 0.)
{
newdec0 = -1.*newdec0;
newha0 = -1.*newha0;
}
if ((newha0 >= -12.) && ( newha0 < -6.))
{
slewphase = 1;
encoderaz = newha0*15. + 180.;
encoderalt = newdec0;
}
else if ((newha0 >= -6.) && ( newha0 <= 0.))
{
slewphase = 1;
encoderaz = newha0*15. + 180.;
encoderalt = newdec0;
}
else if ((newha0 > 0.) && ( newha0 <= 6.))
{
slewphase = 1;
encoderaz = newha0*15.;
encoderalt = 180. - newdec0;
}
else if ((newha0 > 6.) && ( newha0 <= 12.))
{
slewphase = 1;
encoderaz = newha0*15.;
encoderalt = 180. - newdec0;
}
else
{
fprintf(stderr,"German equatorial slew request error\n");
return(0);
}
if (newha0 == 0.)
{
/* OTA looking at meridian */
/* This is ambiguous unless we know which side of the pier it is on */
/* Assume telescope was on the west side looking east */
/* and was moved to point to the meridian with the OTA west of pier */
slewphase = 1;
fprintf(stderr,"Warning: assuming OTA is west of pier.\n");
encoderaz = 90.;
encoderalt = newdec0 - 90.;
}
else if (newha0 == -6.)
{
/* OTA looking east */
slewphase = 1;
encoderaz = 0.;
encoderalt = newdec0 - 90.;
}
else if (newha0 == 6.)
{
/* OTA looking west */
slewphase = 1;
encoderaz = 0.;
encoderalt = 90. - newdec0;
}
else if ((newha0 > -12.) && ( newha0 < -6.))
{
/* OTA east of pier looking below the pole */
slewphase = 1;
encoderaz = newha0*15. + 90.;
encoderalt = newdec0 - 90.;
}
else if ((newha0 > -6.) && ( newha0 < 0.))
{
/* OTA west of pier looking east */
slewphase = 1;
encoderaz = newha0*15. + 90.;
encoderalt = newdec0 - 90.;
}
else if ((newha0 > 0.) && ( newha0 <= 6.))
{
/*OTA east of pier looking west */
slewphase = 1;
encoderaz = newha0*15. - 90.;
encoderalt = 90. - newdec0;
}
else if ((newha0 > 6.) && ( newha0 < 12.))
{
/* OTA west of pier looking below the pole */
slewphase = 1;
encoderaz = newha0*15. - 90.;
encoderalt = 90. - newdec0;
}
else
{
fprintf(stderr,"German equatorial slew request outside limits\n");
return(0);
}
/* Tests for safe slew based on encoder readings would go here */
/* Test need for two-segment slew for changes of more than 90 degrees */
if ((fabs(telencoderalt - encoderalt) > 90.1) ||
(fabs(telencoderaz - encoderaz) > 90.1))
{
/* Slew request of more than 90 degrees on one axis */
if ( fabs(telencoderalt) > 10. )
{
/* Telescope currently more than 10 degrees from the pole in dec */
/* Set new target to switch position */
encoderalt = switchalt;
encoderaz = switchaz;
slewphase = 2;
}
}
encoderalt = encoderalt*altcountperdeg;
encoderaz = encoderaz*azcountperdeg;
}
/* Equatorial fork */
if (telmount == EQFORK)
{
slewphase = 1;
encoderaz = newha0*15.;
encoderalt = newdec0;
/* Tests for safe slew based on encoder readings would go here */
encoderalt = encoderalt*altcountperdeg;
encoderaz = encoderaz*azcountperdeg;
}
/* Alt-az fork */
if (telmount == ALTAZ)
{
slewphase = 1;
encoderaz = newaz0;
encoderalt = newalt0;
/* Tests for safe slew based on encoder readings would go here */
encoderaz = encoderaz*azcountperdeg;
encoderalt = encoderalt*altcountperdeg;
}
/* Convert encoder angle readings to encoder counter readings */
azcount = encoderaz;
if (azcount < 0)
{
azcount = 16777217 + azcount;
}
altcount = encoderalt;
if (altcount < 0)
{
altcount = 16777217 + altcount;
}
/* Prepare NexStar commands */
/* Parse each of the 3 bytes of the counters */
azcount0 = azcount / 65536;
azcount = azcount % 65536;
azcount1 = azcount / 256;
azcount2 = azcount % 256;
altcount0 = altcount / 65536;
altcount = altcount % 65536;
altcount1 = altcount / 256;
altcount2 = altcount % 256;
/* Send command to go to new RA/Azimuth */
sendstr[1] = 0x04;
sendstr[2] = 0x10;
sendstr[3] = 0x17;
sendstr[4] = (unsigned short) azcount0;
sendstr[5] = (unsigned short) azcount1;
sendstr[6] = (unsigned short) azcount2;
tcflush(TelPortFD,TCIOFLUSH);
writen(TelPortFD,sendstr,8);
numread=readn(TelPortFD,returnstr,1,2);
/* Send command to go to new Dec/Altitude */
sendstr[1] = 0x04;
sendstr[2] = 0x11;
sendstr[3] = 0x17;
sendstr[4] = (unsigned short) altcount0;
sendstr[5] = (unsigned short) altcount1;
sendstr[6] = (unsigned short) altcount2;
tcflush(TelPortFD,TCIOFLUSH);
writen(TelPortFD,sendstr,8);
numread=readn(TelPortFD,returnstr,1,2);
tcflush(TelPortFD,TCIOFLUSH);
/* A slew is in progress */
return(1);
}
/**
* @Function RunLine_FollowingSubHSM(ES_Event ThisEvent)
* @param ThisEvent - the event (type and param) to be responded.
* @return Event - return event (type and param), in general should be ES_NO_EVENT
* @brief This function is where you implement the whole of the heirarchical state
* machine, as this is called any time a new event is passed to the event
* queue. This function will be called recursively to implement the correct
* order for a state transition to be: exit current state -> enter next state
* using the ES_EXIT and ES_ENTRY events.
* @note Remember to rename to something appropriate.
* The lower level state machines are run first, to see if the event is dealt
* with there rather than at the current level. ES_EXIT and ES_ENTRY events are
* not consumed as these need to pass pack to the higher level state machine.
* @author J. Edward Carryer, 2011.10.23 19:25
* @author Gabriel H Elkaim, 2011.10.23 19:25 */
ES_Event Run_AA_LineFollowing_SubHSM(ES_Event ThisEvent) {
uint8_t makeTransition = FALSE; // use to flag transition
AA_LineFollowing_subHSM_t nextState; // <- change type to correct enum
ES_Tattle(); // trace call stack
switch (CurrentState) {
case Init_AA_LineFollowing: // If current state is initial Psedudo State
if (ThisEvent.EventType == ES_INIT)// only respond to ES_Init
{
nextState = EntryLineFollowing;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case EntryLineFollowing:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
StraightForward(speed);
break;
case ES_EXIT:
break;
case TAPE:
if (ThisEvent.EventParam == 0b100000) { // LL: 20, 100000
FullStop();
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if (ThisEvent.EventParam == 0b000010) { // RR: 02
FullStop();
nextState = Pivoting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b110010) == 0b110010) { // LL, L, RR
FullStop();
nextState = Reversing_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b001001) == 0b001001) { // F, B
FullStop();
nextState = Adjusting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
}
}
break;
case Adjusting_Left:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
TurnLeft(speed * 1);
break;
case TAPE:
if ((ThisEvent.EventParam & 0b000010) == 0b000010) { // RR (corner)
nextState = Pivoting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b001000) == 0b001000) { //F
FullStop();
nextState = Adjusting_Right; //Adjusting_Right
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
break;
}
}
break;
case Adjusting_Right:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
TurnRight(speed * 1);
break;
case TAPE:
if ((ThisEvent.EventParam & 0b010000) == 0b010000) { // L
FullStop();
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b100000) == 0b100000) { // LL
FullStop();
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b000010) == 0b000010) { // RR (corner)
nextState = Pivoting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b000110) == 0b000110) { //R, RR (corner)
nextState = Pivoting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
break;
}
}
break;
case Pivoting_Right:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
ES_Timer_InitTimer(ammofindtimer, 2000);
PivotRight(speed);
break;
case ES_EXIT:
ES_Timer_StopTimer(ammofindtimer);
break;
case TAPE:
if ((ThisEvent.EventParam & 0b110010) == 0b110010) { // LL, L, RR
nextState = Reversing_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b010000) == 0b010000) { // L
FullStop();
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b100001) == 0b100001) { // LL, B
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b100000) == 0b100000) { // LL
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == ammofindtimer) {
nextState = EntryLineFollowing;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
}
}
break;
case Reversing_Left:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
ES_Timer_InitTimer(turningtimer, turningtimervalue);
TurnLeft(-(speed));
break;
case ES_EXIT:
ES_Timer_StopTimer(turningtimer);
break;
case ES_TIMEOUT:
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
case TAPE:
if ((ThisEvent.EventParam & 0b100000) == 0b100000) { // LL
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
}
}
break;
default: // all unhandled states fall into here
break;
} // end switch on Current State
if (makeTransition == TRUE) { // making a state transition, send EXIT and ENTRY
// recursively call the current state with an exit event
Run_AA_LineFollowing_SubHSM(EXIT_EVENT);
CurrentState = nextState;
Run_AA_LineFollowing_SubHSM(ENTRY_EVENT);
}
ES_Tail(); // trace call stack end
return ThisEvent;
}
