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;
}
