mirror of
https://github.com/usatiuk/EggbotWireless.git
synced 2025-10-27 01:07:49 +01:00
use atmega32 to control stepper motors
Signed-off-by: Stepan Usatyuk <usaatyuk@ustk.me>
This commit is contained in:
40
Firmware/MotorControl/src/Pen.cpp
Normal file
40
Firmware/MotorControl/src/Pen.cpp
Normal file
@@ -0,0 +1,40 @@
|
||||
#include <Arduino.h>
|
||||
#include <Servo.h>
|
||||
|
||||
#include "Pen.h"
|
||||
|
||||
#define DELAY 15
|
||||
|
||||
Pen::Pen(int pin, int posEngaged, int posDisengaged)
|
||||
: posEngaged(posEngaged), posDisengaged(posDisengaged) {
|
||||
servo.attach(pin);
|
||||
}
|
||||
|
||||
void Pen::engage() {
|
||||
if (!engaged) {
|
||||
for (int i = posDisengaged; i > posEngaged; i--) {
|
||||
servo.write(i);
|
||||
delay(DELAY);
|
||||
}
|
||||
}
|
||||
engaged = true;
|
||||
}
|
||||
|
||||
void Pen::disengage() {
|
||||
if (engaged) {
|
||||
for (int i = posEngaged; i < posDisengaged; i++) {
|
||||
servo.write(i);
|
||||
delay(DELAY);
|
||||
}
|
||||
}
|
||||
engaged = false;
|
||||
}
|
||||
|
||||
void Pen::init() {
|
||||
servo.write(posDisengaged);
|
||||
engaged = false;
|
||||
}
|
||||
|
||||
bool Pen::getEngaged() { return engaged; }
|
||||
|
||||
Pen::~Pen() {}
|
||||
208
Firmware/MotorControl/src/Stepper.cpp
Normal file
208
Firmware/MotorControl/src/Stepper.cpp
Normal file
@@ -0,0 +1,208 @@
|
||||
#include "Stepper.h"
|
||||
#include <Arduino.h>
|
||||
|
||||
void Stepper::clockwise() {
|
||||
switch (curStep) {
|
||||
case 1:
|
||||
digitalWrite(pin4, HIGH);
|
||||
digitalWrite(pin3, LOW);
|
||||
digitalWrite(pin2, LOW);
|
||||
digitalWrite(pin1, LOW);
|
||||
break;
|
||||
case 2:
|
||||
digitalWrite(pin4, HIGH);
|
||||
digitalWrite(pin3, HIGH);
|
||||
digitalWrite(pin2, LOW);
|
||||
digitalWrite(pin1, LOW);
|
||||
break;
|
||||
case 3:
|
||||
digitalWrite(pin4, LOW);
|
||||
digitalWrite(pin3, HIGH);
|
||||
digitalWrite(pin2, LOW);
|
||||
digitalWrite(pin1, LOW);
|
||||
break;
|
||||
case 4:
|
||||
digitalWrite(pin4, LOW);
|
||||
digitalWrite(pin3, HIGH);
|
||||
digitalWrite(pin2, HIGH);
|
||||
digitalWrite(pin1, LOW);
|
||||
break;
|
||||
case 5:
|
||||
digitalWrite(pin4, LOW);
|
||||
digitalWrite(pin3, LOW);
|
||||
digitalWrite(pin2, HIGH);
|
||||
digitalWrite(pin1, LOW);
|
||||
break;
|
||||
case 6:
|
||||
digitalWrite(pin4, LOW);
|
||||
digitalWrite(pin3, LOW);
|
||||
digitalWrite(pin2, HIGH);
|
||||
digitalWrite(pin1, HIGH);
|
||||
break;
|
||||
case 7:
|
||||
digitalWrite(pin4, LOW);
|
||||
digitalWrite(pin3, LOW);
|
||||
digitalWrite(pin2, LOW);
|
||||
digitalWrite(pin1, HIGH);
|
||||
break;
|
||||
case 8:
|
||||
digitalWrite(pin4, HIGH);
|
||||
digitalWrite(pin3, LOW);
|
||||
digitalWrite(pin2, LOW);
|
||||
digitalWrite(pin1, HIGH);
|
||||
}
|
||||
if (curStep == 8) {
|
||||
curStep = 1;
|
||||
} else {
|
||||
curStep++;
|
||||
}
|
||||
}
|
||||
|
||||
void Stepper::counterClockwise() {
|
||||
switch (curStep) {
|
||||
case 1:
|
||||
digitalWrite(pin1, HIGH);
|
||||
digitalWrite(pin2, LOW);
|
||||
digitalWrite(pin3, LOW);
|
||||
digitalWrite(pin4, LOW);
|
||||
break;
|
||||
case 2:
|
||||
digitalWrite(pin1, HIGH);
|
||||
digitalWrite(pin2, HIGH);
|
||||
digitalWrite(pin3, LOW);
|
||||
digitalWrite(pin4, LOW);
|
||||
break;
|
||||
case 3:
|
||||
digitalWrite(pin1, LOW);
|
||||
digitalWrite(pin2, HIGH);
|
||||
digitalWrite(pin3, LOW);
|
||||
digitalWrite(pin4, LOW);
|
||||
break;
|
||||
case 4:
|
||||
digitalWrite(pin1, LOW);
|
||||
digitalWrite(pin2, HIGH);
|
||||
digitalWrite(pin3, HIGH);
|
||||
digitalWrite(pin4, LOW);
|
||||
break;
|
||||
case 5:
|
||||
digitalWrite(pin1, LOW);
|
||||
digitalWrite(pin2, LOW);
|
||||
digitalWrite(pin3, HIGH);
|
||||
digitalWrite(pin4, LOW);
|
||||
break;
|
||||
case 6:
|
||||
digitalWrite(pin1, LOW);
|
||||
digitalWrite(pin2, LOW);
|
||||
digitalWrite(pin3, HIGH);
|
||||
digitalWrite(pin4, HIGH);
|
||||
break;
|
||||
case 7:
|
||||
digitalWrite(pin1, LOW);
|
||||
digitalWrite(pin2, LOW);
|
||||
digitalWrite(pin3, LOW);
|
||||
digitalWrite(pin4, HIGH);
|
||||
break;
|
||||
case 8:
|
||||
digitalWrite(pin1, HIGH);
|
||||
digitalWrite(pin2, LOW);
|
||||
digitalWrite(pin3, LOW);
|
||||
digitalWrite(pin4, HIGH);
|
||||
}
|
||||
if (curStep == 8) {
|
||||
curStep = 1;
|
||||
} else {
|
||||
curStep++;
|
||||
}
|
||||
}
|
||||
|
||||
Stepper::Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution,
|
||||
int limit, float degreesPerMM)
|
||||
: pin1(pin1),
|
||||
pin2(pin2),
|
||||
pin3(pin3),
|
||||
pin4(pin4),
|
||||
stepsPerRevolution(stepsPerRevolution),
|
||||
limit(limit),
|
||||
degreesPerMM(degreesPerMM) {
|
||||
pinMode(pin1, OUTPUT);
|
||||
pinMode(pin2, OUTPUT);
|
||||
pinMode(pin3, OUTPUT);
|
||||
pinMode(pin4, OUTPUT);
|
||||
}
|
||||
|
||||
void Stepper::doStep() {
|
||||
if (remainingSteps > 0) {
|
||||
if (direction) {
|
||||
clockwise();
|
||||
} else {
|
||||
counterClockwise();
|
||||
}
|
||||
remainingSteps--;
|
||||
}
|
||||
}
|
||||
|
||||
void Stepper::step(int steps) {
|
||||
if (steps == 0) {
|
||||
return;
|
||||
}
|
||||
if (steps > 0) {
|
||||
direction = true;
|
||||
remainingSteps += steps;
|
||||
} else {
|
||||
direction = false;
|
||||
remainingSteps += abs(steps);
|
||||
}
|
||||
}
|
||||
|
||||
int Stepper::degreesToSteps(float degrees) {
|
||||
return (degrees * stepsPerRevolution) / 360;
|
||||
}
|
||||
|
||||
void Stepper::rotate(float degrees) {
|
||||
int steps = degreesToSteps(degrees);
|
||||
if (!limit) {
|
||||
pos = fmod((pos + degrees), 360);
|
||||
if (pos < 0) {
|
||||
pos = 360 + pos;
|
||||
}
|
||||
step(steps);
|
||||
} else {
|
||||
if (degrees + pos > limit) {
|
||||
step(degreesToSteps(limit - pos));
|
||||
pos = limit;
|
||||
} else if (degrees + pos < 0) {
|
||||
step(degreesToSteps(-pos));
|
||||
pos = 0;
|
||||
} else {
|
||||
step(steps);
|
||||
pos += degrees;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float Stepper::getDist(float degrees) {
|
||||
float mod = fmod(degrees, 360);
|
||||
return abs(mod - pos);
|
||||
}
|
||||
|
||||
void Stepper::rotateTo(float degrees) {
|
||||
float mod = fmod(degrees, 360);
|
||||
rotate(mod - pos);
|
||||
}
|
||||
|
||||
void Stepper::moveTo(float dist) { rotateTo(dist * degreesPerMM); }
|
||||
|
||||
int Stepper::mmToSteps(float dist) {
|
||||
return abs(degreesToSteps(dist * degreesPerMM));
|
||||
}
|
||||
|
||||
float Stepper::getPosMm() { return pos / degreesPerMM; }
|
||||
|
||||
float Stepper::getDistMm(float pos) { return abs(getPosMm() - pos); }
|
||||
|
||||
void Stepper::setPos(float degrees) { pos = degrees; }
|
||||
|
||||
int Stepper::getRemainingSteps() { return remainingSteps; }
|
||||
|
||||
bool Stepper::finished() { return remainingSteps == 0; }
|
||||
float Stepper::getPos() { return pos; }
|
||||
1
Firmware/MotorControl/src/commonSrc
Symbolic link
1
Firmware/MotorControl/src/commonSrc
Symbolic link
@@ -0,0 +1 @@
|
||||
../../commonSrc
|
||||
153
Firmware/MotorControl/src/main.cpp
Normal file
153
Firmware/MotorControl/src/main.cpp
Normal file
@@ -0,0 +1,153 @@
|
||||
#include <Arduino.h>
|
||||
#include <Servo.h>
|
||||
#include <Wire.h>
|
||||
#include "Globals.h"
|
||||
#include "common/Commands.h"
|
||||
|
||||
int curRPM = DEF_RPM;
|
||||
int adjustDelay = 10;
|
||||
void adjustRPM() {
|
||||
unsigned int stepsX = servoStepper.getRemainingSteps();
|
||||
unsigned int stepsY = eggStepper.getRemainingSteps();
|
||||
if (stepsX != 0 && stepsY != 0) {
|
||||
if (stepsX > stepsY) {
|
||||
float rpm = (float)curRPM * (float)stepsY / (float)stepsX;
|
||||
eggStepperRPM = rpm;
|
||||
} else if (stepsY > stepsX) {
|
||||
float rpm = (float)curRPM * (float)stepsX / (float)stepsY;
|
||||
servoStepperRPM = rpm;
|
||||
} else {
|
||||
eggStepperRPM = curRPM;
|
||||
servoStepperRPM = curRPM;
|
||||
}
|
||||
recalculateDelays = true;
|
||||
}
|
||||
}
|
||||
|
||||
int curFloat = 0;
|
||||
float command[4];
|
||||
|
||||
bool newCommand = false;
|
||||
bool executing = false;
|
||||
|
||||
int curByte = 0;
|
||||
byte rxBuffer[4];
|
||||
void receiveEvent(int howMany) {
|
||||
while (Wire.available() > 0) {
|
||||
if (!newCommand) {
|
||||
char c = Wire.read();
|
||||
rxBuffer[curByte] = c;
|
||||
curByte++;
|
||||
if (curByte == 4) {
|
||||
curByte = 0;
|
||||
bytesToFloat(&command[curFloat], rxBuffer);
|
||||
curFloat++;
|
||||
}
|
||||
if (curFloat == 4) {
|
||||
curFloat = 0;
|
||||
newCommand = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
byte txBuffer[5 * sizeof(float)];
|
||||
void requestEvent() {
|
||||
if (command[0] == M99) {
|
||||
floatToBytes(&txBuffer[0], servoStepper.getPos());
|
||||
floatToBytes(&txBuffer[sizeof(float)], eggStepper.getPos());
|
||||
|
||||
floatToBytes(&txBuffer[sizeof(float) * 2], servoStepper.getPosMm());
|
||||
floatToBytes(&txBuffer[sizeof(float) * 3], eggStepper.getPosMm());
|
||||
|
||||
floatToBytes(&txBuffer[sizeof(float) * 4], (float)pen.getEngaged());
|
||||
Wire.write(txBuffer, 5 * sizeof(float));
|
||||
} else if (executing || newCommand) {
|
||||
Wire.write(WAIT);
|
||||
} else {
|
||||
Wire.write(NEXT);
|
||||
}
|
||||
}
|
||||
|
||||
float calculateDelay(float rpm, int stepsPerRevolution) {
|
||||
return ((float)1000 * (float)60) / (rpm * (float)stepsPerRevolution);
|
||||
}
|
||||
|
||||
Servo servo;
|
||||
|
||||
void execCommand(float *command) {
|
||||
if (command[0] == G01 || command[0] == G00) {
|
||||
if (command[0] == G01) {
|
||||
needAdjust = true;
|
||||
} else {
|
||||
needAdjust = false;
|
||||
}
|
||||
|
||||
if (command[X] != -1) {
|
||||
servoStepper.moveTo(command[X]);
|
||||
}
|
||||
|
||||
if (command[Y] != -1) {
|
||||
eggStepper.moveTo(command[Y]);
|
||||
}
|
||||
|
||||
if (command[Z] < 0) {
|
||||
pen.engage();
|
||||
}
|
||||
if (command[Z] >= 0) {
|
||||
pen.disengage();
|
||||
}
|
||||
|
||||
adjustRPM();
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void setup() {
|
||||
cli();
|
||||
Serial.begin(115200);
|
||||
Wire.begin(8);
|
||||
Wire.onReceive(receiveEvent);
|
||||
Wire.onRequest(requestEvent);
|
||||
Serial.println("Hello!");
|
||||
recalculateDelays = true;
|
||||
eggStepperRPM = servoStepperRPM = 4;
|
||||
OCR2 = 250;
|
||||
TCCR2 |= (1 << WGM12) | (1 << CS22);
|
||||
TIMSK |= (1 << OCIE2);
|
||||
sei();
|
||||
servoStepper.setPos(X_LIMIT);
|
||||
pen.init();
|
||||
}
|
||||
|
||||
ISR(TIMER2_COMP_vect) {
|
||||
unsigned long ms = millis();
|
||||
if (fmod(ms, eggStepperDelay) < 1) {
|
||||
eggStepper.doStep();
|
||||
}
|
||||
if (fmod(ms, servoStepperDelay) < 1) {
|
||||
servoStepper.doStep();
|
||||
}
|
||||
if (fmod(ms, adjustDelay) < 1) {
|
||||
adjustRPM();
|
||||
}
|
||||
}
|
||||
|
||||
void loop() {
|
||||
if (eggStepper.getRemainingSteps() == 0 &&
|
||||
servoStepper.getRemainingSteps() == 0) {
|
||||
executing = false;
|
||||
}
|
||||
if (newCommand) {
|
||||
newCommand = false;
|
||||
executing = true;
|
||||
execCommand(command);
|
||||
}
|
||||
if (recalculateDelays) {
|
||||
eggStepperDelay = calculateDelay(eggStepperRPM, STEPS_PER_REVOLUTION);
|
||||
servoStepperDelay =
|
||||
calculateDelay(servoStepperRPM, STEPS_PER_REVOLUTION);
|
||||
recalculateDelays = false;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user