Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
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;


}
Ejemplo n.º 3
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);
  }
}
Ejemplo n.º 4
0
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;
}
Ejemplo n.º 5
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;
}