// Initializes the motor in port C
void motor_init() {
max520_init(MAX520_TWI_ADDRESS);
DDRC |= (1<<PC3) | (1<<PC4) | (1<<PC5) | (1<<PC6) | (1<<PC7);
DDRK = 0;
motor_output_enable(ENABLE);
motor_encoder_reset();
motor_encoder_select_byte(ENCODER_HIGH);
motor_speed(0);
motor_enable(ENABLE);
}
/*
motor - x, y or z motor, can be 'motor_x', 'motor_y' or 'motor_z'
dir - direction, can be 'positive' or 'negative'
speed - speed, number in mm/s
*/
void motor_step(Motor_Typedef motor, Motor_Direction dir, float speed){
uint32_t delay;
delay = (int)((float)1/(((float)speed/2)*200)*1000000); //Need change if different stepping methods implemented
if(dir == positive)
GPIO_SetBits(MOTOR_DIR_PORT[motor], MOTOR_DIR_PIN[motor]);
if(dir == negative)
GPIO_ResetBits(MOTOR_DIR_PORT[motor], MOTOR_DIR_PIN[motor]);
motor_enable(motor); //enable
GPIO_ResetBits(MOTOR_STEP_PORT[motor], MOTOR_STEP_PIN[motor]);
motor_delay(delay/2); //need change
GPIO_ToggleBits(MOTOR_STEP_PORT[motor], MOTOR_STEP_PIN[motor]);
motor_delay(delay/2);
}
int main(int argc, char *argv[]) {
struct motor motor1;
led_init(LED3);
button_init(BUTTON_USER);
motor_init(&motor1, GPIOD, MOTOR_ENABLE1, MOTOR_INPUT1, MOTOR_INPUT2);
motor_enable(&motor1);
/* Wait until button get pressed */
button_wait_set(BUTTON_USER);
motor_run(&motor1, MOTOR_RUN_RIGHT);
{
if (fault_detect()) {
fault_handle();
}
}
motor_stop(&motor1);
return 0;
}
void Timer0A_ISR(void)
{
unsigned long timer;
int i;
unsigned char all_steps_completed;
float tmp;
unsigned char endstop_bits;
unsigned char endstops[4];
TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
ROM_IntMasterDisable();
if (cur_block == NULL)
{
if (num_blocks > 0 && blk_queue[blk_queue_head].status == BLOCK_QUEUED && blk_queue[blk_queue_head].recalculate == 0)
{
cur_block = &blk_queue[blk_queue_head];
cur_block->status = BLOCK_RUNNING;
step_rate = cur_block->initial_rate;
acceleration_time=0;
if (!motors_enabled)
motor_enable();
if (endstop_triggered())
{
endstop_bits = ROM_GPIOPinRead(ENDSTOP_PORT, ENDSTOP_ALLPINS);
endstops[X] = ( (endstop_bits & XLIM_PIN) == 0)?1:0;
endstops[Y] = ( (endstop_bits & YLIM_PIN) == 0)?1:0;
endstops[Z] = ( (endstop_bits & ZLIM_PIN) == 0)?1:0;
for (i=0;i<3;i++)
if (cur_block->dir[i]<0 && cur_block->steps[i] > 0 && endstops[i] )
cur_block->status = BLOCK_ABORTED;
}
} else
{
cur_block = NULL;
timer=calculate_timer(MIN_STEP_RATE);
ROM_TimerLoadSet(TIMER0_BASE,TIMER_A, timer);
ROM_TimerEnable(TIMER0_BASE, TIMER_A);
ROM_IntMasterEnable();
return ;
}
}
//if an endstop is hit, the block is aborted.
if (cur_block->status == BLOCK_ABORTED )
{
cur_block->status = BLOCK_ABORTED_RDY;
cur_block = NULL;
//dump the buffer
num_blocks=0;
blk_queue_head=0;
blk_queue_tail=0;
step_events_completed=0;
for (i=0;i<4;i++)
cur_steps[i]=0;
timer=calculate_timer(MIN_STEP_RATE);
ROM_TimerLoadSet(TIMER0_BASE, TIMER_A, timer);
ROM_TimerEnable(TIMER0_BASE, TIMER_A);
ROM_IntMasterEnable();
return;
}
if (step_events_completed <= (cur_block->accelerate_until))
{
step_rate = (unsigned long)((float)acceleration_time/80000000 * (float)(cur_block->acceleration_rate)) + cur_block->initial_rate;
if (step_rate >(cur_block->nominal_rate))
step_rate=cur_block->nominal_rate;
// printf("accel %d %d", acceleration_time, cur_block->acceleration_rate);
} else if (step_events_completed > (cur_block->decelerate_after) && step_rate > cur_block->final_rate)
{
step_rate = peak_rate - (unsigned long)((float)acceleration_time/80000000 * (float)(cur_block->acceleration_rate));
if (step_rate < cur_block->final_rate)
step_rate = cur_block->final_rate;
// printf("decel %d %d ",deceleration_time, timer );
}
timer=calculate_timer(step_rate);
if (step_events_completed == cur_block->decelerate_after)
{
acceleration_time = 0;
peak_rate = step_rate;
}
acceleration_time += timer;
step_events_completed++;
all_steps_completed = 1;
for (i=0;i<4;i++)
{
// if (i==1)
// printf(" %d %d > 0 && %d > %d\r\n", i, cur_block->steps[i], step_events_completed*(cur_block->steps[i])/(cur_block->step_event_count), cur_steps[i]);
if (cur_block->steps[i] > 0 && lround(step_events_completed*(cur_block->steps[i])/(cur_block->step_event_count)) > cur_steps[i])
{
//printf("step %d ", i);
motor_step(i,cur_block->dir[i]);
cur_steps[i]++;
// printf("cursteps %d\r\n", cur_steps[i]);
}
if (cur_steps[i] < (cur_block->steps[i]))
all_steps_completed=0;
}
// printf("step_event_completd: %d blk_event_count: %d\r\n", step_events_completed, cur_block->step_event_count);
if (step_events_completed < cur_block->step_event_count || !all_steps_completed )
{
ROM_TimerConfigure(TIMER0_BASE,TIMER_CFG_ONE_SHOT);
ROM_TimerLoadSet(TIMER0_BASE,TIMER_A, timer);
ROM_TimerEnable(TIMER0_BASE,TIMER_A);
} else
{
//ROM_GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3, 0x00);
cur_block->status=BLOCK_COMPLETED;
cur_block = NULL;
blk_queue_head++;
if (blk_queue_head >= BLOCK_QUEUE_SIZE)
blk_queue_head = 0;
num_blocks--;
step_events_completed =0;
for (i=0;i<4;i++)
cur_steps[i]=0;
//printf("%d block completed. Rescheduling timer for next block.\r\n",num_blocks);
ROM_TimerConfigure(TIMER0_BASE,TIMER_CFG_ONE_SHOT);
timer=calculate_timer(cur_block->final_rate);
ROM_TimerLoadSet(TIMER0_BASE,TIMER_A, timer);
ROM_TimerEnable(TIMER0_BASE, TIMER_A);
}
/*ROM_TimerConfigure(TIMER0_BASE,TIMER_CFG_ONE_SHOT);
timer=calculate_timer(cur_block->final_rate);
ROM_TimerLoadSet(TIMER0_BASE,TIMER_A, timer);
ROM_TimerEnable(TIMER0_BASE, TIMER_A);
*/
motor_unstep();
// printf("step_events: %d, step_rate: %d, timer: %d\r\n",step_events_completed, step_rate,timer);
ROM_IntMasterEnable();
}
