static void find_start_position() { motor_current_active(); /* Move until we find the hole in the wheel. * The hole identifies a position between digits 9 and 0. */ while (lightsensor_is_blocked()) { motor_step(); _delay_ms(9); /* FIXME: will position incorrectly if stopped over marking at powerup. * maybe force it to get blocked first, then unblocked? */ } /* Move by configured offset, so we display number zero * (the mark on the wheel is between 9 and 0) */ for (int i = 0; i < conf_offset; i++) { motor_step(); _delay_ms(13); } /* Record our current position */ current_pos = conf_offset; target_pos = conf_offset; steps_until_done = 0; motor_current_idle(); _delay_ms(100); }
int main(void) { WDTCTL = WDTPW + WDTHOLD; // Stop WDT // configure the CPU clock (MCLK) // to run from SMCLK: DCO @ 16MHz and SMCLK = DCO // If no DCO calibration => stop here by loop if(CALBC1_16MHZ==0xff) while(1) _NOP(); _BIC_SR(GIE); DCOCTL = 0; BCSCTL1= CALBC1_16MHZ; DCOCTL = CALDCO_16MHZ; clock_init(); uart_init(); motor_init(); protocols_init(); pwm_init(); // ---------- // **action** // ---------- _BIS_SR(GIE); FOREVER { // read input input_scanner(); // handle any pending state changes in motor motor_step(); // send status reports to pi reporter(); } }
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(); }
int main (void) { debug_init(); config_read(); motor_init(); lightsensor_init(); find_start_position(); debug_fstr("done"); lcd_setcursor(2,2); debug_fstr("durr "); debug_hex8(conf_address); /* now listen on the bus and wait for data */ si2cs_init(); sei(); /* we're done with setup. activate watchdog */ wdt_enable(WDTO_4S); while (1) { /* moving to a new position */ motor_current_active(); uint16_t steps_since_start = 0; while (current_pos != target_pos) { /* do one step */ motor_step(); /* update current position. * overflow of the current position is handled by the * light sensor interrupt */ current_pos++; /* acceleration */ if (steps_since_start > 20 && steps_until_done > 20) { _delay_ms(3); } else if (steps_since_start > 10 && steps_until_done > 10) { _delay_ms(6); } else if (steps_since_start > 5 && steps_until_done > 5) { _delay_ms(9); } else { _delay_ms(10); } steps_since_start++; steps_until_done--; wdt_reset(); } /* holding the current position */ motor_current_idle(); while (current_pos == target_pos) { _delay_ms(20); wdt_reset(); } } }
void motor_set_time(unsigned int new_time) { unsigned int minutes = ((new_time / 100) * 60) + (new_time % 100); unsigned int clock_minutes = ((current_time / 100) * 60) + (current_time % 100); char shift_error = 0; // moving error, because we can't set 1 minute precisely unsigned int steps = 0; unsigned char reminder = 0; unsigned char dir = 0; if(minutes >= 12 * 60) // 12 hours clock format minutes -= 12 * 60; if(minutes >= 12 * 60 || (new_time % 100) >= 60 || (minutes == clock_minutes)) // minutes >= 60 the same time { motor_step( counterclockwise, 200 ); // display error _delay_ms(100); motor_step( clockwise, 200 ); return; // if error exit function } if(minutes > clock_minutes) // calculate how much steps motor needs to make and in what direction { steps = ((minutes - clock_minutes) / 3) * 160; //80 dir = clockwise; } else { steps = ((clock_minutes - minutes) / 3) * 160; dir = counterclockwise; } if(dir == clockwise) reminder = (minutes - clock_minutes + 3) % 3; else reminder = (clock_minutes - minutes + 3) % 3; if(reminder != 0) { if(reminder == 1) { if(shift_error <= 1 && shift_error >= -1 ) { if(dir == clockwise) { steps += 52; shift_error++; } else { steps += 52; shift_error--; } } else { if(dir == clockwise) steps += 56; else steps += 56; shift_error = 0; } } else if(reminder == 2) { if(shift_error == 0) { if(dir == clockwise) { steps += 52 * 2; shift_error += 2; } else { steps += 52 * 2; shift_error -= 2; } } else if(shift_error == 1) { if(dir == clockwise) { steps += 52 + 56; shift_error = 0; } else { steps += 52 + 56; shift_error = 0; } } else { if(dir == clockwise) { steps += 52 + 56; shift_error = 1; } else { steps += 56 + 52; shift_error = -1; } } } } motor_step(dir, steps); current_time = minutes / 60 * 100 + minutes % 60; current_minutes = ((current_time / 100) * 60) + (current_time % 100); win_minutes = ((win_time / 100) * 60) + (win_time % 100); if(((current_minutes + 20 >= win_minutes + 20 - 6) && (current_minutes + 20 <= win_minutes + 20 + 6)) || ((current_minutes >= win_minutes + 720 - 6) && (current_minutes <= win_minutes + 720 + 6)) || ((current_minutes + 720 >= win_minutes - 6) && (current_minutes + 720 <= win_minutes + 6)) ) { PORTC |= (1<<5); // open door } else { PORTC &= ~(1<<5); } }
int main (void) { uint8_t bu, bd; // buttons up, buttons down (read-in buffers) uint8_t i; // iterator uint8_t adcchan = 0; uint8_t ramp; uint16_t cycle; motor_t motor[NMOTORS]; for (i = 0; i < NMOTORS; i++) { motor[i].isrunning = false; motor[i].position = 0; motor[i].bu = false; motor[i].bd = false; motor[i].pot = 0; motor[i].trgspeed = SPEEDMIN; motor[i].curspeed = SPEEDMIN; motor[i].counter = 0; } led_init(); spi_init(); adc_init(); motors_init(); /* // debug */ /* uart_init(); */ /* stdout = &uart_output; */ adc_set_chan(adcchan); adc_start(); while (1) { led_off(); // keep the ADC running "in background" if ( !( adc_is_running() )) { motor[adcchan].pot = adc_get(); if (motor[adcchan].bu != motor[adcchan].bd) { motor[adcchan].trgspeed = motor_scale_speed(motor[adcchan].pot); } else { motor[adcchan].trgspeed = SPEEDMIN; } adcchan++; if (adcchan >= 5) adcchan = 0; // HACK: lame jumpers if (adcchan == 0) adc_set_chan(6); if (adcchan == 1) adc_set_chan(1); if (adcchan == 2) adc_set_chan(2); if (adcchan == 3) adc_set_chan(3); if (adcchan == 4) adc_set_chan(7); adc_start(); } // read in buttons shiftreg_load(); bu = spi_transmit(SPI_TRANSMIT_DUMMY); bd = spi_transmit(SPI_TRANSMIT_DUMMY); for (i = 0; i < NMOTORS; i++) { motor[i].bu = bit_is_set(bu, i) ? true : false; motor[i].bd = bit_is_set(bd, i) ? true : false; // both buttons pressed if (motor[i].bu && motor[i].bd) { led_on(); motor[i].trgspeed = SPEEDMIN; } if (motor[i].bu != motor[i].bd) { motor[i].isrunning = true; // set dir if (motor[i].bu) { motor_set_dir(i, DIR_UP); } else if (motor[i].bd) { motor_set_dir(i, DIR_DOWN); } } // allow ramp-down else if (motor[i].curspeed != motor[i].trgspeed) { motor[i].isrunning = true; } // mark as ok-to-stop else if (motor[i].curspeed == SPEEDMIN) { motor[i].isrunning = false; } } for (ramp = 0; ramp < RAMPCYCLES; ramp++) { // push current speed towards target for (i = 0; i < NMOTORS; i++) { if (motor[i].curspeed < motor[i].trgspeed) { motor[i].curspeed += 1; } if (motor[i].curspeed > motor[i].trgspeed) { motor[i].curspeed -= 1; } } // run at current speed for (cycle = 0; cycle < RUNCYCLES; cycle++) { for (i = 0; i < NMOTORS; i++) { // either button pressed or ramp-down if (motor[i].isrunning) { if (motor[i].counter > 0) { motor[i].counter -= 1; } else { // reset counter motor[i].counter = motor[i].curspeed; motor_step(i); } } } } } } return 0; }