Last active
June 22, 2017 23:18
-
-
Save murilopolese/3a799523fa635848ec8c663977238863 to your computer and use it in GitHub Desktop.
Fabulous machine arduino firmware
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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