From 1de6f3efcb2f59d0ead1e4bb1febc691947ba4c8 Mon Sep 17 00:00:00 2001 From: Marc-Antoine Lafreniere Date: Sun, 7 Apr 2019 12:22:28 -0400 Subject: [PATCH 1/6] Homing issues Fix homing if axis is reverse --- .../GcodeInterpreter/GcodeInterpreter.ino | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino b/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino index 407c010..5051585 100644 --- a/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino +++ b/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino @@ -94,7 +94,7 @@ void setup() pinMode(ySwitchPin, INPUT_PULLUP); pinMode(zSwitchPin, INPUT_PULLUP); pinMode(emergencySwitchPin, INPUT_PULLUP); - attachInterrupt(digitalPinToInterrupt(emergencySwitchPin), LimiteSwitch, RISING); + attachInterrupt(digitalPinToInterrupt(emergencySwitchPin), LimiteSwitch, FALLING); Serial.begin(57600); while(!Serial); // Open a Serial Monitor @@ -120,7 +120,7 @@ void setup() Write(idY, 0, HOMING_OFFSET); Write(idZ, 0, HOMING_OFFSET); - dxl_wb.writeRegister(idX, 10, 1, &X_REVERSE, &NULL_POINTER); + dxl_wb.writeRegister(idX, 10, 1, & , &NULL_POINTER); dxl_wb.writeRegister(idY, 10, 1, &Y_REVERSE, &NULL_POINTER); dxl_wb.writeRegister(idZ, 10, 1, &Z_REVERSE, &NULL_POINTER); @@ -238,12 +238,12 @@ int Homing() int state = 0; homing = true; - state += HomingAxis(idZ, -50, xSwitchPin, homeOffsetZ); - state += HomingAxis(idY, -100, xSwitchPin, homeOffsetY); + //state += HomingAxis(idZ, -50, zSwitchPin, homeOffsetZ); + state += HomingAxis(idY, -100, ySwitchPin, homeOffsetY); state += HomingAxis(idX, -100, xSwitchPin, homeOffsetX); homing = false; - return state == 3 ? 1 : -1; + return state == 2 ? 1 : -1; } int HomingAxis(uint8_t id, int speed, int switchPin, int offset) @@ -252,7 +252,7 @@ int HomingAxis(uint8_t id, int speed, int switchPin, int offset) Write(id, 0, HOMING_OFFSET); Torque_on(id); Wheel(id, speed); - while(digitalRead(switchPin)); + while(!digitalRead(switchPin)); OffsetAxe(id, offset); Write(id, 4, OPERATING_MODE); return MovingTick(id, 0); @@ -262,6 +262,14 @@ void OffsetAxe(uint8_t id, int offset){ int32_t posPresent = Read(id, PRESENT_POSITION); int32_t homePosition = - posPresent - offset; 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); } From 6425c1433037c0fde8a6e0fb3fb396cafa3e623f Mon Sep 17 00:00:00 2001 From: Marc-Antoine Lafreniere Date: Sun, 7 Apr 2019 19:59:01 -0400 Subject: [PATCH 2/6] Noise filter Add noise filter to all limit switches --- devicecommunication/CommunicationMain.py | 2 +- .../GcodeInterpreter/GcodeInterpreter.ino | 209 +++++++++++++++--- 2 files changed, 176 insertions(+), 35 deletions(-) diff --git a/devicecommunication/CommunicationMain.py b/devicecommunication/CommunicationMain.py index 0e837ca..fddf734 100644 --- a/devicecommunication/CommunicationMain.py +++ b/devicecommunication/CommunicationMain.py @@ -73,7 +73,7 @@ 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: diff --git a/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino b/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino index 5051585..4be9a3c 100644 --- a/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino +++ b/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino @@ -61,10 +61,31 @@ 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 homeOffsetZ = 5*tickFromMm; + +// 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; // Fonctions prototypes : void Begin(uint32_t baud); @@ -81,25 +102,22 @@ 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); +void HomingAxis(uint8_t id, int speed); uint8_t getIdFromChar(char letter); // 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, FALLING); + 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); @@ -120,7 +138,7 @@ void setup() Write(idY, 0, HOMING_OFFSET); Write(idZ, 0, HOMING_OFFSET); - dxl_wb.writeRegister(idX, 10, 1, & , &NULL_POINTER); + dxl_wb.writeRegister(idX, 10, 1, &X_REVERSE, &NULL_POINTER); dxl_wb.writeRegister(idY, 10, 1, &Y_REVERSE, &NULL_POINTER); dxl_wb.writeRegister(idZ, 10, 1, &Z_REVERSE, &NULL_POINTER); @@ -132,10 +150,146 @@ void setup() // Main Program void loop() { + if(isFalling) + { + if(digitalRead(xSwitchPin)) + { + if(millis() - lastTimeXFalling > debounceTimeFalling) + { + lastTimeXFalling = millis(); + isFalling = false; + isXSwitchPress = true; + + if(homingX) + { + homingX = false; + OffsetAxe(idX, homeOffsetX); + Write(idX, 4, OPERATING_MODE); + homingState += MovingTick(idX, 0); + + Serial.println(homingState == 3 ? "1" : "-1"); + homingState = 0; + homing = false; + } + else + isEmegencyState = true; + } + } + else + { + lastTimeXFalling = millis(); + isXSwitchPress = false; + } + + if(digitalRead(ySwitchPin)) + { + if(millis() - lastTimeYFalling > debounceTimeFalling) + { + lastTimeYFalling = millis(); + isFalling = false; + isYSwitchPress = true; + + if(homingY) + { + homingY = false; + OffsetAxe(idY, homeOffsetY); + Write(idY, 4, OPERATING_MODE); + homingState += MovingTick(idY, 0); + + homingX = true; + HomingAxis(idX, -100); + } + else + isEmegencyState = true; + } + } + else + { + lastTimeYFalling = millis(); + isYSwitchPress = false; + } + + if(digitalRead(zSwitchPin)) + { + if(millis() - lastTimeZFalling > debounceTimeFalling) + { + lastTimeZFalling = millis(); + isFalling = false; + isZSwitchPress = true; + + if(homingZ) + { + homingZ = false; + OffsetAxe(idZ, homeOffsetZ); + Write(idZ, 4, OPERATING_MODE); + homingState += MovingTick(idZ, 0); + + homingY = true; + HomingAxis(idY, -100); + } + else + isEmegencyState = 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; + } + } + else + lastTimeXRising = millis(); + + if(!digitalRead(ySwitchPin)) + { + if(millis() - lastTimeYRising > debounceTimeRising) + { + lastTimeYRising = millis(); + isRising = false; + isYSwitchPress = false; + } + } + else + lastTimeYRising = millis(); + + if(!digitalRead(zSwitchPin)) + { + if(millis() - lastTimeZRising > debounceTimeRising) + { + lastTimeZRising = millis(); + isRising = false; + isZSwitchPress = false; + } + } + else + lastTimeZRising = millis(); + + if(digitalRead(xSwitchPin) && digitalRead(ySwitchPin) && digitalRead(zSwitchPin)) + { + isRising = false; + } + } + + if(isEmegencyState) setEmergency(); - if (Serial.available()) + if (!homing && Serial.available()) { String read_string = Serial.readStringUntil('\n'); if(!isEmegencyState) @@ -182,8 +336,11 @@ void loop() } } - 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(); @@ -233,29 +390,12 @@ uint8_t getIdFromChar(char letter) return -1; } -int Homing() -{ - int state = 0; - homing = true; - - //state += HomingAxis(idZ, -50, zSwitchPin, homeOffsetZ); - state += HomingAxis(idY, -100, ySwitchPin, homeOffsetY); - state += HomingAxis(idX, -100, xSwitchPin, homeOffsetX); - - homing = false; - return state == 2 ? 1 : -1; -} - -int HomingAxis(uint8_t id, int speed, int switchPin, int offset) +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){ @@ -284,7 +424,7 @@ int MovingTick(uint8_t id, int32_t value){ } else { - Torque_off(id); + Torque_off(id); return -1; } @@ -311,9 +451,10 @@ int MovingTick(uint8_t id, int32_t value){ } void LimiteSwitch(){ - if(!homing){ - isEmegencyState = true; - } + if(!digitalRead(emergencySwitchPin)) + isRising = true; + else + isFalling = true; } void setEmergency() @@ -321,7 +462,7 @@ void setEmergency() TorqueOffAll(); Led(idX, !digitalRead(xSwitchPin)); Led(idY, !digitalRead(ySwitchPin)); - Led(idZ, !digitalRead(zSwitchPin)); + Led(idZ, !digitalRead(zSwitchPin)); } From a12c9fe08351b012bef48ffd24746f990b4f8420 Mon Sep 17 00:00:00 2001 From: Marc-Antoine Lafreniere Date: Tue, 9 Apr 2019 21:15:02 -0400 Subject: [PATCH 3/6] Fixing bugs Removed blocking whiles --- devicecommunication/CommunicationMain.py | 2 + .../GcodeInterpreter/GcodeInterpreter.ino | 249 +++++++++++------- devicesoftware/GcodeInterpreter/functions.h | 22 ++ devicesoftware/GcodeInterpreter/models.h | 10 + gcodeextractor/gcode/GcodeBuilder.py | 4 +- 5 files changed, 191 insertions(+), 96 deletions(-) create mode 100644 devicesoftware/GcodeInterpreter/functions.h create mode 100644 devicesoftware/GcodeInterpreter/models.h diff --git a/devicecommunication/CommunicationMain.py b/devicecommunication/CommunicationMain.py index fddf734..d4e354a 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 diff --git a/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino b/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino index 4be9a3c..c9d9e6b 100644 --- a/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino +++ b/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino @@ -9,6 +9,8 @@ Baud for motors : 57600 b/s Adress for motors : 11 and 12 and 13 *******************************************************************************/ #include +#include "functions.h" +#include "models.h" #if defined(__OPENCM904__) #define DEVICE_NAME "3" @@ -19,6 +21,7 @@ Adress for motors : 11 and 12 and 13 #define STRING_BUF_NUM 64 #define MINTICK 0 #define MAXTICK 1048575 +const int ACCEPTABLE_RANGE[3] = { 2, 2, 3 }; // 0 = not reverse, 1 = reverse uint8_t X_REVERSE = 0; @@ -66,9 +69,9 @@ bool homingY = false; bool homingZ = false; int homingState = 0; -const int homeOffsetX = 10*tickFromMm; -const int homeOffsetY = 10*tickFromMm; -const int homeOffsetZ = 5*tickFromMm; +const int homeOffsetX = /*28*/48*tickFromMm; +const int homeOffsetY = /*19.5*/40*tickFromMm; +const int homeOffsetZ = 2.5*tickFromMm; // Debounce timer variables bool isFalling = false; @@ -87,23 +90,14 @@ bool isXSwitchPress = false; bool isYSwitchPress = false; bool isZSwitchPress = false; -// 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); -void HomingAxis(uint8_t id, int speed); -uint8_t getIdFromChar(char letter); +// Moving variables +bool isMoving = false; +bool currentMoveDone = false; +bool hasFailed = false; +int currentMove = 0; +int nbsMovements = 0; +int32_t currentPosition = 0; +MovingCommand commands[3]; // Initialisation : void setup() @@ -141,10 +135,13 @@ void setup() dxl_wb.writeRegister(idX, 10, 1, &X_REVERSE, &NULL_POINTER); dxl_wb.writeRegister(idY, 10, 1, &Y_REVERSE, &NULL_POINTER); dxl_wb.writeRegister(idZ, 10, 1, &Z_REVERSE, &NULL_POINTER); + Torque_on(idX); Torque_on(idY); Torque_on(idZ); + + resetMovingVariables(); } // Main Program @@ -164,12 +161,16 @@ void loop() { homingX = false; OffsetAxe(idX, homeOffsetX); + Torque_off(idX); Write(idX, 4, OPERATING_MODE); - homingState += MovingTick(idX, 0); - - Serial.println(homingState == 3 ? "1" : "-1"); - homingState = 0; - homing = false; + 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; @@ -193,11 +194,16 @@ void loop() { homingY = false; OffsetAxe(idY, homeOffsetY); + Torque_off(idY); Write(idY, 4, OPERATING_MODE); - homingState += MovingTick(idY, 0); - - homingX = true; - HomingAxis(idX, -100); + 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; @@ -221,11 +227,16 @@ void loop() { homingZ = false; OffsetAxe(idZ, homeOffsetZ); + Torque_off(idZ); Write(idZ, 4, OPERATING_MODE); - homingState += MovingTick(idZ, 0); + Torque_on(idZ); - homingY = true; - HomingAxis(idY, -100); + 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; @@ -285,11 +296,60 @@ void loop() } } + 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; + } + resetMovingVariables(); + } + else + { + resetMovingVariables(); + Serial.println("1"); + } + } + else + { + currentMoveDone = false; + currentMove++; + Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION); + } + } if(isEmegencyState) setEmergency(); - if (!homing && Serial.available()) + if (!homing && !isMoving && Serial.available()) { String read_string = Serial.readStringUntil('\n'); if(!isEmegencyState) @@ -309,31 +369,42 @@ 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") @@ -366,6 +437,10 @@ void loop() TorqueOnAll(); Serial.println("1"); } + else if(words[0] == "G90") + { + Serial.println("1"); + } else { Serial.println("-1"); @@ -390,6 +465,27 @@ uint8_t getIdFromChar(char letter) return -1; } +bool isInRange(int goal, int curPos, uint8_t id) +{ + return (abs(goal - curPos) <= ACCEPTABLE_RANGE[id-idX]); +} + +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); @@ -401,7 +497,7 @@ void HomingAxis(uint8_t id, int speed) 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; @@ -411,43 +507,7 @@ void OffsetAxe(uint8_t id, int offset){ 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(){ @@ -459,10 +519,11 @@ void LimiteSwitch(){ void setEmergency() { + isMoving = false; TorqueOffAll(); - Led(idX, !digitalRead(xSwitchPin)); - Led(idY, !digitalRead(ySwitchPin)); - Led(idZ, !digitalRead(zSwitchPin)); + Led(idX, digitalRead(xSwitchPin)); + Led(idY, digitalRead(ySwitchPin)); + Led(idZ, digitalRead(zSwitchPin)); } diff --git a/devicesoftware/GcodeInterpreter/functions.h b/devicesoftware/GcodeInterpreter/functions.h new file mode 100644 index 0000000..e873e54 --- /dev/null +++ b/devicesoftware/GcodeInterpreter/functions.h @@ -0,0 +1,22 @@ +#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(); + +#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 From e1e0ca6fa4fc62934920042ae17f2dd9f5535a38 Mon Sep 17 00:00:00 2001 From: Marc-Antoine Lafreniere Date: Wed, 10 Apr 2019 01:06:24 -0400 Subject: [PATCH 4/6] Last fixes --- .../GcodeInterpreter/GcodeInterpreter.ino | 38 ++++++++++++++++--- devicesoftware/GcodeInterpreter/functions.h | 1 + 2 files changed, 33 insertions(+), 6 deletions(-) diff --git a/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino b/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino index c9d9e6b..a445d29 100644 --- a/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino +++ b/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino @@ -69,8 +69,8 @@ bool homingY = false; bool homingZ = false; int homingState = 0; -const int homeOffsetX = /*28*/48*tickFromMm; -const int homeOffsetY = /*19.5*/40*tickFromMm; +const int homeOffsetX = /*28*/50*tickFromMm; +const int homeOffsetY = /*19.5*/50*tickFromMm; const int homeOffsetZ = 2.5*tickFromMm; // Debounce timer variables @@ -90,6 +90,11 @@ bool isXSwitchPress = false; bool isYSwitchPress = false; bool isZSwitchPress = false; +// eStop +bool ledX = false; +bool ledY = false; +bool ledZ = false; + // Moving variables bool isMoving = false; bool currentMoveDone = false; @@ -97,6 +102,7 @@ bool hasFailed = false; int currentMove = 0; int nbsMovements = 0; int32_t currentPosition = 0; +int32_t movingSpeed[] = { 75, 75 }; MovingCommand commands[3]; // Initialisation : @@ -135,7 +141,9 @@ void setup() dxl_wb.writeRegister(idX, 10, 1, &X_REVERSE, &NULL_POINTER); 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); @@ -173,7 +181,10 @@ void loop() Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION); } else + { isEmegencyState = true; + ledX = true; + } } } else @@ -206,7 +217,10 @@ void loop() Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION); } else + { isEmegencyState = true; + ledY = true; + } } } else @@ -239,7 +253,10 @@ void loop() Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION); } else + { isEmegencyState = true; + ledZ = true; + } } } else @@ -329,6 +346,9 @@ void loop() homing = false; Serial.println(homingState == 3 ? "1" : "-1"); homingState = 0; + + Write(idX, movingSpeed[0], "Profile_Velocity"); + Write(idY, movingSpeed[1], "Profile_Velocity"); } resetMovingVariables(); } @@ -521,9 +541,15 @@ 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); + //Write(id, movingSpeed[id-idX], "Profile_Velocity"); } diff --git a/devicesoftware/GcodeInterpreter/functions.h b/devicesoftware/GcodeInterpreter/functions.h index e873e54..22cc75f 100644 --- a/devicesoftware/GcodeInterpreter/functions.h +++ b/devicesoftware/GcodeInterpreter/functions.h @@ -18,5 +18,6 @@ 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 From b878159a2742db29eda66010ced80a191f26cfeb Mon Sep 17 00:00:00 2001 From: Marc-Antoine Lafreniere Date: Wed, 10 Apr 2019 16:59:01 -0400 Subject: [PATCH 5/6] Build fix Fixed test errors --- devicecommunication/CommunicationMain.py | 2 +- gcodeextractor/tests/gcode/test_listToGcode.py | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/devicecommunication/CommunicationMain.py b/devicecommunication/CommunicationMain.py index d4e354a..6c4242f 100644 --- a/devicecommunication/CommunicationMain.py +++ b/devicecommunication/CommunicationMain.py @@ -78,7 +78,7 @@ def sendWithAck(gcodeCommand, timeoutCom): raise RuntimeError('Command error : ' + gcodeCommand) else: commandTimeout += 1 - if commandTimeout > timeoutCom * 10: + if commandTimeout > timeoutCom * 100: raise RuntimeError('Command not executed') 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') From ea7f3906a674c84bcc2baf3474c6c64145b49170 Mon Sep 17 00:00:00 2001 From: Marc-Antoine Lafreniere Date: Wed, 10 Apr 2019 20:49:54 -0400 Subject: [PATCH 6/6] Add header Add a header to explain the code --- .../GcodeInterpreter/GcodeInterpreter.ino | 98 +++++++++++++------ 1 file changed, 66 insertions(+), 32 deletions(-) diff --git a/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino b/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino index a445d29..40bd30b 100644 --- a/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino +++ b/devicesoftware/GcodeInterpreter/GcodeInterpreter.ino @@ -1,12 +1,56 @@ /******************************************************************************* -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" @@ -16,34 +60,31 @@ Adress for motors : 11 and 12 and 13 #define DEVICE_NAME "3" #elif defined(__OPENCR__) #define DEVICE_NAME "" -#endif +#endif #define STRING_BUF_NUM 64 #define MINTICK 0 #define MAXTICK 1048575 -const int ACCEPTABLE_RANGE[3] = { 2, 2, 3 }; - -// 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; @@ -69,8 +110,8 @@ bool homingY = false; bool homingZ = false; int homingState = 0; -const int homeOffsetX = /*28*/50*tickFromMm; -const int homeOffsetY = /*19.5*/50*tickFromMm; +const int homeOffsetX = 28*tickFromMm; +const int homeOffsetY = 19.5*tickFromMm; const int homeOffsetZ = 2.5*tickFromMm; // Debounce timer variables @@ -91,6 +132,7 @@ bool isYSwitchPress = false; bool isZSwitchPress = false; // eStop +bool isEmegencyState = false; bool ledX = false; bool ledY = false; bool ledZ = false; @@ -105,7 +147,6 @@ int32_t currentPosition = 0; int32_t movingSpeed[] = { 75, 75 }; MovingCommand commands[3]; -// Initialisation : void setup() { // Initialisation des pins : @@ -152,7 +193,6 @@ void setup() resetMovingVariables(); } -// Main Program void loop() { if(isFalling) @@ -165,6 +205,7 @@ void loop() isFalling = false; isXSwitchPress = true; + // Insert what to do if the button is press (executed on change) if(homingX) { homingX = false; @@ -201,6 +242,7 @@ void loop() isFalling = false; isYSwitchPress = true; + // Insert what to do if the button is press (executed on change) if(homingY) { homingY = false; @@ -237,6 +279,7 @@ void loop() isFalling = false; isZSwitchPress = true; + // Insert what to do if the button is press (executed on change) if(homingZ) { homingZ = false; @@ -278,6 +321,7 @@ void loop() lastTimeXRising = millis(); isRising = false; isXSwitchPress = false; + // Insert what to do if the button is release (executed on change) } } else @@ -290,6 +334,7 @@ void loop() lastTimeYRising = millis(); isRising = false; isYSwitchPress = false; + // Insert what to do if the button is release (executed on change) } } else @@ -302,6 +347,7 @@ void loop() lastTimeZRising = millis(); isRising = false; isZSwitchPress = false; + // Insert what to do if the button is release (executed on change) } } else @@ -446,17 +492,6 @@ void loop() Serial.println("1"); } - else if(words[0] == "M1") - { - 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 if(words[0] == "G90") { Serial.println("1"); @@ -549,7 +584,6 @@ void setEmergency() void changeMode(uint8_t id) { Write(id, 4, OPERATING_MODE); - //Write(id, movingSpeed[id-idX], "Profile_Velocity"); }