int main(void) {
int i;
unsigned short pattern;
char j,k;
SERIAL_Init();
AD1PCFG = 0xFF;
INTEnableSystemMultiVectoredInt();
//enable LED bank 1
TRISDbits.TRISD6 = 0;
TRISDbits.TRISD11 = 0;
TRISDbits.TRISD3 = 0;
TRISDbits.TRISD5 = 0;
printf("\nHello World!");
Stepper_Init(100);
printf("\nStepping forward 420 steps");
Stepper_SetSteps(FORWARD,420);
for (j=0;j<10;j++) {
DELAY();
printf(".");
DELAY();
printf("\t%u",Stepper_GetRemainingCount());
}
while(Stepper_GetRemainingCount() > 1) {
printf("o");
DELAY();
}
printf("\nForward steps done");
Stepper_SetSteps(REVERSE,220);
while (Stepper_GetRemainingCount() > 1) {
printf("+");
}
Stepper_End();
TRIS_COIL_A_DIRECTION = 0;
TRIS_COIL_A_ENABLE = 0;
TRIS_COIL_B_DIRECTION = 0;
TRIS_COIL_B_ENABLE = 0;
ShutDownDrive();
COIL_A_ENABLE = 0;
COIL_A_DIRECTION = 0;
COIL_B_ENABLE = 0;
COIL_B_DIRECTION = 0;
printf("\n Turning on COIL_B_DIRECTION (PortZ-07");
for (j=0;j<10;j++) {
printf("o");
DELAY();
}
while (1) {
COIL_B_DIRECTION ^= 1;
DELAY();
printf(".");
}
return 0;
}
int main(void) {
int i;
SERIAL_Init();
AD1PCFG = 0xFF;
INTEnableSystemMultiVectoredInt();
//enable LED bank 1
TRISDbits.TRISD6 = 0;
TRISDbits.TRISD11 = 0;
TRISDbits.TRISD3 = 0;
TRISDbits.TRISD5 = 0;
//enable LED bank 2
TRISFbits.TRISF6 = 0;
TRISGbits.TRISG7 = 0;
TRISDbits.TRISD7 = 0;
TRISGbits.TRISG8 = 0;
printf("\nHello World!");
Stepper_Init(STEPPER_SPEED);
Stepper_SetMode(STEPPER_MODE);
printf("\nStepping forward 420 steps");
Stepper_SetSteps(FORWARD,220);
while(Stepper_GetRemainingCount() > 1) {
printf("\t%u,speed=%u",Stepper_GetRemainingCount(),Stepper_GetSpeed());
DELAY();
#ifdef STEPPER_RAMPS
if (Stepper_GetRemainingCount() % 5) {
Stepper_ChangeStepRate( (Stepper_GetSpeed() / 4) + Stepper_GetSpeed() );
}
#endif
}
printf("\nForward steps done");
DELAY();
Stepper_SetSteps(REVERSE,220);
while (Stepper_GetRemainingCount() > 1) {
printf("\t%u",Stepper_GetRemainingCount());
DELAY();
}
Stepper_End();
return 0;
}
int main(void)
{
/* Setup SysTick Timer for 3 msec interrupts. */
if (SysTick_Config(SystemCoreClock * 3 / 35000))
while (1);
/* Enable board LEDs */
STM_EVAL_LEDInit(LED3); /* PC9 */
STM_EVAL_LEDInit(LED4); /* PC8 */
/* Init USART on PA9(TX), PA10(RX) */
USARTx_Init();
/* M0(PA2,PA3,PA4,PA5); M1(PC0,PC1,PC2,PC3); M2(PC4,PC5,PC6,PC7) */
Stepper_Init();
/* Init encoders on TIM3(PA6,PA7), TIM1(PA8, PA9) and TIM2(PA0, PA1) */
Encoder_1_Init();
Encoder_2_Init();
Encoder_3_Init();
while (1)
{
Stepper_Set_Destination(0, Encoder_1_Get_Value() * ENCODER_TO_STEPPER_RATE);
Stepper_Set_Destination(1, Encoder_2_Get_Value() * ENCODER_TO_STEPPER_RATE);
Stepper_Set_Destination(2, Encoder_3_Get_Value() * ENCODER_TO_STEPPER_RATE);
/* [TODO]: Check right value */
if (checkValue(Encoder_1_Get_Value(), ENCODER_1_RIGHT_VALUE) &&
checkValue(Encoder_2_Get_Value(), ENCODER_2_RIGHT_VALUE) &&
checkValue(Encoder_3_Get_Value(), ENCODER_3_RIGHT_VALUE))
STM_EVAL_LEDOn(LED3);
else
STM_EVAL_LEDOff(LED3);
}
}
int main(void) {
int i;
unsigned short pattern;
char j,k;
SERIAL_Init();
AD1PCFG = 0xFF;
INTEnableSystemMultiVectoredInt();
//enable LED bank 1
TRISDbits.TRISD6 = 0;
TRISDbits.TRISD11 = 0;
TRISDbits.TRISD3 = 0;
TRISDbits.TRISD5 = 0;
//enable LED bank 2
TRISFbits.TRISF6 = 0;
TRISGbits.TRISG7 = 0;
TRISDbits.TRISD7 = 0;
TRISGbits.TRISG8 = 0;
printf("\nHello World!");
Stepper_Init(stepSpeed);
printf("\nStepping forward 420 steps");
Stepper_SetSteps(FORWARD,420);
while(Stepper_GetRemainingCount() > 1) {
printf("\t%u,%u",Stepper_GetRemainingCount(),stepSpeed);
DELAY();
#ifdef STEPPER_RAMPS
if (Stepper_GetRemainingCount() % 10) {
Stepper_SetSpeed( (stepSpeed / 4) + stepSpeed );
}
#endif
}
printf("\nForward steps done");
DELAY();
Stepper_SetSteps(REVERSE,220);
while (Stepper_GetRemainingCount() > 1) {
printf("\t%u",Stepper_GetRemainingCount());
DELAY();
}
Stepper_End();
TRIS_COIL_A_DIRECTION = 0;
TRIS_COIL_A_ENABLE = 0;
TRIS_COIL_B_DIRECTION = 0;
TRIS_COIL_B_ENABLE = 0;
ShutDownDrive();
ADisable();
ADirectionOff();
BDisable();
BDirectionOff();
printf("\n Turning on COIL_B_DIRECTION (PortZ-07");
for (j=0;j<10;j++) {
printf("o");
DELAY();
}
while (1) {
COIL_B_DIRECTION ^= 1;
DELAY();
printf(".");
}
return 0;
}
/**
* @Function RunRoachFSM(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 is the function that implements the actual state machine; in the
* roach case, it is a simple ping pong state machine that will form the
* basis of your more complicated exploration. 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.
* @author Gabriel H Elkaim, 2013.09.26 15:33 */
ES_Event RunMotorYFSM(ES_Event ThisEvent) {
uint8_t makeTransition = FALSE;
Motor_Y_State_t nextState;
ES_Tattle(); // trace call stack
static unsigned int eventParam = 0;
static unsigned int i = 0;
static unsigned int j = 0;
switch (CurrentState) {
case InitQState:
if (ThisEvent.EventType == ES_INIT) {
nextState = selfCalibrateY;
makeTransition = TRUE;
}
break;
case selfCalibrateY:
switch (ThisEvent.EventType) {
case ES_ENTRY:
break;
case CALIBRATED:
if (ThisEvent.EventParam == MOTOR_X) {
Stepper_Init(MOTOR_Y, 77);
reverseY(4000);
ES_Timer_InitTimer(MOTOR_Y_TIMER, 5000);
}
break;
case Y_LIMIT:
if (ThisEvent.EventParam == Y_MIN_MASK) {
initialY = getFilteredAD(Y_SLIDER);
newPosition = AD_SCALE_Y * initialY;
currentPosition = 0;
setStepsTaken(MOTOR_Y, 0);
positionDifference = currentPosition - newPosition;
if (positionDifference < 0) {
//forwardX(abs(positionDifference));
} else if (positionDifference > 0) {
1; //do nothing, already at zero
}
Stepper_Halt(MOTOR_Y);
}
break;
//case Y_LIMIT:
case ES_TIMEOUT:
if (ThisEvent.EventParam == MOTOR_Y_TIMER) {
initialY = getFilteredAD(Y_SLIDER);
//Stepper_Resume(MOTOR_Y);
newPosition = AD_SCALE_Y * initialY;
currentPosition = 0;
setStepsTaken(MOTOR_Y, 0);
positionDifference = currentPosition - newPosition;
if (positionDifference < 0) {
//forwardY(abs(positionDifference));
} else if (positionDifference > 0) {
1; //do nothing, already at zero
}
nextState = waitingY;
makeTransition = TRUE;
}
break;
case ES_EXIT:
Stepper_Halt(MOTOR_Y);
ES_Timer_StopTimer(MOTOR_Y_TIMER);
break;
default:
break;
}
break;
case waitingY:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
break;
case XY_CHANGE:
nextState = movingBufferY;
makeTransition = TRUE;
memEventY = ThisEvent;
break;
case PRESETCHANGE:
//Coming from presetFSM
//nextState = movingY_Preset;
//makeTransition = TRUE;
break;
case ES_EXIT:
Stepper_Resume(MOTOR_Y);
setStepsRate(MOTOR_Y, 77);
//ES_Timer_StopTimer(MOTOR_Y_TIMER);
break;
default:
break;
}
}
break;
case movingBufferY:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
ES_Timer_InitTimer(MOTOR_Y_TIMER, 200);
break;
case XY_CHANGE:
nextState = movingBufferY;
makeTransition = TRUE;
memEventY = ThisEvent;
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam = MOTOR_Y_TIMER) {
nextState = movingY;
makeTransition = TRUE;
}
break;
case ES_EXIT:
setStepsRate(MOTOR_Y, 77);
break;
default:
break;
}
}
break;
case movingY:
switch (ThisEvent.EventType) {
case ES_ENTRY:
Stepper_Init(MOTOR_Y, 77);
newPosition = AD_SCALE_Y * memEventY.EventParam;
currentPosition = getStepsTaken(MOTOR_Y);
positionDifference = currentPosition - newPosition;
if (positionDifference < 0) {
yDirection = FORWARD;
forwardY(abs(positionDifference));
} else {
yDirection = REVERSE;
reverseY(abs(positionDifference));
}
break;
case ALMOSTOUTOFSTEPS:
setStepsRate(MOTOR_Y, 15);
break;
case OUTOFSTEPS:
Stepper_Halt(MOTOR_Y);
ES_Timer_InitTimer(MOTOR_Y_TIMER, 50);
setStepsRate(MOTOR_Y, 77);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == MOTOR_Y_TIMER) {
Stepper_Halt(MOTOR_Y);
nextState = waitingY;
makeTransition = TRUE;
}
break;
case XY_CHANGE:
nextState = movingBufferY;
makeTransition = TRUE;
memEventY = ThisEvent;
break;
case ES_EXIT:
lastYDirection = yDirection;
setStepsRate(MOTOR_Y, 77);
ES_Timer_StopTimer(MOTOR_Y);
break;
default:
nextState = waitingY;
makeTransition = TRUE;
break;
}
break;
case movingBufferY2:
//ThisEvent = RunmovingYSubFSM(ThisEvent);
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
//printf("movingBuffer Enter:%d\n", memEventY.EventParam);
ES_Timer_InitTimer(MOTOR_Y_TIMER, 200);
break;
case XY_CHANGE:
//printf("Y_CHANGE\n");
//determine if a change of direction is about to occur
newPosition = AD_SCALE_Y * ThisEvent.EventParam;
currentPosition = getStepsTaken(MOTOR_Y);
positionDifference = currentPosition - newPosition;
if (positionDifference < 0) {
yDirection = FORWARD;
} else {
yDirection = REVERSE;
}
//if direction change, start slowing down
memEventY = ThisEvent;
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam = MOTOR_Y_TIMER) {
printf("yDirection:%d, lastYDirection:%d\n", yDirection, lastYDirection);
if (yDirection != lastYDirection) {
printf("directionChange!\n");
//setStepsRate(MOTOR_Y, 15);
//make an event instead!
//ES_Timer_InitTimer(MOTOR_Y, 150);
//while (!(getStepsRate(MOTOR_Y) < 20));
nextState = changingDirection;
makeTransition = TRUE;
} else {
nextState = movingY;
makeTransition = TRUE;
}
}
break;
case ES_EXIT:
//Stepper_Init(MOTOR_Y,500);
break;
default:
break;
}
}
break;
case changingDirection:
switch (ThisEvent.EventType) {
case ES_ENTRY:
//printf("movingBuffer Enter:%d\n", memEventY.EventParam);
//ES_Timer_InitTimer(MOTOR_Y_TIMER, 50);
Stepper_ChangeStepRate(MOTOR_Y, 15);
break;
case SPEEDMATCH:
printf("speedmatch!\n");
nextState = movingY;
makeTransition = TRUE;
break;
case XY_CHANGE:
//printf("Y_CHANGE\n");
nextState = movingBufferY2;
makeTransition = TRUE;
memEventY = ThisEvent;
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam = MOTOR_Y_TIMER) {
nextState = movingY;
makeTransition = TRUE;
}
break;
case ES_EXIT:
//Stepper_Init(MOTOR_Y,500);
break;
default:
break;
}
break;
default:
break;
}
if (makeTransition == TRUE) {
RunMotorYFSM(EXIT_EVENT);
lastState = CurrentState;
CurrentState = nextState;
RunMotorYFSM(ENTRY_EVENT);
}
//printf("Leaving RunMotorYFSM with CurrentState = %d\n", CurrentState);
ES_Tail(); // trace call stack end
return ThisEvent;
}
