stepus53/EggbotWireless
1
0
Fork 0
mirror of https://github.com/usatiuk/EggbotWireless.git synced 2025-10-26 16:57:48 +01:00
Code Issues Packages Projects Releases Wiki Activity

fix pen servo

Browse Source 
Signed-off-by: Stepan Usatyuk <usaatyuk@ustk.me>
This commit is contained in:
Stepan Usatiuk
2019-05-18 12:40:53 +03:00
parent a96a47f0f9
commit 704fe420e7
5 changed files with 32 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Firmware/EggbotWireless/src/main.cpp
View File

@@ -19,6 +19,7 @@ void sendCommand(float *command) {
Wire.endTransmission(); Wire.endTransmission();
} else { } else {
Serial.println("UNK"); Serial.println("UNK");
return;
} }
if (command[0] == G01 || command[0] == G00) { if (command[0] == G01 || command[0] == G00) {
@@ -29,8 +30,8 @@ void sendCommand(float *command) {
if (command[0] == M99) { if (command[0] == M99) {
Wire.requestFrom(8, 5 * sizeof(float)); Wire.requestFrom(8, 5 * sizeof(float));
float resp[5];
float resp[5];
byte buffer[4]; byte buffer[4];
for (int i = 0; i < 5; i++) { for (int i = 0; i < 5; i++) {
@@ -63,7 +64,6 @@ void loop() {
while (Serial.available() > 0) { while (Serial.available() > 0) {
char inChar = Serial.read(); char inChar = Serial.read();
inString += inChar; inString += inChar;
Serial.write(inChar);
if (inChar == '\n') { if (inChar == '\n') {
inString.trim(); inString.trim();

1
Firmware/MotorControl/include/Globals.h
View File

@@ -27,7 +27,6 @@ float servoStepperDelay;
float eggStepperRPM; float eggStepperRPM;
float servoStepperRPM; float servoStepperRPM;
bool recalculateDelays;
bool needAdjust; bool needAdjust;
#endif #endif

1
Firmware/MotorControl/include/Pen.h
View File

@@ -7,6 +7,7 @@ class Pen {
private: private:
int posEngaged; int posEngaged;
int posDisengaged; int posDisengaged;
int pin;
bool engaged = true; bool engaged = true;
Servo servo; Servo servo;

5
Firmware/MotorControl/src/Pen.cpp
View File

@@ -6,9 +6,7 @@
#define DELAY 15 #define DELAY 15
Pen::Pen(int pin, int posEngaged, int posDisengaged) Pen::Pen(int pin, int posEngaged, int posDisengaged)
: posEngaged(posEngaged), posDisengaged(posDisengaged) { : posEngaged(posEngaged), posDisengaged(posDisengaged), pin(pin) {}
servo.attach(pin);
}
void Pen::engage() { void Pen::engage() {
if (!engaged) { if (!engaged) {
@@ -31,6 +29,7 @@ void Pen::disengage() {
} }
void Pen::init() { void Pen::init() {
servo.attach(pin);
servo.write(posDisengaged); servo.write(posDisengaged);
engaged = false; engaged = false;
} }

59
Firmware/MotorControl/src/main.cpp
View File

@@ -6,22 +6,29 @@
int curRPM = DEF_RPM; int curRPM = DEF_RPM;
int adjustDelay = 10; int adjustDelay = 10;
float calculateDelay(float rpm, int stepsPerRevolution) {
return ((float)1000 * (float)60) / (rpm * (float)stepsPerRevolution);
}
void adjustRPM() { void adjustRPM() {
unsigned int stepsX = servoStepper.getRemainingSteps(); eggStepperRPM = curRPM;
unsigned int stepsY = eggStepper.getRemainingSteps(); servoStepperRPM = curRPM;
if (stepsX != 0 && stepsY != 0) { if (needAdjust) {
if (stepsX > stepsY) { unsigned int stepsX = servoStepper.getRemainingSteps();
float rpm = (float)curRPM * (float)stepsY / (float)stepsX; unsigned int stepsY = eggStepper.getRemainingSteps();
eggStepperRPM = rpm; if (stepsX != 0 && stepsY != 0) {
} else if (stepsY > stepsX) { if (stepsX > stepsY) {
float rpm = (float)curRPM * (float)stepsX / (float)stepsY; float rpm = (float)curRPM * (float)stepsY / (float)stepsX;
servoStepperRPM = rpm; eggStepperRPM = rpm;
} else { } else if (stepsY > stepsX) {
eggStepperRPM = curRPM; float rpm = (float)curRPM * (float)stepsX / (float)stepsY;
servoStepperRPM = curRPM; servoStepperRPM = rpm;
}
} }
recalculateDelays = true;
} }
eggStepperDelay = calculateDelay(eggStepperRPM, STEPS_PER_REVOLUTION);
servoStepperDelay = calculateDelay(servoStepperRPM, STEPS_PER_REVOLUTION);
} }
int curFloat = 0; int curFloat = 0;
@@ -69,12 +76,6 @@ void requestEvent() {
} }
} }
float calculateDelay(float rpm, int stepsPerRevolution) {
return ((float)1000 * (float)60) / (rpm * (float)stepsPerRevolution);
}
Servo servo;
void execCommand(float *command) { void execCommand(float *command) {
if (command[0] == G01 || command[0] == G00) { if (command[0] == G01 || command[0] == G00) {
if (command[0] == G01) { if (command[0] == G01) {
@@ -111,8 +112,7 @@ void setup() {
Wire.onReceive(receiveEvent); Wire.onReceive(receiveEvent);
Wire.onRequest(requestEvent); Wire.onRequest(requestEvent);
Serial.println("Hello!"); Serial.println("Hello!");
recalculateDelays = true; eggStepperRPM = servoStepperRPM = curRPM;
eggStepperRPM = servoStepperRPM = 4;
OCR2 = 250; OCR2 = 250;
TCCR2 |= (1 << WGM12) | (1 << CS22); TCCR2 |= (1 << WGM12) | (1 << CS22);
TIMSK |= (1 << OCIE2); TIMSK |= (1 << OCIE2);
@@ -129,25 +129,20 @@ ISR(TIMER2_COMP_vect) {
if (fmod(ms, servoStepperDelay) < 1) { if (fmod(ms, servoStepperDelay) < 1) {
servoStepper.doStep(); servoStepper.doStep();
} }
if (fmod(ms, adjustDelay) < 1) {
adjustRPM();
}
} }
void loop() { void loop() {
if (eggStepper.getRemainingSteps() == 0 && unsigned long ms = millis();
servoStepper.getRemainingSteps() == 0) { if (ms % adjustDelay < 2) {
executing = false; adjustRPM();
} }
if (newCommand) { if (newCommand) {
newCommand = false; newCommand = false;
executing = true; executing = true;
execCommand(command); execCommand(command);
} }
if (recalculateDelays) { if (eggStepper.getRemainingSteps() == 0 &&
eggStepperDelay = calculateDelay(eggStepperRPM, STEPS_PER_REVOLUTION); servoStepper.getRemainingSteps() == 0) {
servoStepperDelay = executing = false;
calculateDelay(servoStepperRPM, STEPS_PER_REVOLUTION);
recalculateDelays = false;
} }
} }