diff --git a/devicecommunication/CommunicationMain.py b/devicecommunication/CommunicationMain.py index 0e837ca..6c4242f 100644 --- a/devicecommunication/CommunicationMain.py +++ b/devicecommunication/CommunicationMain.py @@ -52,6 +52,8 @@ def sendAllLines(lines, timeoutCom): for line in lines: if not line == '\n': sendWithAck(line, timeoutCom) + if line == 'G28': + sleep(1) def sendWithAck(gcodeCommand, timeoutCom): global serial @@ -73,10 +75,10 @@ def sendWithAck(gcodeCommand, timeoutCom): elif received.startswith('-2'): raise RuntimeError('Device error, please reset the device') elif received.startswith('-1'): - raise RuntimeError('Command error') + raise RuntimeError('Command error : ' + gcodeCommand) else: commandTimeout += 1 - if commandTimeout > timeoutCom * 10: + if commandTimeout > timeoutCom * 100: raise RuntimeError('Command not executed') diff --git a/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino b/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino index 407c010..40bd30b 100644 --- a/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino +++ b/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino @@ -1,46 +1,90 @@ /******************************************************************************* -Titre : OpenCR -Date : 6 février 2019 -Auteur : Maxime Desmarais-Laporte +Title : OpenCR +Date : April 10th 2019 +Authors : Maxime Desmarais-Laporte and Marc-Antoine Lafreniere + Descritpion : + GCode interpretor, use in a PCB maker device. + + The device has 3 limits switches (one for every axis) use for a homing of the device + These switches are attach to one interrupt and to 3 digital pins. + The interrupt was noizy so a filter is apply to the interrupt + + +Interrupt filter : + To filter the noize on the interrupt we use the digital pin connected to the NC of the switch + The filter simply looks if the digital pin is LOW for *debounceTimeFalling* time + If it stays LOW for the whole *debounceTimeFalling* then a flag is set to true + If it stays HIGH for the whole *debounceTimeRising* then a flag is set to false + +GCode status : + G0 Xx Yy Zz Done + G28 Done + G90 Inactive + G91 Not implemeted + M18 Done + M112 Done + +Commincations : + When a command is recived, "2" is printed to the Serial port + When the command is done executing, "1" is printed to the Serial port + + If the command can't complete, "-1" is printed to the Serial port + If the device is in eStop mode, "-2" is printed to the Serial port on a commmand request + + +Electrical : + The 3 homing switches are connected the same way : + NC to a digital pin + NO to the interrupt + COM to the ground + Hence every digital and interrupt pin MUST be in INPUT_PULLUP + + +TODO : + Connect 2 switches on the other end of the X and Y axis as a fail-safe and add a filter if needed + Change some variable usage : at the moment everything works only if motors' ids are +1 between each (ex: x=9, y=10, z=11) Specifications : -Baud for motors : 57600 b/s -Adress for motors : 11 and 12 and 13 + Baud for motors : 57600 b/s + +Dynamixel and OpenCR documentation : + http://emanual.robotis.com/docs/en/dxl/x/xm430-w350/ + http://emanual.robotis.com/docs/en/parts/controller/opencr10/ *******************************************************************************/ #include +#include "functions.h" +#include "models.h" #if defined(__OPENCM904__) #define DEVICE_NAME "3" #elif defined(__OPENCR__) #define DEVICE_NAME "" -#endif +#endif #define STRING_BUF_NUM 64 #define MINTICK 0 #define MAXTICK 1048575 - -// 0 = not reverse, 1 = reverse -uint8_t X_REVERSE = 0; -uint8_t Y_REVERSE = 1; -uint8_t Z_REVERSE = 0; +const int ACCEPTABLE_RANGE[3] = { 2, 2, 4 }; const String HOMING_OFFSET = "Homing_Offset"; const String OPERATING_MODE = "Operating_Mode"; const String PRESENT_POSITION = "Present_Position"; const String GOAL_POSITION = "Goal_Position"; -String cmd[STRING_BUF_NUM]; +// 0 = not reverse, 1 = reverse +uint8_t X_REVERSE = 0; +uint8_t Y_REVERSE = 1; +uint8_t Z_REVERSE = 0; +// Dynamixel variables DynamixelWorkbench dxl_wb; +String cmd[STRING_BUF_NUM]; uint8_t get_id[16]; uint8_t scan_cnt = 0; uint8_t ping_cnt = 0; - const char *NULL_POINTER = NULL; -bool isEmegencyState = false; - // Motors Propertys : uint8_t idX = 11; uint8_t idY = 12; @@ -61,45 +105,60 @@ const int emergencySwitchPin = 2; // intrupt pin // Homing variables bool homing = false; +bool homingX = false; +bool homingY = false; +bool homingZ = false; +int homingState = 0; -const int homeOffsetX = 10*tickFromMm; -const int homeOffsetY = 10*tickFromMm; -const int homeOffsetZ = 10*tickFromMm; +const int homeOffsetX = 28*tickFromMm; +const int homeOffsetY = 19.5*tickFromMm; +const int homeOffsetZ = 2.5*tickFromMm; -// Fonctions prototypes : -void Begin(uint32_t baud); -void Ping(int identification); -void Scan(); -void Joint(uint8_t id, uint16_t goal); -void Wheel(uint8_t id, int32_t goal); -void Torque_on(uint8_t id); -void Torque_off(uint8_t id); -void Write(uint8_t id, uint32_t value, String commande); -int32_t Read(uint8_t id, String commande); -void Led(uint8_t id, bool state); -void TorqueOffAll(); -void OffsetAxe(uint8_t id, int offset); -void LimiteSwitch(); -int MovingTick(uint8_t id, int32_t value); -int Homing(); -int HomingAxis(uint8_t id, int speed, int switchPin, int offset); -uint8_t getIdFromChar(char letter); +// Debounce timer variables +bool isFalling = false; +long debounceTimeFalling = 250; +long lastTimeXFalling = 0; +long lastTimeYFalling = 0; +long lastTimeZFalling = 0; + +bool isRising = false; +long debounceTimeRising = 250; +long lastTimeXRising = 0; +long lastTimeYRising = 0; +long lastTimeZRising = 0; + +bool isXSwitchPress = false; +bool isYSwitchPress = false; +bool isZSwitchPress = false; + +// eStop +bool isEmegencyState = false; +bool ledX = false; +bool ledY = false; +bool ledZ = false; + +// Moving variables +bool isMoving = false; +bool currentMoveDone = false; +bool hasFailed = false; +int currentMove = 0; +int nbsMovements = 0; +int32_t currentPosition = 0; +int32_t movingSpeed[] = { 75, 75 }; +MovingCommand commands[3]; -// Initialisation : void setup() { - // Initialisation des pins : pinMode(xSwitchPin, INPUT_PULLUP); pinMode(ySwitchPin, INPUT_PULLUP); pinMode(zSwitchPin, INPUT_PULLUP); pinMode(emergencySwitchPin, INPUT_PULLUP); - attachInterrupt(digitalPinToInterrupt(emergencySwitchPin), LimiteSwitch, RISING); + attachInterrupt(digitalPinToInterrupt(emergencySwitchPin), LimiteSwitch, CHANGE); Serial.begin(57600); - while(!Serial); // Open a Serial Monitor - //Motor Initialisation Section : + //Motor Initialisation Section : Begin((uint32_t)57600); Ping(idX); Ping(idY); @@ -124,18 +183,239 @@ void setup() dxl_wb.writeRegister(idY, 10, 1, &Y_REVERSE, &NULL_POINTER); dxl_wb.writeRegister(idZ, 10, 1, &Z_REVERSE, &NULL_POINTER); + Write(idX, movingSpeed[0], "Profile_Velocity"); + Write(idY, movingSpeed[1], "Profile_Velocity"); + Torque_on(idX); Torque_on(idY); Torque_on(idZ); + + resetMovingVariables(); } -// Main Program void loop() { + if(isFalling) + { + if(digitalRead(xSwitchPin)) + { + if(millis() - lastTimeXFalling > debounceTimeFalling) + { + lastTimeXFalling = millis(); + isFalling = false; + isXSwitchPress = true; + + // Insert what to do if the button is press (executed on change) + if(homingX) + { + homingX = false; + OffsetAxe(idX, homeOffsetX); + Torque_off(idX); + Write(idX, 4, OPERATING_MODE); + Torque_on(idX); + + currentMove = 0; + nbsMovements = 1; + commands[currentMove]._motorId = idX; + commands[currentMove]._goalPosition = 0; + isMoving = true; + Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION); + } + else + { + isEmegencyState = true; + ledX = true; + } + } + } + else + { + lastTimeXFalling = millis(); + isXSwitchPress = false; + } + + if(digitalRead(ySwitchPin)) + { + if(millis() - lastTimeYFalling > debounceTimeFalling) + { + lastTimeYFalling = millis(); + isFalling = false; + isYSwitchPress = true; + + // Insert what to do if the button is press (executed on change) + if(homingY) + { + homingY = false; + OffsetAxe(idY, homeOffsetY); + Torque_off(idY); + Write(idY, 4, OPERATING_MODE); + Torque_on(idY); + + currentMove = 0; + nbsMovements = 1; + commands[currentMove]._motorId = idY; + commands[currentMove]._goalPosition = 0; + isMoving = true; + Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION); + } + else + { + isEmegencyState = true; + ledY = true; + } + } + } + else + { + lastTimeYFalling = millis(); + isYSwitchPress = false; + } + + if(digitalRead(zSwitchPin)) + { + if(millis() - lastTimeZFalling > debounceTimeFalling) + { + lastTimeZFalling = millis(); + isFalling = false; + isZSwitchPress = true; + + // Insert what to do if the button is press (executed on change) + if(homingZ) + { + homingZ = false; + OffsetAxe(idZ, homeOffsetZ); + Torque_off(idZ); + Write(idZ, 4, OPERATING_MODE); + Torque_on(idZ); + + currentMove = 0; + nbsMovements = 1; + commands[currentMove]._motorId = idZ; + commands[currentMove]._goalPosition = 0; + isMoving = true; + Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION); + } + else + { + isEmegencyState = true; + ledZ = true; + } + } + } + else + { + lastTimeZFalling = millis(); + isZSwitchPress = false; + } + + if(!digitalRead(xSwitchPin) && !digitalRead(ySwitchPin) && !digitalRead(zSwitchPin)) + isFalling = false; + } + + if(isRising) + { + if(!digitalRead(xSwitchPin)) + { + if(millis() - lastTimeXRising > debounceTimeRising) + { + lastTimeXRising = millis(); + isRising = false; + isXSwitchPress = false; + // Insert what to do if the button is release (executed on change) + } + } + else + lastTimeXRising = millis(); + + if(!digitalRead(ySwitchPin)) + { + if(millis() - lastTimeYRising > debounceTimeRising) + { + lastTimeYRising = millis(); + isRising = false; + isYSwitchPress = false; + // Insert what to do if the button is release (executed on change) + } + } + else + lastTimeYRising = millis(); + + if(!digitalRead(zSwitchPin)) + { + if(millis() - lastTimeZRising > debounceTimeRising) + { + lastTimeZRising = millis(); + isRising = false; + isZSwitchPress = false; + // Insert what to do if the button is release (executed on change) + } + } + else + lastTimeZRising = millis(); + + if(digitalRead(xSwitchPin) && digitalRead(ySwitchPin) && digitalRead(zSwitchPin)) + { + isRising = false; + } + } + + if(isMoving) + { + currentPosition = Read(commands[currentMove]._motorId, PRESENT_POSITION); + if(isInRange(currentPosition, commands[currentMove]._goalPosition, commands[currentMove]._motorId)) + { + currentMoveDone = true; + } + } + + if(isMoving && currentMoveDone) + { + if(currentMove+1 >= nbsMovements) + { + if(homing) + { + if(commands[currentMove]._motorId == idZ) + { + homingY = true; + HomingAxis(idY, -100); + homingState++; + } + else if(commands[currentMove]._motorId == idY) + { + homingX = true; + HomingAxis(idX, -100); + homingState++; + } + else if(commands[currentMove]._motorId == idX) + { + homingState++; + homing = false; + Serial.println(homingState == 3 ? "1" : "-1"); + homingState = 0; + + Write(idX, movingSpeed[0], "Profile_Velocity"); + Write(idY, movingSpeed[1], "Profile_Velocity"); + } + resetMovingVariables(); + } + else + { + resetMovingVariables(); + Serial.println("1"); + } + } + else + { + currentMoveDone = false; + currentMove++; + Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION); + } + } + if(isEmegencyState) setEmergency(); - if (Serial.available()) + if (!homing && !isMoving && Serial.available()) { String read_string = Serial.readStringUntil('\n'); if(!isEmegencyState) @@ -155,35 +435,49 @@ void loop() } } words[wordIndex] = read_string.substring(start, read_string.length()); - - Serial.println("2"); + nbsMovements = wordIndex; + Serial.println("2"); if(words[0] == "G0") { - for(int i = 1; i < wordIndex+1; i++) - { - if(words[i].length() > 1) - { - float value = words[i].substring(1, words[i].length()).toFloat(); - uint8_t idMotor = getIdFromChar((char)words[i].charAt(0)); - if(idMotor == -1) - Serial.println("-1"); - else - statut[i] = MovingTick(idMotor, value*tickFromMm); - } - else + int i; + for(i = 1; i < wordIndex+1; i++) + { + if(words[i].length() > 1) + { + float value = words[i].substring(1, words[i].length()).toFloat(); + uint8_t idMotor = getIdFromChar((char)words[i].charAt(0)); + if(idMotor == -1) + { Serial.println("-1"); - } - if((statut[0] == 1 || statut[0] == 0) && (statut[1] == 1 || statut[1] == 0) && (statut[2] == 1|| statut[2] == 0)){ - Serial.println("1"); - } - else{ + break; + } + else + { + commands[i-1]._motorId = idMotor; + commands[i-1]._goalPosition = value*tickFromMm; + } + } + else + { Serial.println("-1"); - } + break; + } + } + + if(i == wordIndex+1) + { + isMoving = true; + currentMove = 0; + Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION); + } } - else if(words[0] == "G28"){ - Serial.println(Homing()); + else if(words[0] == "G28") + { + homing = true; + homingZ = true; + HomingAxis(idZ, -50); } else if(words[0] == "M18"){ TorqueOffAll(); @@ -198,15 +492,8 @@ void loop() Serial.println("1"); } - else if(words[0] == "M1") + else if(words[0] == "G90") { - TorqueOffAll(); - - Write(idX, -Read(idX, PRESENT_POSITION), HOMING_OFFSET); - Write(idY, -Read(idY, PRESENT_POSITION), HOMING_OFFSET); - Write(idZ, -Read(idZ, PRESENT_POSITION), HOMING_OFFSET); - - TorqueOnAll(); Serial.println("1"); } else @@ -233,87 +520,70 @@ uint8_t getIdFromChar(char letter) return -1; } -int Homing() +bool isInRange(int goal, int curPos, uint8_t id) { - int state = 0; - homing = true; - - state += HomingAxis(idZ, -50, xSwitchPin, homeOffsetZ); - state += HomingAxis(idY, -100, xSwitchPin, homeOffsetY); - state += HomingAxis(idX, -100, xSwitchPin, homeOffsetX); - - homing = false; - return state == 3 ? 1 : -1; + return (abs(goal - curPos) <= ACCEPTABLE_RANGE[id-idX]); } -int HomingAxis(uint8_t id, int speed, int switchPin, int offset) +void resetMovingVariables() +{ + isMoving = false; + currentMoveDone = false; + currentMove = 0; + nbsMovements = 0; + currentPosition = 0; + hasFailed = false; + + for(int i = 0; i < 3; i++) + { + commands[i]._motorId = i+idX; + commands[i]._goalPosition = Read(i+idX, PRESENT_POSITION); + } +} + +void HomingAxis(uint8_t id, int speed) { Torque_off(id); Write(id, 0, HOMING_OFFSET); Torque_on(id); Wheel(id, speed); - while(digitalRead(switchPin)); - OffsetAxe(id, offset); - Write(id, 4, OPERATING_MODE); - return MovingTick(id, 0); } void OffsetAxe(uint8_t id, int offset){ int32_t posPresent = Read(id, PRESENT_POSITION); int32_t homePosition = - posPresent - offset; - Torque_off(id); + Torque_off(id); + + if(id == idX && X_REVERSE) + homePosition *= -1; + else if(id == idY && Y_REVERSE) + homePosition *= -1; + else if(id == idZ && Z_REVERSE) + homePosition *= -1; + Write(id, homePosition, HOMING_OFFSET); -} - -int MovingTick(uint8_t id, int32_t value){ - int32_t CurrentPosition = Read(id, PRESENT_POSITION); - bool Forward = value > CurrentPosition; - - if((Forward && (CurrentPosition < MAXTICK)) || (!Forward && (CurrentPosition > MINTICK))) - { - Torque_on(id); - Write(id, value, GOAL_POSITION); - } - else - { - Torque_off(id); - return -1; - } - - if(Forward){ - while(CurrentPosition < value-1 && !isEmegencyState){ - CurrentPosition = Read(id, PRESENT_POSITION); - if(CurrentPosition >= MAXTICK && !homing){ - Torque_off(id); - return -1; - } - } - } - else { - while(CurrentPosition > value+1 && !isEmegencyState){ - CurrentPosition = Read(id, PRESENT_POSITION); - if(CurrentPosition <= MINTICK && !homing){ - Torque_off(id); - return -1; - } - } - } - - return 1; + Torque_on(id); } void LimiteSwitch(){ - if(!homing){ - isEmegencyState = true; - } + if(!digitalRead(emergencySwitchPin)) + isRising = true; + else + isFalling = true; } void setEmergency() { + isMoving = false; TorqueOffAll(); - Led(idX, !digitalRead(xSwitchPin)); - Led(idY, !digitalRead(ySwitchPin)); - Led(idZ, !digitalRead(zSwitchPin)); + Led(idX, ledX); + Led(idY, ledY); + Led(idZ, ledZ); +} + +void changeMode(uint8_t id) +{ + Write(id, 4, OPERATING_MODE); } diff --git a/devicesoftware/GcodeInterpreter/functions.h b/devicesoftware/GcodeInterpreter/functions.h new file mode 100644 index 0000000..22cc75f --- /dev/null +++ b/devicesoftware/GcodeInterpreter/functions.h @@ -0,0 +1,23 @@ +#ifndef FUNCTIONS_H +#define FUNCTIONS_H + +void Begin(uint32_t baud); +void Ping(int identification); +void Scan(); +void Joint(uint8_t id, uint16_t goal); +void Wheel(uint8_t id, int32_t goal); +void Torque_on(uint8_t id); +void Torque_off(uint8_t id); +void Write(uint8_t id, uint32_t value, String commande); +int32_t Read(uint8_t id, String commande); +void Led(uint8_t id, bool state); +void TorqueOffAll(); +void OffsetAxe(uint8_t id, int offset); +void LimiteSwitch(); +void HomingAxis(uint8_t id, int speed); +uint8_t getIdFromChar(char letter); +bool isInRange(int curPos, int goal); +void resetMovingVariables(); +void changeMode(uint8_t id); + +#endif diff --git a/devicesoftware/GcodeInterpreter/models.h b/devicesoftware/GcodeInterpreter/models.h new file mode 100644 index 0000000..f216abd --- /dev/null +++ b/devicesoftware/GcodeInterpreter/models.h @@ -0,0 +1,10 @@ +#ifndef MODELS_H +#define MODELS_H + +typedef struct MovingCommand +{ + uint8_t _motorId = 0; + uint32_t _goalPosition = 0; +}; + +#endif diff --git a/gcodeextractor/gcode/GcodeBuilder.py b/gcodeextractor/gcode/GcodeBuilder.py index 06f0e17..b5c7a88 100644 --- a/gcodeextractor/gcode/GcodeBuilder.py +++ b/gcodeextractor/gcode/GcodeBuilder.py @@ -24,12 +24,12 @@ def listToGCode(listIndex, pHeight, pWidth): else: gcodeCommand.append('G0 X' + str(round(coord.getX()*pWidth, 2)) + ' Y' + str(round(coord.getY()*pHeight, 2))) if toolUp: - gcodeCommand.append('G0 Z3') + gcodeCommand.append('G0 Z10.5') toolUp = False # FOOTER gcodeCommand.append('\nG0 Z0') - gcodeCommand.append('G28') + gcodeCommand.append('G0 X0 Y0') gcodeCommand.append('M18') return gcodeCommand \ No newline at end of file diff --git a/gcodeextractor/tests/gcode/test_listToGcode.py b/gcodeextractor/tests/gcode/test_listToGcode.py index 3941d77..e3740ac 100644 --- a/gcodeextractor/tests/gcode/test_listToGcode.py +++ b/gcodeextractor/tests/gcode/test_listToGcode.py @@ -52,10 +52,10 @@ class TestListToGCode(TestCase): def getExpected(coords, ySize, xSize): header = ['G28', 'G90\n'] - footer = ['\nG0 Z0', 'G28', 'M18'] + footer = ['\nG0 Z0', 'G0 X0 Y0', 'M18'] content = ['G0 X' + str(round(xSize * coords[0].getX(), 2)) + ' Y' + str(round(ySize * coords[0].getY(), 2)), - 'G0 Z3', + 'G0 Z10.5', ] for index, coord in enumerate(coords): @@ -63,7 +63,7 @@ def getExpected(coords, ySize, xSize): if coord.getX() != -1 and coord.getY() != -1: content.append('G0 X' + str(xSize * coord.getX()) + ' Y' + str(ySize * coord.getY())) if coords[index - 1].getX() == -1 and coords[index - 1].getX() == -1: - content.append('G0 Z3') + content.append('G0 Z10.5') else: content.append('G0 Z0')