void add_steps(int Steps){
long S;
U8 sign = 0;
// pause
TIM2_CR1 &= ~TIM_CR1_CEN;
if(Motor_number == 5){ // motors are stopped - just move last active motor
move_motor(Steps);
return;
}
// if(PORT(STP_DIR_PORT, IDR) & STP_DIR_PIN) // left direction
// Nsteps *= -1L;
if(Steps < 0){
sign = 1;
Steps *= -1;
}
S = (long)Steps << Ustepping;
if(sign)
S *= -1L;
Nsteps += S;
// now change direction
if(Nsteps < 0){
uart_write("reverce\n");
PORT(STP_DIR_PORT, ODR) ^= STP_DIR_PIN; // go to the opposite side
Nsteps *= -1L;
}
// resume if Nsteps != 0
if(Nsteps)
TIM2_CR1 |= TIM_CR1_CEN;
}
//-------------------------------------------------------------------------------
void t_stepper_motor_controller::go_home(t_potentiometers_controller *potentiometers_control, t_infrared_analog_sensors_controller *infrareds_control, t_buttons_controller* buttons_controller)
{
if (sensors_count > 0) {
int sensor_index = sensors[0].index;
byte sensor_type = sensors[0].type;
if (sensor_type == POTENTIOMETER) {
//calculate the remaining distance from the current position to home position, relative to the direction and position of the potentiometer
int pot_dir = sensors[0]._direction; // potentiometers_control->get_direction(sensor_index);
int pot_home = sensors[0].home_pos; // potentiometers_control->get_home_position(sensor_index);
int pot_pos = potentiometers_control->get_position(sensor_index);
int distance_to_home = pot_dir * (pot_home - pot_pos) * 10;
// Serial.println(pot_dir);
// Serial.println(pot_home);
// Serial.println(pot_pos);
// Serial.println(distance_to_home);
going_home = true;
move_motor(distance_to_home);
}
else if (sensor_type == INFRARED_ANALOG) {
//calculate the remaining distance from the current position to home position, relative to the direction and position of the potentiometer
//int i_dir = infrareds_control->get_direction(sensor_index);
int i_home = sensors[0].home_pos;
int i_pos = infrareds_control->get_signal_strength(sensor_index);
int distance_to_home;
if (i_home < i_pos)
distance_to_home = -32000;
else
distance_to_home = 32000;
going_home = true;
move_motor(distance_to_home);
}
else if (sensor_type == BUTTON) {
int b_direction = 0;//buttons_controller->get_direction(sensor_index);
int distance_to_home;
if (b_direction > 0)
distance_to_home = 200; // assume 1.8 degrees steps
else
distance_to_home = -200;
going_home = true;
move_motor(distance_to_home);
}
}
}
static INTERRUPT_GEN( stactics_interrupt )
{
stactics_state *state = (stactics_state *)device->machine->driver_data;
move_motor(device->machine, state);
cpu_set_input_line(device, 0, HOLD_LINE);
}
/************************************************
* axis_position function
* During milling time
***********************************************/
place axis_position(place positions)
{
place pos_prov;
pos_prov = check_position(positions); //check if we invalidate the limits
if(pos_prov.x_pos==0 && pos_prov.y_pos==0 && pos_prov.z_pos==0)
{
read_docs();//sd read
}
else if(pos_prov.x_pos!=0 || pos_prov.y_pos!=0 || pos_prov.z_pos!=0 ) //if does not
{
result=actual_to_previous(positions); //actualize the positions & return the difference
if(result.x_pos!=0) //if x axis has changed
{
prove.axes=driverx;
prove.step=mm_to_steps(result.x_pos);
if(result.x_pos>0) prove.direction=pos;
else prove.direction=neg;
move_motor(prove, 3000);
result.x_pos=0;
}
if(result.y_pos!=0)
{
prove.axes=drivery;
prove.step=mm_to_steps(result.y_pos);
if(result.y_pos>0) prove.direction=pos;
else prove.direction=neg;
move_motor(prove, 3000);
result.y_pos=0;
}
if(result.z_pos!=0)
{
prove.axes=driverz;
prove.step=mm_to_steps(result.z_pos);
if(result.z_pos>0) prove.direction=pos;
else prove.direction=neg;
move_motor(prove, 3000);
result.z_pos=0;
}
//aqui iniciar el timer GIE
}
return positions;
}
/**
* Move motors in direction dir to Nsteps
* (if Nsteps == 0 then move infinitely)
*/
void Xmove(int dir, unsigned int Nsteps){
#ifdef __arm__
steps = 0;
stopat = Nsteps;
move_motor(dir);
#else
printf("Move x axis to %u in dir %d\n", Nsteps, dir);
#endif // __arm__
}
void motor_on() {
TCCR1A = (1 << COM3A1) | (1 << COM3B1) | (1 << WGM31);
//TCCR3A = (1 << COM3A1) | (1 << COM3A0) | (1 << WGM31) | (1 << WGM30);
//TCCR3A = (1 << COM3A0); // orig
// COM3A0: Toggle PB6 on compare match between counter and OCR3A
TCCR1B = (1 << WGM33) | (1 << WGM32) | (1 << CS31) | (1 << CS30);
//TCCR3B = (1 << WGM32) | (1 << CS31) | (1 << CS30); // orig
// WGM32: When counter (TCNT3) matches OCR3A, reset counter
// CS31 & CS30: Set a prescaler of 64
move_motor(0,0);
}
/************************************************
* move_axis function
* During Jog Control display move the axis certain mm
***********************************************/
void move_axis(driver_control motors)
{
place check_pos,checked;
switch(motors.axes)
{
case(driverx):
if(motors.direction==pos) check_pos.x_pos = motors.step; //still mm not steps
else check_pos.x_pos = -motors.step;
check_pos.y_pos=0;
check_pos.z_pos=0;
break;
case(drivery):
if(motors.direction==pos)check_pos.y_pos =motors.step; //still mm not steps
else check_pos.y_pos = -motors.step;
check_pos.x_pos=0;
check_pos.z_pos=0;
break;
case(driverz):
if(motors.direction==pos)check_pos.z_pos =motors.step; //still mm not steps
else check_pos.z_pos = -motors.step;
check_pos.y_pos=0;
check_pos.x_pos=0;
break;
}
//sumar
checked = check_position_advance(check_pos);
if (checked.x_pos>=0 && checked.y_pos>=0 && checked.z_pos>=0 )
{
if((check_pos.x_pos>0) || (check_pos.y_pos>0) || (check_pos.z_pos<0) )
{motors.direction=pos;}
else if((check_pos.x_pos<0) || (check_pos.y_pos<0) || (check_pos.z_pos>0))
{motors.direction=neg;}
switch(motors.axes)
{
case(driverx):motors.step=mm_to_steps(check_pos.x_pos); // change to steps
break;
case(drivery):motors.step=mm_to_steps(check_pos.y_pos); // change to steps
break;
case(driverz):motors.step=mm_to_steps(check_pos.z_pos); // change to steps
break;
}
move_motor(motors, 1000);
}
}
/*****************************************************
* Home_axis_home function
* send a selected axis home
****************************************************/
void home_axis_home (driver axis)
{
//pick the right axis to go home
if (driverz==axis)
{
prove.direction = pos;
}
else
{
prove.direction = neg;
}
prove.axes = axis;
prove.step = -1; //send PWM for ever until end stop interrupt is triggered
move_motor(prove, 1000);
}
bool filter::Move_to(int next_node){
int motor_one_status, motor_two_status, motor_one_steps, motor_two_steps;
int dir = 1;
int steps_to=distance_to[current_node][next_node]; //
if(steps_to <=0) // checks the direction
{
dir = 0;
steps_to= -1*steps_to;
}
std::cout<<steps_to<<" " << distance_to[current_node][next_node]<< std::endl;
int time_interval=10; // time interval between ticks the motor 10 ms
int reply; //used to call methods from stepper_bee_interface
reply = init_controller(); // turns the board
reply = set_modes(1,1,1,1); // allows the board to report back to use
reply = move_motor(1, dir, steps_to, time_interval, 0, 0, 0); // moves motor stepper_bee_interface just change the first int from a 1 to a 0
int wait=0; //built in for the delay that will occur on the usb
do{
reply = get_motor_status(&motor_one_status, &motor_two_status, &motor_one_steps, &motor_two_steps) ;
std::cout<<"MOTOR - steps left = "<< motor_two_steps<< " \n"; //CTM: change to motor_one_steps if using other motor
j++;
}while(motor_one_status == 1 || motor_two_status == 1|| wait<5); //this how we wait for the motor
std::cout<<"DONE !! \n";
current_node=next_node; // so the class knows where it is at
reply=close_controller(); //prevent bad signals being sent to the motor
return true;
}
void step_cycle()
{
if (step == 0)
step = 1;
if (start_pos < end_pos && step < 0)
step *= -1;
else if (start_pos > end_pos && step > 0)
step *= -1;
for (double pos = start_pos; pos < end_pos; pos += step)
{
double actual;
if (!move_motor(pos, &actual))
{
printf("error moving to %f\n", pos);
break;
}
printf("commanded %f actual %f\n", pos, actual);
}
}
int main() {
unsigned long T = 0L;
int Ival;
U8 rb, Num;
CFG_GCR |= 1; // disable SWIM
// Configure clocking
CLK_CKDIVR = 0; // F_HSI = 16MHz, f_CPU = 16MHz
// Timer 4 (8 bit) used as system tick timer
// prescaler == 128 (2^7), Tfreq = 125kHz
// period = 1ms, so ARR = 125
TIM4_PSCR = 7;
TIM4_ARR = 125;
// interrupts: update
TIM4_IER = TIM_IER_UIE;
// auto-reload + interrupt on overflow + enable
TIM4_CR1 = TIM_CR1_APRE | TIM_CR1_URS | TIM_CR1_CEN;
// Configure pins
// PC2 - PP output (on-board LED)
PORT(LED_PORT, DDR) |= LED_PIN;
PORT(LED_PORT, CR1) |= LED_PIN;
// PD5 - UART2_TX -- pseudo open-drain output; don't forget an pullup resistor!
PORT(UART_PORT, DDR) |= UART_TX_PIN;
PORT(UART_PORT, ODR) |= UART_TX_PIN; // torn off N push-down ???
//PORT(UART_PORT, CR1) |= UART_TX_PIN;
// Configure UART
// 9 bit, no parity, 1 stop (UART_CR3 = 0 - reset value)
// 57600 on 16MHz: BRR1=0x11, BRR2=0x06
UART2_BRR1 = 0x11; UART2_BRR2 = 0x06;
UART2_CR1 = UART_CR1_M; // M = 1 -- 9bits
UART2_CR2 = UART_CR2_REN | UART_CR2_RIEN; // Allow RX, generate ints on rx
setup_stepper_pins();
// enable all interrupts
enableInterrupts();
// Loop
do{
if((Global_time - T > paused_val) || (T > Global_time)){
T = Global_time;
PORT(LED_PORT, ODR) ^= LED_PIN; // blink on-board LED
}
if(UART_read_byte(&rb)){ // buffer isn't empty
switch(rb){
case 'h': // help
case 'H':
uart_write("\nPROTO:\n"
"+/-\tLED period\n"
"Ex/ex\tset/get end-switches stored\n"
"p\tget HW end-switches\n"
"Mx\tstop on end-switch\n"
"Sx/sx\tset/get Mspeed\n"
"mx\tget steps\n"
"Px\tpause/resume\n"
"Xx\tstop\n"
"0..2N\tmove xth motor for N steps\n"
"=\tinfinity moving (after 0..2)"
"U/u\tset/get U-stepping\n"
"I\tget serial ID\n"
"N\tchange HW number\n"
"n\tshow HW number\n"
);
break;
case 'I': // get serial id
show_uid();
break;
case '+':
paused_val += 100;
if(paused_val > 10000)
paused_val = 500; // but not more than 10s
break;
case '-':
paused_val -= 100;
if(paused_val < 100) // but not less than 0.1s
paused_val = 500;
break;
case 'E': // set end-switches value
if(get_motor_number(&Num)){
if(readInt(&Ival) && (Ival == (Ival & 0x1f))){
if(Num)
EPs[Num] = Ival & 0x0f; // 4 bits in motors 1&2
else
EPs[0] = Ival; // all 5 bits in motor 0
}else
error_msg("bad EP");
}
break;
case 'e': // get stored end-switches value
if(get_motor_number(&Num)){
printUint(&EPs[Num], 1);
}
break;
case 'p': // get hardware end-switches value
if(get_motor_number(&Num)){
Num = get_ep_value(Num);
printUint(&Num, 1);
}
break;
case 'S': // set stepper speed
if(get_motor_number(&Num)){
if(readInt(&Ival) && Ival > MIN_STEP_LENGTH)
set_stepper_speed(Num, Ival);
else
error_msg("bad speed");
}
break;
case 's': // get stepper speed
if(get_motor_number(&Num))
printUint((U8*)&Stepper_speed[Num], 2);
break;
case 'M': // move till EP, you can call it before starting motor
if(get_motor_number(&Num))
Stop_on_EP[Num] = 1;
break;
case 'm': // how much steps there is to the end of moving
if(get_motor_number(&Num))
printUint((U8*)&Nsteps[Num], 2);
break;
case 'X': // stop
if(get_motor_number(&Num))
stop_motor(Num);
break;
case 'P': // pause/resume
if(get_motor_number(&Num))
pause_resume(Num);
break;
case 'N':
if(readInt(&Ival) && Ival > 0 && Ival < 256)
if(!change_progmem_value(&UART_devNUM, (unsigned int) Ival))
error_msg("can't change val");
break;
case 'n': // show HW num
printUint(&UART_devNUM, 1);
break;
case 'u': // show UStepping
printUint(&USteps, 1);
break;
case 'U': // set UStepping
if(readInt(&Ival) && Ival > 0 && Ival < 256)
USteps = Ival;
break;
case '=': // infinity moving: just don't decrement steps
StepperInfty = 1;
break;
default:
if(rb >= '0' && rb <= '2'){ // run motor
Num = rb - '0';
if(readInt(&Ival) && Ival)
move_motor(Num, Ival);
else{
error_msg("bad Nsteps");
}
}
}
}
}while(1);
}
/**
* Main thread for steppers management
*/
void *steppers_thread(_U_ void *buf){
DBG("steppers_thr");
#ifdef __arm__
double laststeptime, curtime;
halfsteptime = 1. / (stepspersec * 8.);
DBG("halfsteptime: %g", halfsteptime);
laststeptime = dtime();
int eswsteps = 0;
while(!force_exit){
usleep(10); // garanteed pause == 10us
if(glob_dir){
//DBG("cur-last=%g, half: %g", dtime() - laststeptime, halfsteptime);
if((curtime = dtime()) - laststeptime > halfsteptime){
//DBG("1/8step");
Write(MOTOR_PIN1, steps_half[steppart][0]);
Write(MOTOR_PIN2, steps_half[steppart][1]);
Write(MOTOR_PIN3, steps_half[steppart][2]);
Write(MOTOR_PIN4, steps_half[steppart][3]);
/*
Write(MOTOR_PIN1, steps_full[steppart][0]);
Write(MOTOR_PIN2, steps_full[steppart][1]);
Write(MOTOR_PIN3, steps_full[steppart][2]);
Write(MOTOR_PIN4, steps_full[steppart][3]);
*/
//DBG("steppart: %d", steppart);
laststeptime = curtime;
int fullstep = 0;
if(glob_dir > 0){
if(++steppart == 8){
//if(++steppart == 4){
steppart = 0;
fullstep = 1;
}
}else{
if(--steppart < 0){
steppart = 7;
//steppart = 3;
fullstep = 1;
}
}
if(fullstep){ // full step processed
++steps;
if(steps % 10 == 0)
GLOB_MESG("nsteps=%d", steps);
// check end-switches, if motor reach esw, stop it
if((digitalRead(ESW1_PIN) == 0 && glob_dir == -1) ||
(digitalRead(ESW2_PIN) == 0 && glob_dir == 1)){
if(++eswsteps == 5){
int Nsw = get_endsw();
if(Nsw){
DBG("Reach end-switch %d @%u steps", Nsw, steps);
GLOB_MESG("esw=%d", Nsw);
move_motor(0);
GLOB_MESG("end-switch %d reached at %u steps", Nsw, steps);
}else eswsteps = 0;
}
}else eswsteps = 0;
//DBG("step");
if(stopat && stopat == steps){ // finite move for stopat steps
DBG("Reach position of %d steps", steps);
GLOB_MESG("position %d steps reached", steps);
move_motor(0);
}
}
}
}else{
steppart = 0;
}
}
#else // __arm__
printf("Main steppers' thread\n");
while(1) usleep(1000);
#endif // __arm__
DBG("return motors_thr");
return NULL;
}
int main() {
unsigned long T = 0L;
int Ival;
U8 rb;
CFG_GCR |= 1; // disable SWIM
// Configure clocking
CLK_CKDIVR = 0; // F_HSI = 16MHz, f_CPU = 16MHz
// Configure timer 1 - systick
// prescaler = f_{in}/f_{tim1} - 1
// set Timer1 to 1MHz: 1/1 - 1 = 15
TIM1_PSCRH = 0;
TIM1_PSCRL = 15; // LSB should be written last as it updates prescaler
// auto-reload each 1ms: TIM_ARR = 1000 = 0x03E8
TIM1_ARRH = 0x03;
TIM1_ARRL = 0xE8;
// interrupts: update
TIM1_IER = TIM_IER_UIE;
// auto-reload + interrupt on overflow + enable
TIM1_CR1 = TIM_CR1_APRE | TIM_CR1_URS | TIM_CR1_CEN;
// Configure pins
// PC2 - PP output (on-board LED)
PORT(LED_PORT, DDR) |= LED_PIN;
PORT(LED_PORT, CR1) |= LED_PIN;
// PD5 - UART2_TX
PORT(UART_PORT, DDR) |= UART_TX_PIN;
PORT(UART_PORT, CR1) |= UART_TX_PIN;
// Configure UART
// 8 bit, no parity, 1 stop (UART_CR1/3 = 0 - reset value)
// 57600 on 16MHz: BRR1=0x11, BRR2=0x06
UART2_BRR1 = 0x11; UART2_BRR2 = 0x06;
UART2_CR2 = UART_CR2_TEN | UART_CR2_REN | UART_CR2_RIEN; // Allow RX/TX, generate ints on rx
// enable all interrupts
enableInterrupts();
set_stepper_speed(1000);
setup_stepper_pins();
// Loop
do{
if((Global_time - T > paused_val) || (T > Global_time)){
T = Global_time;
PORT(LED_PORT, ODR) ^= LED_PIN; // blink on-board LED
}
if(UART_read_byte(&rb)){ // buffer isn't empty
switch(rb){
case 'h': // help
case 'H':
uart_write("\nPROTO:\n+/-\tLED period\nS/s\tset/get Mspeed\n"
"m\tget steps\nx\tstop\np\tpause/resume\nM\tmove motor\na\tadd Nstps\n"
"u\tunipolar motor\nb\tbipolar motor\n");
break;
case '+':
paused_val += 100;
if(paused_val > 10000)
paused_val = 500; // but not more than 10s
break;
case '-':
paused_val -= 100;
if(paused_val < 100) // but not less than 0.1s
paused_val = 500;
break;
case 'S': // set stepper speed
if(readInt(&Ival) && Ival > MIN_STEP_LENGTH)
set_stepper_speed(Ival);
else
error_msg("bad speed");
break;
case 's': // get stepper speed
printUint((U8*)&Stepper_speed, 2);
break;
case 'm': // how much steps there is to the end of moving
printUint((U8*)&Nsteps, 4);
break;
case 'M': // move motor
if(Nsteps){
error_msg("moving!");
break;
}
if(readInt(&Ival) && Ival)
move_motor(Ival);
else{
error_msg("bad Nsteps");
}
break;
case 'x': // stop
stop_motor();
break;
case 'p': // pause/resume
pause_resume();
break;
case 'a': // add N steps
if(readInt(&Ival) && Ival){
add_steps(Ival);
}else{
error_msg("bad value");
}
break;
case 'u': // unipolar
usteps = ustepsUNI;
break;
case 'b': // bipolar
usteps = ustepsBIP;
break;
}
}
}while(1);
}
void main (int argc, char ** argv)
{
int loop_done = 0;
int task_A_done = 0;
int task_B_done = 0;
int angle = 0;
int dangle = 0;
int angle0 = 0;
int distance0 = 0;
int distance = 0 ;
int ddistance = 0;
// float lf_args[20];
// read inputs
// printf("Args: %d\n", argc);
// for (i = 2; i < argc; i++)
// {
// scanf("%lf%, argv[i]
// }
//get_create_total_ angle (.1);
//create_drive_direct (left speed, right speed)\
//move_to_position (motor #, motor power(-for backwards), end position)
//get_motor_done (motor#)
//clear_motor_position_counter (motor#)
//msleep (seconds)
//i'll probably use this often
//to start by light, do:
//while(analog(1) > 200)
if (0)
{
//we have seen the light
msleep(100);
if(analog(1) < 800){
shut_down_in(119);
}
}
create_connect ();
clear_motor_position_counter (0);
enable_servo (0);
set_servo_position (0 ,1300);
//set_servo_position (1800);
// ------------------------------------------------------------------------
//step:1 aim the arm to knock over the box
//raise arm
printf("-----Step 1-----\n");
move_motor (0, -900, -1200);
// swing arm to hit the box, then get back to position
spin (50, -50, 20);
spin (-50, 50, 20);
create_stop ();
// ------------------------------------------------------------------------
//step:2 knock over box and turn
//lift arm
//turn 90
printf("-----Step 2-----\n");
move_to_position (0, -900, -2600);
msleep (900);
//turn create in a circular position and raise arm so it doesn't hit anything
angle0 = get_create_total_angle (.05);
angle = angle0;
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
create_drive_direct (-178,-648);
while(! loop_done)
{
if (dangle >= 155)
{
task_A_done = 1;
create_stop ();
}
else if (dangle >= 125)
{
create_drive_direct (-120, -181);
}
if (get_motor_done (0))
{
task_B_done = 1;
}
if (task_A_done && task_B_done)
{
loop_done = 1;
}
msleep(100);
angle = get_create_total_angle (.05);
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
}
create_stop ();
// ------------------------------------------------------------------------
//step:4 aim claw to point of botguy
//turn 20
//lower arm
printf("-----Step 4-----\n");
//--tweaking position so it gets in just the right position
move_to_position (0, 400, -900);
move (20, 20, 50);
set_servo_position (0 ,400);
create_drive_straight (-30);
move_motor (0, 600, -300);
create_drive_straight (-38);// at this point, botguy is in his palm
//can't remember function to program
msleep (3000);
loop_done = 0;
if (! loop_done)
{
create_stop ();
if (! digital (10))
{
move_motor (0, 400, -900);
spin (20, -20, 5);
move_motor (0, 600, -300);
create_stop ();
loop_done = 1;
}
else
{
loop_done = 1;
}
}
// ------------------------------------------------------------------------
//step:5 grab botguy and lift him up (not complete)
printf("-----Step 5-----\n");
move (-10, -10, 10);
msleep (1300);
//spin (60, -60, 5);
//close arm and grab botguy
set_servo_position (0, 1510);
msleep (400);
move_to_position (0, -1000, -3000);
//wait for him to finish
msleep (5000);
// ------------------------------------------------------------------------
//step:6 move backwards till it's in position
printf("-----Step 6-----\n");
//back up in an arc
move (130, 280, 780);
//move towards the center and lower drop height
move_to_position (0, -900, -500);
create_drive_straight (-20);
msleep (3000);
//drop botguy in transport
set_servo_position (0, 200);
create_stop ();
//start moving towards the pipe
move_motor (0, 500, -1200);
spin (50, -50, 70);
move (50, 50, 100);
spin (-50, 50, 70);
/*
if (! loop_done)
{
if (get_object_count(2) == 1)
{
create_stop ();
spin(-10, 10, 5);
create_stop ();
move(20, 20, 20);
create_stop ();
set_servo_position (0, 1400);
loop_done = 1;
}
}
loop_done = 0;
spin (10, 10, 5);
set_servo_position (0, 200);
*/
/*
spin (50, -50, 85);
move_motor (0, 400, -400);
move (-100, -100, 50);
loop_done = 0;
//close arm on pipe
set_servo_position (0, 1510);
//turn arm to face the pole
spin (-50, 50, 50);
//up, forward, then drop pipe
move_motor (0, 400, -2300);
move (-100, -100, 100);
set_servo_position (0, 200);
ao ();
*/
create_disconnect ();
}
void run_timelapse()
{
static unsigned long last = millis();
unsigned long currtime = millis();
unsigned long diff = currtime-last;
char shootAndMove = 0;
if(diff>=time_between_two_step)
{
last=currtime - (diff-time_between_two_step);
shootAndMove=1;
}
if(timelapse_task==TIMELAPSE_STARTED)
{
db.isStarted=1;
switch(timelapse_taskhelper)
{
case 0:
if(isMotorIdle())
{
if(conf.fromLeftToRight)
{
move_motor(0xffffffff, 0);
}
else
{
move_motor(0xffffffff, 1);
}
timelapse_taskhelper++;
}
break;
case 1:
if(isMotorIdle())
{
if(shootAndMove)
{
shoot();
if(conf.fromLeftToRight)
{
move_motor(steps_per_photo, 1);
}
else
{
move_motor(steps_per_photo, 0);
}
photo_count--;
if(photo_count==0)
{
timelapse_task = TIMELAPSE_IDLE;
}
}
}
break;
}
}
else
{
db.isStarted=0;
}
}
