|
/** |
|
* @file stepper_example.c |
|
*/ |
|
|
|
#include <stdio.h> |
|
#include <robotcontrol.h> // includes ALL Robot Control subsystems |
|
|
|
void step(int direction); |
|
|
|
/** |
|
* This template contains these critical components |
|
* - ensure no existing instances are running and make new PID file |
|
* - start the signal handler |
|
* - initialize subsystems you wish to use |
|
* - while loop that checks for EXITING condition |
|
* - cleanup subsystems at the end |
|
* |
|
* @return 0 during normal operation, -1 on error |
|
*/ |
|
int main() |
|
{ |
|
int position = 0; |
|
int direction = 1; |
|
|
|
// make sure another instance isn't running |
|
// if return value is -3 then a background process is running with |
|
// higher privaledges and we couldn't kill it, in which case we should |
|
// not continue or there may be hardware conflicts. If it returned -4 |
|
// then there was an invalid argument that needs to be fixed. |
|
if(rc_kill_existing_process(2.0)<-2) return -1; |
|
|
|
// start signal handler so we can exit cleanly |
|
if(rc_enable_signal_handler()==-1){ |
|
fprintf(stderr,"ERROR: failed to start signal handler\n"); |
|
return -1; |
|
} |
|
|
|
// make PID file to indicate your project is running |
|
// due to the check made on the call to rc_kill_existing_process() above |
|
// we can be fairly confident there is no PID file already and we can |
|
// make our own safely. |
|
rc_make_pid_file(); |
|
|
|
// Keep looping until state changes to EXITING |
|
rc_set_state(RUNNING); |
|
rc_led_set(RC_LED_GREEN, 1); |
|
rc_led_set(RC_LED_RED, 0); |
|
while(rc_get_state()!=EXITING){ |
|
// do stepping here |
|
if(position >= 800){ |
|
direction = -1; |
|
} else if(position <= 0) { |
|
direction = 1; |
|
} |
|
step(direction); |
|
position += direction; |
|
|
|
// always sleep at some point |
|
rc_usleep(10000); |
|
} |
|
|
|
// turn off LEDs and close file descriptors |
|
rc_led_set(RC_LED_GREEN, 0); |
|
rc_led_set(RC_LED_RED, 0); |
|
rc_remove_pid_file(); // remove pid file LAST |
|
return 0; |
|
} |
|
|
|
#define M1 1 |
|
#define M2 4 |
|
|
|
void step(int direction) |
|
{ |
|
static int state = 0; |
|
static int enabled = 0; |
|
|
|
if(!enabled) { |
|
rc_motor_init(); |
|
enabled = 1; |
|
} |
|
|
|
if(direction > 0) { |
|
state++; |
|
} else if (direction < 0) { |
|
state--; |
|
} |
|
|
|
if(state > 4) state = 0; |
|
else if(state < 0) state = 3; |
|
|
|
switch(state) { |
|
case 0: |
|
default: |
|
rc_motor_set(M1,1); |
|
rc_motor_set(M2,1); |
|
break; |
|
case 1: |
|
rc_motor_set(M1,-1); |
|
rc_motor_set(M2,1); |
|
break; |
|
case 2: |
|
rc_motor_set(M1,-1); |
|
rc_motor_set(M2,-1); |
|
break; |
|
case 3: |
|
rc_motor_set(M1,1); |
|
rc_motor_set(M2,-1); |
|
break; |
|
} |
|
} |