Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Fabulous machine arduino firmware
#include <Servo.h>
Servo servo;
typedef struct motor
{
int pin1;
int pin2;
int pin3;
int pin4;
};
// Stepper motor properties
int motorSpeed = 3500;
int lookup[8] = {B01000, B01100, B00100, B00110, B00010, B00011, B00001, B01001};
// Motors
motor motor1 = {2, 3, 4, 5};
motor motor2 = {6, 7, 8, 9};
motor motors[2] = {motor1, motor2};
// Instruction parser
String inputString = ""; // a string to hold incoming data
boolean stringComplete = false; // whether the string is complete
char delimiter = ' '; // Command/params delimiter
// Set quartet to motor pins
void setOutput(motor m, int out)
{
digitalWrite(m.pin1, bitRead(lookup[out], 0));
digitalWrite(m.pin2, bitRead(lookup[out], 1));
digitalWrite(m.pin3, bitRead(lookup[out], 2));
digitalWrite(m.pin4, bitRead(lookup[out], 3));
}
// Perform a step on the motor forwards or backwards
void motor_spin(motor m, int dir)
{
if(dir > 0) {
for(int i = 0; i < 8; i++)
{
setOutput(m, i);
delayMicroseconds(motorSpeed);
}
}
if(dir < 0) {
for(int i = 7; i >= 0; i--)
{
setOutput(m, i);
delayMicroseconds(motorSpeed);
}
}
}
// Perform many steps on the motor forwards or backwards
void motor_long_spin(motor m, int dir, int steps) {
for(int i = 0; i < steps; i++)
{
motor_spin(m, dir);
}
}
// cw command expects 2 parameters:
// cw [steps] [motor number]
void cw(String params)
{
int delimiter_pos = params.indexOf(delimiter);
int n = params.substring(0, delimiter_pos).toInt();
int motor = params.substring(delimiter_pos).toInt();
motor_long_spin(motors[motor], 1, n);
}
// ccw command expects 2 parameters:
// ccw [steps] [motor number]
void ccw(String params)
{
int delimiter_pos = params.indexOf(delimiter);
int n = params.substring(0, delimiter_pos).toInt();
int motor = params.substring(delimiter_pos).toInt();
motor_long_spin(motors[motor], -1, n);
}
// p command expects one parameter:
// p [angle]
void p(String params)
{
int angle = params.toInt();
servo.write(angle);
delay(1000);
}
void up(String params)
{
int steps = params.toInt();
for(int i = 0; i < steps; i++)
{
motor_spin(motors[0], -1);
motor_spin(motors[1], 1);
}
}
void down(String params)
{
int steps = params.toInt();
for(int i = 0; i < steps; i++)
{
motor_spin(motors[0], 1);
motor_spin(motors[1], -1);
}
}
void left(String params)
{
int steps = params.toInt();
for(int i = 0; i < steps; i++)
{
motor_spin(motors[0], -1);
motor_spin(motors[1], -1);
}
}
void right(String params)
{
int steps = params.toInt();
for(int i = 0; i < steps; i++)
{
motor_spin(motors[0], 1);
motor_spin(motors[1], 1);
}
}
void parseInstructions(String str)
{
int delimiter_pos = str.indexOf(delimiter);
String command = str.substring(0, delimiter_pos); // command
String params = str.substring(delimiter_pos+1); // steps and which motor
if(command.equals("cw")) {
cw(params);
} else if (command.equals("ccw")) {
ccw(params);
} else if (command.equals("p")) {
p(params);
} else if (command.equals("up")) {
up(params);
} else if (command.equals("down")) {
down(params);
} else if (command.equals("left")) {
left(params);
} else if (command.equals("right")) {
right(params);
}
// Host should wait for this message to send a new one
Serial.print("> " + str);
}
void serialEvent() {
while (Serial.available()) {
char inChar = (char) Serial.read();
inputString += inChar;
if (inChar == '\n') {
stringComplete = true;
}
}
}
void setup()
{
Serial.begin(9600);
Serial.println("> command steps motorNumber");
pinMode(motor1.pin1, OUTPUT);
pinMode(motor1.pin2, OUTPUT);
pinMode(motor1.pin3, OUTPUT);
pinMode(motor1.pin4, OUTPUT);
pinMode(motor2.pin1, OUTPUT);
pinMode(motor2.pin2, OUTPUT);
pinMode(motor2.pin3, OUTPUT);
pinMode(motor2.pin4, OUTPUT);
servo.attach(10);
}
void loop()
{
if (stringComplete) {
parseInstructions(inputString);
inputString = "";
stringComplete = false;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment