From 7e2d4f0e4ffa56eca48c85308dd01544da0ff779 Mon Sep 17 00:00:00 2001 From: Sandy Noble Date: Fri, 19 Apr 2013 23:58:41 +0100 Subject: [PATCH] Updated with new files. Currently too big for a duemilanove! --- comms.ino | 32 +++++++----- eeprom.ino | 36 ++++++++------ exec.ino | 102 ++++++++++++++++++++++++++------------- penlift.ino | 87 ++++++++++++++++++++------------- pixel.ino | 2 +- polargraph_server_a1.ino | 4 +- util.ino | 49 ++++++++++++++++++- 7 files changed, 214 insertions(+), 98 deletions(-) diff --git a/comms.ino b/comms.ino index 180158a..d86469b 100644 --- a/comms.ino +++ b/comms.ino @@ -40,14 +40,15 @@ String comms_waitForNextCommand() // this also sets usingCrc AND commandConfirmed // to true or false inS = comms_readCommand(); + + // if it's using the CRC check, then confirmation is easy + if (usingCrc && !commandConfirmed) + { + comms_requestResend(); + inS = ""; + } } - // if it's using the CRC check, then confirmation is easy - if (usingCrc && !commandConfirmed) - { - comms_requestResend(); - return ""; - } // CRC was ok, or we aren't using one idleTime = millis(); @@ -76,7 +77,7 @@ String comms_readCommand() while (Serial.available() > 0) { char ch = Serial.read(); // get it - delay(15); + delay(1); inString[inCount] = ch; if (ch == INTERMINATOR) { @@ -87,7 +88,7 @@ String comms_readCommand() } inString[inCount] = 0; // null terminate the string String inS = inString; - + // check the CRC for this command // and set commandConfirmed true or false int colonPos = inS.lastIndexOf(":"); @@ -106,10 +107,13 @@ String comms_readCommand() } else { + Serial.print(F("I got ")); + Serial.println(inString); Serial.print(F("Checksum not matched!:")); Serial.println(calcCrc); commandConfirmed = false; } + } else { @@ -127,14 +131,18 @@ void comms_parseAndExecuteCommand(String &in) if (commandParsed) { impl_processCommand(lastCommand); + in = ""; + commandConfirmed = false; + comms_ready(); } else { - Serial.println(F("Command not parsed.")); + Serial.print(F("Command (")); + Serial.print(in); + Serial.println(F(") not parsed.")); } - in = ""; - commandConfirmed = false; - comms_ready(); + inNoOfParams = 0; + } diff --git a/eeprom.ino b/eeprom.ino index 969359f..49640a5 100644 --- a/eeprom.ino +++ b/eeprom.ino @@ -27,7 +27,7 @@ void eeprom_resetEeprom() } void eeprom_dumpEeprom() { - for (int i = 0; i <20; i++) + for (int i = 0; i <40; i++) { Serial.print(i); Serial.print(". "); @@ -76,7 +76,7 @@ void eeprom_loadSpoolSpec() void eeprom_loadPenLiftRange() { EEPROM_readAnything(EEPROM_PENLIFT_DOWN, downPosition); - if (downPosition < 1) + if (downPosition < 0) { downPosition = DEFAULT_DOWN_POSITION; } @@ -84,7 +84,7 @@ void eeprom_loadPenLiftRange() Serial.println(downPosition); EEPROM_readAnything(EEPROM_PENLIFT_UP, upPosition); - if (upPosition < 1) + if (upPosition < 0) { upPosition = DEFAULT_UP_POSITION; } @@ -120,6 +120,24 @@ void eeprom_loadStepMultiplier() Serial.println(stepMultiplier); } +void eeprom_loadSpeed() +{ + // load speed, acceleration + EEPROM_readAnything(EEPROM_MACHINE_MOTOR_SPEED, currentMaxSpeed); + + // not sure why this requires a cast to int for the comparision, but a + // if (currentMaxSpeed < 1.0) wasn't catching cases where + // currentMaxSpeed == 0.00, ODD. + if (int(currentMaxSpeed) < 1) { + currentMaxSpeed = 800.0; + } + + EEPROM_readAnything(EEPROM_MACHINE_MOTOR_ACCEL, currentAcceleration); + if (int(currentAcceleration) < 1) { + currentAcceleration = 800.0; + } +} + void eeprom_loadMachineSpecFromEeprom() { impl_loadMachineSpecFromEeprom(); @@ -129,22 +147,12 @@ void eeprom_loadMachineSpecFromEeprom() eeprom_loadStepMultiplier(); eeprom_loadMachineName(); eeprom_loadPenLiftRange(); + eeprom_loadSpeed(); - - // load speed, acceleration - EEPROM_readAnything(EEPROM_MACHINE_MOTOR_SPEED, currentMaxSpeed); - if (currentMaxSpeed < 1) - currentMaxSpeed = 800.0; - - EEPROM_readAnything(EEPROM_MACHINE_MOTOR_ACCEL, currentAcceleration); - if (currentAcceleration < 1) - currentAcceleration = 800.0; - // load penwidth EEPROM_readAnything(EEPROM_MACHINE_PEN_WIDTH, penWidth); if (penWidth < 0.0001) penWidth = 0.8; - mmPerStep = mmPerRev / multiplier(motorStepsPerRev); stepsPerMM = multiplier(motorStepsPerRev) / mmPerRev; diff --git a/exec.ino b/exec.ino index d0e00d6..4915c55 100644 --- a/exec.ino +++ b/exec.ino @@ -27,10 +27,6 @@ boolean exec_executeBasicCommand(String &com) exec_changeLengthDirect(); else if (com.startsWith(CMD_CHANGEPENWIDTH)) exec_changePenWidth(); - else if (com.startsWith(CMD_CHANGEMOTORSPEED)) - exec_changeMotorSpeed(); - else if (com.startsWith(CMD_CHANGEMOTORACCEL)) - exec_changeMotorAcceleration(); else if (com.startsWith(CMD_SETMOTORSPEED)) exec_setMotorSpeed(); else if (com.startsWith(CMD_SETMOTORACCEL)) @@ -39,8 +35,6 @@ boolean exec_executeBasicCommand(String &com) pixel_drawSquarePixel(); else if (com.startsWith(CMD_DRAWSCRIBBLEPIXEL)) pixel_drawScribblePixel(); -// else if (com.startsWith(CMD_DRAWRECT)) -// drawRectangle(); else if (com.startsWith(CMD_CHANGEDRAWINGDIRECTION)) exec_changeDrawingDirection(); else if (com.startsWith(CMD_SETPOSITION)) @@ -77,10 +71,10 @@ void exec_changeDrawingDirection() { globalDrawDirectionMode = asInt(inParam1); globalDrawDirection = asInt(inParam2); - Serial.print(F("Changed draw direction mode to be ")); - Serial.print(globalDrawDirectionMode); - Serial.print(F(" and direction is ")); - Serial.println(globalDrawDirection); +// Serial.print(F("Changed draw direction mode to be ")); +// Serial.print(globalDrawDirectionMode); +// Serial.print(F(" and direction is ")); +// Serial.println(globalDrawDirection); } @@ -109,6 +103,18 @@ void exec_reportMachineSpec() Serial.print(stepMultiplier); Serial.println(CMD_END); + Serial.print(F("PGLIFT,")); + Serial.print(downPosition); + Serial.print(COMMA); + Serial.print(upPosition); + Serial.println(CMD_END); + + Serial.print(F("PGSPEED,")); + Serial.print(currentMaxSpeed); + Serial.print(COMMA); + Serial.print(currentAcceleration); + Serial.println(CMD_END); + } void exec_setMachineSizeFromCommand() @@ -135,6 +141,8 @@ void exec_setMachineSizeFromCommand() // reload eeprom_loadMachineSize(); } + + void exec_setMachineNameFromCommand() { String name = inParam1; @@ -165,18 +173,45 @@ void exec_setMachineStepMultiplierFromCommand() EEPROM_writeAnything(EEPROM_MACHINE_STEP_MULTIPLIER, asInt(inParam1)); eeprom_loadMachineSpecFromEeprom(); } + void exec_setPenLiftRange() { int down = asInt(inParam1); int up = asInt(inParam2); - EEPROM_writeAnything(EEPROM_PENLIFT_DOWN, down); - EEPROM_writeAnything(EEPROM_PENLIFT_UP, up); - eeprom_loadPenLiftRange(); + + Serial.print(F("Down: ")); + Serial.println(down); + Serial.print(F("Up: ")); + Serial.println(up); + + if (inNoOfParams == 4) + { + // 4 params (C45,,,1,END) means save values to EEPROM + EEPROM_writeAnything(EEPROM_PENLIFT_DOWN, down); + EEPROM_writeAnything(EEPROM_PENLIFT_UP, up); + eeprom_loadPenLiftRange(); + } + else if (inNoOfParams == 3) + { + // 3 params (C45,,,END) means just do a range test + penlift_movePen(down, up, penLiftSpeed); + delay(200); + penlift_movePen(up, down, penLiftSpeed); + delay(200); + penlift_movePen(down, up, penLiftSpeed); + delay(200); + penlift_movePen(up, down, penLiftSpeed); + delay(200); + } } +/* Single parameter to set max speed, add a second parameter of "1" to make it persist. +*/ void exec_setMotorSpeed() { exec_setMotorSpeed(asFloat(inParam1)); + if (inNoOfParams == 3 && asInt(inParam2) == 1) + EEPROM_writeAnything(EEPROM_MACHINE_MOTOR_SPEED, currentMaxSpeed); } void exec_setMotorSpeed(float speed) @@ -188,18 +223,13 @@ void exec_setMotorSpeed(float speed) Serial.println(currentMaxSpeed); } -void exec_changeMotorSpeed() -{ - float speedChange = asFloat(inParam1); - float newSpeed = currentMaxSpeed + speedChange; - exec_setMotorSpeed(newSpeed); -} - - - +/* Single parameter to set acceleration, add a second parameter of "1" to make it persist. +*/ void exec_setMotorAcceleration() { exec_setMotorAcceleration(asFloat(inParam1)); + if (inNoOfParams == 3 && asInt(inParam2) == 1) + EEPROM_writeAnything(EEPROM_MACHINE_MOTOR_ACCEL, currentAcceleration); } void exec_setMotorAcceleration(float accel) { @@ -209,12 +239,6 @@ void exec_setMotorAcceleration(float accel) Serial.print(F("New acceleration: ")); Serial.println(currentAcceleration); } -void exec_changeMotorAcceleration() -{ - float speedChange = asFloat(inParam1); - float newAccel = currentAcceleration + speedChange; - exec_setMotorAcceleration(newAccel); -} void exec_changePenWidth() { @@ -269,7 +293,6 @@ void exec_changeLengthDirect() } else { - Serial.println("dr.5"); exec_drawBetweenPoints(startA, startB, endA, endB, maxSegmentLength); } } @@ -295,8 +318,19 @@ void exec_drawBetweenPoints(float p1a, float p1b, float p2a, float p2b, int maxS float c2y = getCartesianYFP(c2x, p2a); // test to see if it's on the page - if (c2x > 20 && c2x 20 && c2y 20 + && c2x 20 + && c2y 20 + && c1x 20 + && c1y 0) { +// Serial.print("Line segment: " ); +// Serial.println(linesegs); // compute next new location c1x = c1x + deltaX; c1y = c1y + deltaY; @@ -341,7 +377,7 @@ void exec_drawBetweenPoints(float p1a, float p1b, float p2a, float p2b, int maxS // do the move runSpeed = desiredSpeed(linesegs, runSpeed, currentAcceleration*4); -// Serial.print("Runspeed:"); +// Serial.print("Setting speed:"); // Serial.println(runSpeed); motorA.setSpeed(runSpeed); @@ -353,8 +389,6 @@ void exec_drawBetweenPoints(float p1a, float p1b, float p2a, float p2b, int maxS } // do the end point in case theres been some rounding errors etc - motorA.setSpeed(0); - motorB.setSpeed(0); reportingPosition = true; changeLength(p2a, p2b); diff --git a/penlift.ino b/penlift.ino index 66ee126..b880d36 100644 --- a/penlift.ino +++ b/penlift.ino @@ -11,30 +11,63 @@ This is one of the core files for the polargraph server program. This file contains the servo calls that raise or lower the pen from the page. +The behaviour of the pen lift is this: + +If a simple "pen up", or "pen lift" command is received ("C14,END"), then the machine will +not try to lift the pen if it thinks it is already up. It checks the value of the +global boolean variable "isPenUp" to decide this. + +If a qualified "pen up" is received, that is one that includes a pen position (eg "C14,150,END"), +then the global "up" position variable is updated, and the servo is moved to that position, +even if it already is "up". Because naturally, if the up position has changed, even if it +is already up, there's a good chance it won't be up enough. + +The same goes for the + */ -void penlift_penUp() + +void penlift_movePen(int start, int end, int delay_ms) { - if (inNoOfParams > 1) + penHeight.attach(PEN_HEIGHT_SERVO_PIN); + if(start < end) { - upPosition = asInt(inParam1); + for (int i=start; i<=end; i++) + { + penHeight.write(i); + delay(delay_ms); +// Serial.println(i); + } } - if (isPenUp == false) + else { - penlift_movePenUp(); + for (int i=start; i>=end; i--) + { + penHeight.write(i); + delay(delay_ms); +// Serial.println(i); + } } + penHeight.detach(); } -void penlift_movePenUp() +void penlift_penUp() { - penHeight.attach(PEN_HEIGHT_SERVO_PIN); - for (int i=downPosition; i 1) + { +// Serial.print("Penup with params"); + int positionToMoveFrom = isPenUp ? upPosition : downPosition; + upPosition = asInt(inParam1); + penlift_movePen(positionToMoveFrom, upPosition, penLiftSpeed); + } + else + { + if (isPenUp == false) + { + penlift_movePen(downPosition, upPosition, penLiftSpeed); + } } - penHeight.detach(); isPenUp = true; -} - +} void penlift_penDown() { @@ -42,33 +75,21 @@ void penlift_penDown() // parameter then this sets the "down" motor position too). if (inNoOfParams > 1) { + int positionToMoveFrom = isPenUp ? upPosition : downPosition; downPosition = asInt(inParam1); + penlift_movePen(positionToMoveFrom, downPosition, penLiftSpeed); } - if (isPenUp == true) + else { - penlift_movePenDown(); - } -} -void penlift_movePenDown() -{ - penHeight.attach(PEN_HEIGHT_SERVO_PIN); - for (int i=upPosition; i>downPosition; i--) { -// Serial.println(i); - penHeight.write(i); - delay(penLiftSpeed); + if (isPenUp == true) + { + penlift_movePen(upPosition, downPosition, penLiftSpeed); + } } - penHeight.detach(); isPenUp = false; } -void penlift_testServoRange() +void penlift_testRange() { - penHeight.attach(PEN_HEIGHT_SERVO_PIN); - for (int i=0; i<200; i++) { - Serial.println(i); - penHeight.write(i); - delay(15); - } - penHeight.detach(); } diff --git a/pixel.ino b/pixel.ino index 20b7635..168ec94 100644 --- a/pixel.ino +++ b/pixel.ino @@ -90,7 +90,7 @@ void pixel_drawSquarePixel() } changeLength(endPointA, endPointB); - outputAvailableMemory(); + //outputAvailableMemory(); } byte pixel_getRandomDrawDirection() diff --git a/polargraph_server_a1.ino b/polargraph_server_a1.ino index 29e66f8..f096de5 100644 --- a/polargraph_server_a1.ino +++ b/polargraph_server_a1.ino @@ -223,9 +223,7 @@ void setup() readyString = READY; comms_establishContact(); - //testServoRange(); - penlift_movePenUp(); - + penlift_penUp(); delay(500); outputAvailableMemory(); } diff --git a/util.ino b/util.ino index ea8fa61..928cde0 100644 --- a/util.ino +++ b/util.ino @@ -87,12 +87,56 @@ void changeLength(float tA, float tB) lastOperationTime = millis(); transform(tA,tB); + + float currSpeedA = motorA.speed(); + float currSpeedB = motorB.speed(); + +// Serial.print("A pos: "); +// Serial.print(motorA.currentPosition()); +// Serial.print(", A target: "); +// Serial.println(tA); +// Serial.print("B pos: "); +// Serial.print(motorB.currentPosition()); +// Serial.print(", B target: "); +// Serial.println(tB); + + + motorA.setSpeed(0.0); + motorB.setSpeed(0.0); motorA.moveTo(tA); motorB.moveTo(tB); + if (!usingAcceleration) + { + // The moveTo() function changes the speed in order to do a proper + // acceleration. This counteracts it. Ha. + + if (motorA.speed() < 0) + currSpeedA = -currSpeedA; + if (motorB.speed() < 0) + currSpeedB = -currSpeedB; + +// Serial.print("Setting A speed "); +// Serial.print(motorA.speed()); +// Serial.print(" back to "); +// Serial.println(currSpeedA); +// Serial.print("Setting B speed "); +// Serial.print(motorB.speed()); +// Serial.print(" back to "); +// Serial.println(currSpeedB); + + motorA.setSpeed(currSpeedA); + motorB.setSpeed(currSpeedB); + } + + while (motorA.distanceToGo() != 0 || motorB.distanceToGo() != 0) { +// Serial.print("dA:"); +// Serial.print(motorA.distanceToGo()); +// Serial.print(", dB:"); +// Serial.println(motorB.distanceToGo()); impl_runBackgroundProcesses(); if (currentlyRunning) { @@ -103,6 +147,8 @@ void changeLength(float tA, float tB) } else { +// Serial.print("Run speed.."); +// Serial.println(motorA.speed()); motorA.runSpeedToPosition(); motorB.runSpeedToPosition(); } @@ -124,6 +170,7 @@ void changeLengthRelative(long tA, long tB) while (motorA.distanceToGo() != 0 || motorB.distanceToGo() != 0) { + //impl_runBackgroundProcesses(); if (currentlyRunning) { if (usingAcceleration) @@ -208,7 +255,7 @@ void reportPosition() // Serial.print(cY*mmPerStep); // Serial.println(CMD_END); // - outputAvailableMemory(); + //outputAvailableMemory(); } }