Skip to content

Instantly share code, notes, and snippets.

@iamcsharper
Created March 26, 2021 15:54
Show Gist options
  • Save iamcsharper/489d3d4406a3da244aa44aab0acc4bcf to your computer and use it in GitHub Desktop.
Save iamcsharper/489d3d4406a3da244aa44aab0acc4bcf to your computer and use it in GitHub Desktop.
typedef struct {
uint16_t step_pin;
uint16_t dir_pin;
uint16_t enable_pin;
bool is_stopped;
} Stepper;
typedef enum {
Forward, Backward
} Direction;
Stepper yaw = {54, 55, 38};
Stepper arm = {60, 61, 56};
Stepper wrist = {46, 48, 62};
void setupStepper(Stepper* s) {
pinMode(s->step_pin, OUTPUT);
pinMode(s->dir_pin, OUTPUT);
pinMode(s->enable_pin, OUTPUT);
digitalWrite(s->enable_pin, LOW);
}
void driveStepper(Stepper* s, Direction dir, uint16_t _delay) {
s->is_stopped = false;
digitalWrite(s->dir_pin, dir == Forward ? HIGH : LOW);
digitalWrite(s->step_pin, HIGH);
delayMicroseconds(_delay);
digitalWrite(s->step_pin, LOW);
}
void stopStepper(Stepper* s) {
if (s->is_stopped)
return;
s->is_stopped = true;
digitalWrite(s->dir_pin, LOW);
digitalWrite(s->step_pin, LOW);
}
void setup() {
// put your setup code here, to run once:
setupStepper(&yaw);
setupStepper(&arm);
setupStepper(&wrist);
}
unsigned long last_time = 0;
uint16_t index = 0;
uint16_t change_state_delay = 1000;
void loop() {
unsigned long m = millis();
if (m - last_time > change_state_delay) {
index++;
last_time = m;
}
if (index > 5) {
index = 0;
}
switch (index) {
case 0:
// Drive yaw forward for 2.5 secs
stopStepper(&wrist);
stopStepper(&arm);
change_state_delay = 2500;
driveStepper(&yaw, Forward, 50);
break;
case 1:
// Drive yaw backward for 2.5 secs
stopStepper(&wrist);
stopStepper(&arm);
change_state_delay = 2500;
driveStepper(&yaw, Backward, 50);
break;
case 2:
// Raise arm up for 1 sec
stopStepper(&yaw);
stopStepper(&wrist);
change_state_delay = 1000;
driveStepper(&arm, Forward, 100);
break;
case 3:
// Put arm down for 1 sec
stopStepper(&yaw);
stopStepper(&wrist);
change_state_delay = 1000;
driveStepper(&arm, Backward, 100);
break;
case 4:
change_state_delay = 2000;
stopStepper(&yaw);
stopStepper(&arm);
driveStepper(&wrist, Forward, 50);
break;
case 5:
change_state_delay = 2000;
stopStepper(&yaw);
stopStepper(&arm);
driveStepper(&wrist, Backward, 50);
break;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment