From baeecfb47b6f75c71f9f547a577adf6dfac3befb Mon Sep 17 00:00:00 2001 From: Marc-Antoine Lafreniere Date: Wed, 27 Feb 2019 09:52:05 -0500 Subject: [PATCH 01/17] Send all GCode command Read a gcode file and send it to the arduino --- arduinosender/CommunicationMain.py | 43 ++++++++++++++++++++++++++++++ arduinosender/__init__.py | 0 2 files changed, 43 insertions(+) create mode 100644 arduinosender/CommunicationMain.py create mode 100644 arduinosender/__init__.py diff --git a/arduinosender/CommunicationMain.py b/arduinosender/CommunicationMain.py new file mode 100644 index 0000000..1d845e7 --- /dev/null +++ b/arduinosender/CommunicationMain.py @@ -0,0 +1,43 @@ +import serial +import argparse + +from serial import SerialException + +if __name__ == "__main__": + parser = argparse.ArgumentParser(prog = 'main.py') + parser.add_argument('-g', required = True, help = 'GCode file path') + parser.add_argument('-c', required = True, help = 'Arduino com port') + parser.add_argument('-t', required = False, type = int, help = 'Communication timout') + parser.add_argument('-b', required = False, type = int, help = 'Communication baudrate') + args = parser.parse_args() + + timeoutCom = 2 + baudRate = 9600 + + if args.t: + timeoutCom = int(args.t) + if args.b: + baudRate = int(args.b) + + ser = None + try: + ser = serial.Serial(args.c, baudRate, timeout = timeoutCom) + + file = open(args.g, 'r') + lines = file.readlines() + file.close() + + for line in lines: + if not line == '\n': + ser.write(line.encode('UTF-8')) + if not ser.readline().decode('UTF-8').startswith('1'): + raise RuntimeError('Communication lost') + + ser.close() + except SerialException as errSE: + print('Serial Exception' + str(errSE)) + except RuntimeError as errRE: + print(str(errRE)) + finally: + if ser is not None: + ser.close() \ No newline at end of file diff --git a/arduinosender/__init__.py b/arduinosender/__init__.py new file mode 100644 index 0000000..e69de29 From 2ec7984da260ffbf9baf1ea75945125c218e17fd Mon Sep 17 00:00:00 2001 From: Marc-Antoine Lafreniere Date: Wed, 20 Mar 2019 13:25:30 -0400 Subject: [PATCH 02/17] #56-Delay in Arduino communication Wait for input from the arduino before sending next GCode command --- arduinosender/CommunicationMain.py | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/arduinosender/CommunicationMain.py b/arduinosender/CommunicationMain.py index 1d845e7..d7b9db0 100644 --- a/arduinosender/CommunicationMain.py +++ b/arduinosender/CommunicationMain.py @@ -1,5 +1,6 @@ import serial import argparse +from time import sleep from serial import SerialException @@ -9,20 +10,26 @@ if __name__ == "__main__": parser.add_argument('-c', required = True, help = 'Arduino com port') parser.add_argument('-t', required = False, type = int, help = 'Communication timout') parser.add_argument('-b', required = False, type = int, help = 'Communication baudrate') + parser.add_argument('-waittime', required = False, type = int, help = 'Sleep time before sending (s)') args = parser.parse_args() timeoutCom = 2 baudRate = 9600 + commandTimeout = 0 + waitTime = 1 if args.t: timeoutCom = int(args.t) if args.b: baudRate = int(args.b) + if args.waittime: + waitTime = int(args.waittime) ser = None try: ser = serial.Serial(args.c, baudRate, timeout = timeoutCom) - + sleep(waitTime) + file = open(args.g, 'r') lines = file.readlines() file.close() @@ -30,10 +37,25 @@ if __name__ == "__main__": for line in lines: if not line == '\n': ser.write(line.encode('UTF-8')) - if not ser.readline().decode('UTF-8').startswith('1'): + + if not ser.readline().decode('UTF-8').startswith('2'): raise RuntimeError('Communication lost') + + while True: + received = ser.readline().decode('UTF-8') + if not received.startswith('1') or received.startswith('-1'): + commandTimeout += 1 + if commandTimeout > timeoutCom * 10: + raise RuntimeError('Command not executed') + if received.startswith('-1'): + raise RuntimeError('Command error') + else: + break + + print('Done') ser.close() + except SerialException as errSE: print('Serial Exception' + str(errSE)) except RuntimeError as errRE: From bf1dfbaf7de2c767a116f04b2023554b1c04815b Mon Sep 17 00:00:00 2001 From: Maxime Date: Tue, 26 Mar 2019 11:53:12 -0400 Subject: [PATCH 03/17] Building of G-28 request This commit is for the implementation of the request G28, M18 and M112. G28 is for Homing, M18 is for emergency stop and M112 is for a full stop. --- .../{OpenCR_5.0.ino => GcodeInterpreter.ino} | 228 +++++++++++++++--- 1 file changed, 190 insertions(+), 38 deletions(-) rename pcbdevice/opencrcontroler/{OpenCR_5.0.ino => GcodeInterpreter.ino} (60%) diff --git a/pcbdevice/opencrcontroler/OpenCR_5.0.ino b/pcbdevice/opencrcontroler/GcodeInterpreter.ino similarity index 60% rename from pcbdevice/opencrcontroler/OpenCR_5.0.ino rename to pcbdevice/opencrcontroler/GcodeInterpreter.ino index bca6d3e..161a0ac 100644 --- a/pcbdevice/opencrcontroler/OpenCR_5.0.ino +++ b/pcbdevice/opencrcontroler/GcodeInterpreter.ino @@ -42,10 +42,6 @@ List of function creation : #include -// Gcode requests : -const int G0 = 1; -const int G90 = 2; - #if defined(__OPENCM904__) #define DEVICE_NAME "3" //Dynamixel on Serial3(USART3) <-OpenCM 485EXP #elif defined(__OPENCR__) @@ -61,6 +57,7 @@ uint8_t scan_cnt = 0; uint8_t ping_cnt = 0; const char *NULL_POINTER = NULL; +bool HOMING = 0; // Motors Propertys : uint8_t idX = 11; @@ -72,6 +69,22 @@ int XPosition = 0; int yPosition = 0; int ZPosition = 0; +// Limit Switch propertys +int XSwitchPin = 8; +int YSwitchPin = 9; +int ZSwitchPin = 10; +int EmergencySwitchPin = 2; + +// Homing variables +bool homing = 0; +bool homingX = 0; +bool homingY = 0; +bool homingZ = 0; + +int offsetX = 10; //mm +int offsetY = 10; //mm +int offsetZ = 10; //mm + // Fonctions prototypes : void Begin(uint32_t baud); void Ping(int identification); @@ -82,15 +95,28 @@ 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 EmergencyStop(); +void HomeAxe(String axe, uint8_t ID, int offset); +void LimiteSwitch(uint8_t IDX, uint8_t IDY, uint8_t IDZ, bool homing); int MovingDistance(String axe, float value_mm, uint8_t IDX, uint8_t IDY, uint8_t IDZ); +Homing(uint8_t IDX, uint8_t IDY, uint8_t IDZ); +// Initialisation : void setup() { + + // Initialisation des pins : + pinMode(XSwitchPin, INPUT_PULLUP); + pinMode(YSwitchPin, INPUT_PULLUP); + pinMode(ZSwitchPin, INPUT_PULLUP); + attachInterrupt(digitalPinToInterrupt(EmergencySwitchPin), LimiteSwitch, RISING); + Serial.begin(57600); while(!Serial); // Open a Serial Monitor -//Motor Section : +//Motor Initialisation Section : Begin((uint32_t)57600); Ping(idX); Ping(idY); @@ -110,10 +136,10 @@ void setup() Torque_on(idX); Torque_on(idY); Torque_on(idZ); - Serial.println("Ready"); + //Serial.println("Ready"); } - +// Main Program void loop() { if (Serial.available()) @@ -121,7 +147,6 @@ if (Serial.available()) String read_string = Serial.readStringUntil('\n'); //Serial.println("[CMD] : " + String(read_string)); - //String read_string = "G0 X10.2 Y4.4"; String words[] = {"", "", ""}; int start = 0; @@ -138,21 +163,63 @@ if (Serial.available()) } words[wordIndex] = read_string.substring(start, read_string.length()); - + // Absolute Move G0 + // Call exemple : G0 X100 Y200 Z10 if(words[0] == "G0") { - for(int i = 1; i < 3; i++) + Serial.println("2"); + for(int i = 1; i < wordIndex+1; i++) { float value = words[i].substring(1, words[i].length()).toFloat(); statut[i] = MovingDistance((String)words[i].charAt(0), value, idX, idY, idZ); } - if(statut[1]+statut[2]+statut[3] >= 3){ + if((statut[0] == 1 || statut[0] == 0) && (statut[1] == 1 || statut[1] == 0) && (statut[2] == 1|| statut[2] == 0)){ Serial.println("1"); } else{ Serial.println("-1"); } } + + // Homing G28 (home all axes) + // Call exemple : G28 + else if(words[0] == "G28"){ + Serial.println("2"); + Homing(idX, idY, idZ); + Serial.println("1"); + } + + // Torque OFF all M18 + // Call exemple : M18 + else if(words[0] == "M18"){ + Serial.println("2"); + EmergencyStop(); + Serial.println("1"); + } + + // Emergency state M112 + // Call exemple : M112 + else if(words[0] == "M112"){ + Serial.println("2"); + EmergencyStop(); + + for(int i=0; i<10; i++){ + Led(idX, 1); + Led(idY, 1); + Led(idZ, 1); + delay(200); + Led(idX, 0); + Led(idY, 0); + Led(idZ, 0); + delay(200); + } + Serial.println("1"); + } + + // Request not recognized + else{ + Serial.println("-1"); + } } } @@ -172,6 +239,31 @@ if (Serial.available()) // High level Functions : //######################## +int Homing(uint8_t IDX, uint8_t IDY, uint8_t IDZ){ + homing = 1; + // home Z : + homingZ = HIGH; + homingY = LOW; + homingX = LOW; + Torque_on(IDZ); + Wheel(IDZ, -50); + + // home y : + homingZ = LOW; + homingY = HIGH; + homingX = LOW; + Torque_on(IDY); + Wheel(IDZ, -100); + + // home x : + homingZ = LOW; + homingY = LOW; + homingX = HIGH; + Torque_on(IDX); + Wheel(IDZ, -100); + +} + int MovingDistance(String axe, float value_mm, uint8_t IDX, uint8_t IDY, uint8_t IDZ){ // Physicals propertys: @@ -185,43 +277,65 @@ int MovingDistance(String axe, float value_mm, uint8_t IDX, uint8_t IDY, uint8_t int32_t value = (value_mm/resMotorLinear); if(axe == "X"){ - MovingTick(IDX, value); - //Serial.println("X"); - return 1; + return MovingTick(IDX, value); } - else{ - if(axe == "Y"){ - MovingTick(IDY, (int32_t)value); - //Serial.println("Y"); - return 1; - } - else{ - if(axe == "Z"){ - MovingTick(IDZ, (int32_t)value); - //Serial.println("Z"); - return 1; - } - } + else if(axe == "Y"){ + return MovingTick(IDY, value); + } + else if(axe == "Z"){ + return MovingTick(IDZ, value); } } int MovingTick(uint8_t id, int32_t value){ String commande = "Goal_Position"; - int32_t CurrentPosition = 0; + int32_t CurrentPosition = Read(id, "Present_Position"); int MaxTick = 1048575; - int Iter = 0; + int MinTick = 0; + + bool Front_Back = 0; - Torque_on(id); - Write(id, value, commande); - - while(CurrentPosition < value-1){ - CurrentPosition = Read(id, "Present_Position"); - Iter += 1; - if(Iter >= MaxTick){ - return -1; + if(value > Read(id, "Present_Position")){ + Front_Back = 1; + } + else if(value < Read(id, "Present_Position")){ + Front_Back = 0; + } + + if((Front_Back && (CurrentPosition < MaxTick)) || (!Front_Back && (CurrentPosition > MinTick))) + { + Torque_on(id); + Write(id, value, commande); + } + else + { + Torque_off(id); + Serial.println(CurrentPosition); + return -1; + } + + // Moving to front request + if(Front_Back){ + while(CurrentPosition < value-1){ + CurrentPosition = Read(id, "Present_Position"); + if(CurrentPosition >= MaxTick){ + Torque_off(id); + return -1; + } } } - + + // Moving to back request + else { + while(CurrentPosition > value+1){ + CurrentPosition = Read(id, "Present_Position"); + if(CurrentPosition <= MinTick){ + Torque_off(id); + return -1; + } + } + } + return 1; } @@ -274,3 +388,41 @@ int32_t Read(uint8_t id, String commande){ dxl_wb.readRegister(id, commande.c_str(), &data, &NULL_POINTER); return data; } + +void Led(uint8_t ID, bool state){ + Write(ID, (uint32_t)state, "LED"); +} + +void EmergencyStop(){ + Torque_off(idX); + Torque_off(idY); + Torque_off(idZ); + Serial.println(-1); +} + +void HomeAxe(String axe, uint8_t ID, int offset){ + Torque_off(ID); + Torque_on(ID); + MovingDistance(axe, offset, idX, idY, idZ); + int homingPosition = Read(ID, "Present_Position"); + Write(ID, (uint32_t)homingPosition, "Homing_Offset"); +} + +void LimiteSwitch(){ + if(homing){ + if(homingZ == HIGH && digitalRead(ZSwitchPin) == HIGH){ + HomeAxe("Z", idZ, offsetZ); + } + else if(homingY == HIGH && digitalRead(YSwitchPin) == HIGH){ + HomeAxe("Y", idY, offsetY); + } + else if(homingX == HIGH && digitalRead(XSwitchPin) == HIGH){ + HomeAxe("X", idX, offsetX); + } + } + else{ + EmergencyStop(); + Serial.flush(); + Serial.println("-1"); + } +} From 4753c2486bd5a85d88a611176effa7ca037e4be0 Mon Sep 17 00:00:00 2001 From: Maxime Date: Wed, 27 Mar 2019 10:59:08 -0400 Subject: [PATCH 04/17] pull request review and changes. - Refactoring following pull requests. --- .../opencrcontroler/GcodeInterpreter.ino | 160 +++++++++--------- 1 file changed, 81 insertions(+), 79 deletions(-) diff --git a/pcbdevice/opencrcontroler/GcodeInterpreter.ino b/pcbdevice/opencrcontroler/GcodeInterpreter.ino index 161a0ac..b5faf92 100644 --- a/pcbdevice/opencrcontroler/GcodeInterpreter.ino +++ b/pcbdevice/opencrcontroler/GcodeInterpreter.ino @@ -57,23 +57,22 @@ uint8_t scan_cnt = 0; uint8_t ping_cnt = 0; const char *NULL_POINTER = NULL; -bool HOMING = 0; // Motors Propertys : uint8_t idX = 11; uint8_t idY = 12; uint8_t idZ = 13; -// Positions Propertys : -int XPosition = 0; -int yPosition = 0; -int ZPosition = 0; +// Mecanicals propertys: +float pouleyPitch = 2; //mm +float nbTheets = 20; +int resMotorTick = 4096; //Ticks per revolution // Limit Switch propertys -int XSwitchPin = 8; -int YSwitchPin = 9; -int ZSwitchPin = 10; -int EmergencySwitchPin = 2; +int xSwitchPin = 8; +int ySwitchPin = 9; +int zSwitchPin = 10; +int emergencySwitchPin = 2; // intrupt pin // Homing variables bool homing = 0; @@ -95,23 +94,25 @@ 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 Led(uint8_t id, bool state); void EmergencyStop(); -void HomeAxe(String axe, uint8_t ID, int offset); -void LimiteSwitch(uint8_t IDX, uint8_t IDY, uint8_t IDZ, bool homing); +void HomeAxe(uint8_t id, int offset); +void LimiteSwitch(); -int MovingDistance(String axe, float value_mm, uint8_t IDX, uint8_t IDY, uint8_t IDZ); -Homing(uint8_t IDX, uint8_t IDY, uint8_t IDZ); +int mmToTick(float value_mm, float pouleyPitch, float nbTheets, int resMotorTick); +int MovingTick(uint8_t id, int32_t value); +int MovingDistance(float value_mm, uint8_t id); +int Homing(); // Initialisation : void setup() { // Initialisation des pins : - pinMode(XSwitchPin, INPUT_PULLUP); - pinMode(YSwitchPin, INPUT_PULLUP); - pinMode(ZSwitchPin, INPUT_PULLUP); - attachInterrupt(digitalPinToInterrupt(EmergencySwitchPin), LimiteSwitch, RISING); + pinMode(xSwitchPin, INPUT_PULLUP); + pinMode(ySwitchPin, INPUT_PULLUP); + pinMode(zSwitchPin, INPUT_PULLUP); + attachInterrupt(digitalPinToInterrupt(emergencySwitchPin), LimiteSwitch, RISING); Serial.begin(57600); while(!Serial); // Open a Serial Monitor @@ -136,7 +137,6 @@ void setup() Torque_on(idX); Torque_on(idY); Torque_on(idZ); - //Serial.println("Ready"); } // Main Program @@ -170,8 +170,28 @@ if (Serial.available()) Serial.println("2"); for(int i = 1; i < wordIndex+1; i++) { - float value = words[i].substring(1, words[i].length()).toFloat(); - statut[i] = MovingDistance((String)words[i].charAt(0), value, idX, idY, idZ); + float value = words[i].substring(1, words[i].length()).toFloat(); + + switch ((char)words[i].charAt(0)){ + + case 'X' : + statut[i] = MovingDistance(value, idX); + break; + + case 'Y' : + statut[i] = MovingDistance(value, idY); + break; + + case 'Z' : + statut[i] = MovingDistance(value, idZ); + break; + + default : + Serial.println("-1"); + break; + } + + } if((statut[0] == 1 || statut[0] == 0) && (statut[1] == 1 || statut[1] == 0) && (statut[2] == 1|| statut[2] == 0)){ Serial.println("1"); @@ -185,8 +205,7 @@ if (Serial.available()) // Call exemple : G28 else if(words[0] == "G28"){ Serial.println("2"); - Homing(idX, idY, idZ); - Serial.println("1"); + Serial.println(Homing()); } // Torque OFF all M18 @@ -231,60 +250,43 @@ if (Serial.available()) // Functions of Robotics Engeneering UdeS : //--------------------------------------------------------------------------------------------------------------------------------------------------------------------- -//################## -// Gcode Functions : -//################## - //######################## // High level Functions : //######################## -int Homing(uint8_t IDX, uint8_t IDY, uint8_t IDZ){ +int Homing(){ homing = 1; // home Z : - homingZ = HIGH; - homingY = LOW; - homingX = LOW; - Torque_on(IDZ); - Wheel(IDZ, -50); + homingZ = true; + homingY = false; + homingX = false; + Torque_on(idZ); + Wheel(idZ, -50); // home y : - homingZ = LOW; - homingY = HIGH; - homingX = LOW; - Torque_on(IDY); - Wheel(IDZ, -100); + homingZ = false; + homingY = true; + homingX = false; + Torque_on(idY); + Wheel(idY, -100); // home x : - homingZ = LOW; - homingY = LOW; - homingX = HIGH; - Torque_on(IDX); - Wheel(IDZ, -100); - + homingZ = false; + homingY = false; + homingX = true; + Torque_on(idX); + Wheel(idX, -100); + + return 1; } -int MovingDistance(String axe, float value_mm, uint8_t IDX, uint8_t IDY, uint8_t IDZ){ +int MovingDistance(float value_mm, uint8_t id){ + return MovingTick(id, mmToTick(value_mm, pouleyPitch, nbTheets, resMotorTick)); +} - // Physicals propertys: - float pouleyPitch = 2; //mm - float nbTheets = 20; - float pouleyCircumference = pouleyPitch*nbTheets; //mm - float pouleyRay = pouleyCircumference/6.2832; //mm - int resMotorTick = 4096; //Ticks per revolution - float resMotorLinear = pouleyCircumference/resMotorTick; //mm - - int32_t value = (value_mm/resMotorLinear); - - if(axe == "X"){ - return MovingTick(IDX, value); - } - else if(axe == "Y"){ - return MovingTick(IDY, value); - } - else if(axe == "Z"){ - return MovingTick(IDZ, value); - } +int mmToTick(float value_mm, float pouleyPitch, float nbTheets, int resMotorTick){ + int32_t value = ((value_mm/(pouleyPitch*nbTheets))/resMotorTick); + return value; } int MovingTick(uint8_t id, int32_t value){ @@ -389,8 +391,8 @@ int32_t Read(uint8_t id, String commande){ return data; } -void Led(uint8_t ID, bool state){ - Write(ID, (uint32_t)state, "LED"); +void Led(uint8_t id, bool state){ + Write(id, (uint32_t)state, "LED"); } void EmergencyStop(){ @@ -400,24 +402,24 @@ void EmergencyStop(){ Serial.println(-1); } -void HomeAxe(String axe, uint8_t ID, int offset){ - Torque_off(ID); - Torque_on(ID); - MovingDistance(axe, offset, idX, idY, idZ); - int homingPosition = Read(ID, "Present_Position"); - Write(ID, (uint32_t)homingPosition, "Homing_Offset"); +void HomeAxe(uint8_t id, int offset){ + Torque_off(id); + Torque_on(id); + MovingDistance(offset, id); + int homingPosition = Read(id, "Present_Position"); + Write(id, (uint32_t)homingPosition, "Homing_Offset"); } void LimiteSwitch(){ if(homing){ - if(homingZ == HIGH && digitalRead(ZSwitchPin) == HIGH){ - HomeAxe("Z", idZ, offsetZ); + if(homingZ == HIGH && digitalRead(zSwitchPin) == HIGH){ + HomeAxe(idZ, offsetZ); } - else if(homingY == HIGH && digitalRead(YSwitchPin) == HIGH){ - HomeAxe("Y", idY, offsetY); + else if(homingY == HIGH && digitalRead(ySwitchPin) == HIGH){ + HomeAxe(idY, offsetY); } - else if(homingX == HIGH && digitalRead(XSwitchPin) == HIGH){ - HomeAxe("X", idX, offsetX); + else if(homingX == HIGH && digitalRead(xSwitchPin) == HIGH){ + HomeAxe(idX, offsetX); } } else{ @@ -425,4 +427,4 @@ void LimiteSwitch(){ Serial.flush(); Serial.println("-1"); } -} +} \ No newline at end of file From 3f0e4080a47ddfd71af655f4ac0be4258c997e5d Mon Sep 17 00:00:00 2001 From: Marc-Antoine Lafreniere Date: Wed, 27 Mar 2019 11:49:03 -0400 Subject: [PATCH 05/17] #60 Create UI to control the device Create a UI to control the device in manual mode and auto mode --- arduinosender/CommunicationMain.py | 198 +++++++++++++++++++++++++++++ 1 file changed, 198 insertions(+) create mode 100644 arduinosender/CommunicationMain.py diff --git a/arduinosender/CommunicationMain.py b/arduinosender/CommunicationMain.py new file mode 100644 index 0000000..bc4622d --- /dev/null +++ b/arduinosender/CommunicationMain.py @@ -0,0 +1,198 @@ +from serial import Serial +from serial import SerialException + +from time import sleep +from tkinter import * +from tkinter.ttk import Notebook + +global serial + +class LabelWithEntry: + def __init__(self, parent, labelText, padXLeft = (0, 0), padYLeft = (0, 0), padXRight = (0, 0), padYRight = (0, 0)): + frame = Frame(parent) + frame.pack(side = TOP, fill = BOTH, expand = 1) + self.label = Label(frame, text = labelText) + self.label.pack(side = LEFT, padx = padXLeft, pady = padYLeft) + self.entry = Entry(frame) + self.entry.pack(side = RIGHT, padx = padXRight, pady = padYRight) + + def get(self): + return self.entry.get() + + def setText(self, text): + self.entry.insert(END, text) + +def openSerial(com, baudRate, communicationTimeout, waitTime): + global serial + + try: + serial = Serial(com, int(baudRate), timeout = int(communicationTimeout)) + sleep(int(waitTime)) + except SerialException as errSE: + raise errSE + +def closeSerial(): + global serial + + try: + serial.close() + except SerialException as errSE: + raise errSE + +def readFile(filePath): + file = open(filePath, 'r') + lines = file.readlines() + file.close() + + return lines + +def sendAllLines(lines, timeoutCom): + global serial + + for line in lines: + if not line == '\n': + sendWithAck(line, timeoutCom) + +def sendWithAck(gcodeCommand, timeoutCom): + global serial + commandTimeout = 0 + + serial.write(gcodeCommand.encode('UTF-8')) + if not serial.readline().decode('UTF-8').startswith('2'): + raise RuntimeError('Communication lost') + while True: + received = serial.readline().decode('UTF-8') + if not received.startswith('1') or received.startswith('-1'): + commandTimeout += 1 + if commandTimeout > timeoutCom * 10: + raise RuntimeError('Command not executed') + if received.startswith('-1'): + raise RuntimeError('Command error') + else: + break + + +def sendFileToPort(comPort, baudRate, communicationTimeout, waitTime, gcodePath): + global serial + + try: + openSerial(comPort, baudRate, communicationTimeout, waitTime) + lines = readFile(gcodePath) + sendAllLines(lines, communicationTimeout) + closeSerial() + except SerialException as errSE: + print('Serial Exception' + str(errSE)) + except RuntimeError as errRE: + print(str(errRE)) + finally: + if serial is not None: + closeSerial() + +def callWithParameters(comPortET, communicationTimeoutET, communicationBaudrateET, waitTimeET, gcodePathET): + timeoutCom = 2 + baudRate = 9600 + waitTime = 1 + + if communicationTimeoutET.get() != '': + timeoutCom = int(communicationTimeoutET.get()) + if communicationBaudrateET.get() != '': + baudRate = int(communicationBaudrateET.get()) + if waitTimeET.get() != '': + waitTime = int(waitTimeET.get()) + + sendFileToPort(comPortET.get(), baudRate, timeoutCom, waitTime, gcodePathET.get()) + +def sendX(mov, timeoutCom): + sendWithAck('G0 X' + str(mov), timeoutCom) + +def sendY(mov, timeoutCom): + sendWithAck('G0 Y' + str(mov), timeoutCom) + +def sendZ(mov, timeoutCom): + sendWithAck('G0 Z' + str(mov), timeoutCom) + +def sendHome(timeoutCom): + sendWithAck('G28', timeoutCom) + +def sendAbsPosition(timeoutCom): + sendWithAck('G90', timeoutCom) + +def sendRelativePosition(timeoutCom): + sendWithAck('G91', timeoutCom) + + +if __name__ == "__main__": + mainWindow = Tk() + mainWindow.title('Hello World!') + tabControl = Notebook(mainWindow) + tabAuto = Frame(tabControl) + tabManual = Frame(tabControl) + + # Frames + autoMainFrame = Frame(tabAuto, width = 300, height = 100) + autoMainFrame.pack(fill = None, expand = False) + manualMainFrame = Frame(tabManual, width = 300, height = 100) + manualMainFrame.pack(fill = None, expand = False) + + # Auto mode + padding = 20 + gcodeEntry = LabelWithEntry(autoMainFrame, 'GCode file', padXLeft = (0, padding)) + comEntry = LabelWithEntry(autoMainFrame, 'COM port', padXLeft = (0, padding)) + baudrateEntry = LabelWithEntry(autoMainFrame, 'Baudrate', padXLeft = (0, padding)) + baudrateEntry.setText('9600') + + comTimoutEntry = LabelWithEntry(autoMainFrame, 'Com timout (s)', padXLeft = (0, padding)) + comTimoutEntry.setText('1') + + sleepEntry = LabelWithEntry(autoMainFrame, 'Sleep init (s)', padXLeft = (0, padding)) + sleepEntry.setText('1') + + Button(autoMainFrame, text = 'Start', + command = lambda: callWithParameters(comEntry, baudrateEntry, comTimoutEntry, sleepEntry, gcodeEntry)) \ + .pack(side = BOTTOM, fill = BOTH, expand = 1, pady = (15, 0)) + + # Manual mode + buttonsFrame = Frame(manualMainFrame) + buttonsFrame.pack(side = TOP) + + openButton = Button(buttonsFrame, text = 'Open', command = lambda: openSerial(comEntry.get(), int(baudrateEntry.get()), int(comTimoutEntry.get()), int(sleepEntry.get()))) + openButton.grid(row = 0, column = 0) + closeButton = Button(buttonsFrame, text = 'Close', command = lambda: closeSerial()) + closeButton.grid(row = 0, column = 2) + + sendXP5Button = Button(buttonsFrame, text = '+5X', command = lambda: sendX(5, int(comTimoutEntry.get()))) + sendXP5Button.grid(row = 2, column = 2, padx = 5) + sendXM5Button = Button(buttonsFrame, text = '-5X', command = lambda: sendX(-5, int(comTimoutEntry.get()))) + sendXM5Button.grid(row = 2, column = 0, padx = 5) + + sendYP5Button = Button(buttonsFrame, text = '+5Y', command = lambda: sendY(5, int(comTimoutEntry.get()))) + sendYP5Button.grid(row = 1, column = 1, pady = 5) + sendYM5Button = Button(buttonsFrame, text = '-5Y', command = lambda: sendY(-5, int(comTimoutEntry.get()))) + sendYM5Button.grid(row = 3, column = 1, pady = 5) + + sendHomeButton = Button(buttonsFrame, text = 'Home', command = lambda: sendHome(int(comTimoutEntry.get()))) + sendHomeButton.grid(row = 2, column = 1) + + sendZP5Button = Button(buttonsFrame, text = '+5Z', command = lambda: sendZ(5, int(comTimoutEntry.get()))) + sendZP5Button.grid(row = 3, column = 2) + sendZM5Button = Button(buttonsFrame, text = '-5Z', command = lambda: sendZ(-5, int(comTimoutEntry.get()))) + sendZM5Button.grid(row = 1, column = 2) + + sendZP5Button = Button(buttonsFrame, text = 'G90', command = lambda: sendAbsPosition(int(comTimoutEntry.get()))) + sendZP5Button.grid(row = 1, column = 0) + sendZM5Button = Button(buttonsFrame, text = 'G91', command = lambda: sendRelativePosition(int(comTimoutEntry.get()))) + sendZM5Button.grid(row = 3, column = 0) + + customCommandFrame = Frame(manualMainFrame) + customCommandFrame.pack(side = BOTTOM, fill = BOTH, expand = 1) + + customCommand = Entry(customCommandFrame) + customCommand.pack(side = LEFT, fill = BOTH, expand = 1) + Button(customCommandFrame, text = 'Send', command = lambda: sendWithAck(customCommand.get(), int(comTimoutEntry.get()))).pack(side = RIGHT) + + tabControl.add(tabAuto, text = 'Auto') + tabControl.add(tabManual, text = 'Manual') + tabControl.pack(expand = 1, fill = BOTH) + mainWindow.mainloop() + + closeSerial() \ No newline at end of file From 4867ccb6265c054d0110a806edecece91a16f977 Mon Sep 17 00:00:00 2001 From: Marc-Antoine Lafreniere Date: Thu, 4 Apr 2019 11:02:51 -0400 Subject: [PATCH 06/17] Add error code reviewed with @IanLalonde --- arduinosender/CommunicationMain.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/arduinosender/CommunicationMain.py b/arduinosender/CommunicationMain.py index bc4622d..c66ec2f 100644 --- a/arduinosender/CommunicationMain.py +++ b/arduinosender/CommunicationMain.py @@ -68,6 +68,8 @@ def sendWithAck(gcodeCommand, timeoutCom): raise RuntimeError('Command not executed') if received.startswith('-1'): raise RuntimeError('Command error') + if received.startswith('-2'): + raise RuntimeError('Device error, please reset the device') else: break From ad42d74f1aadc0fff97a357c6fbbcc7b7c0617bd Mon Sep 17 00:00:00 2001 From: IanLalonde <46567178+IanLalonde@users.noreply.github.com> Date: Thu, 4 Apr 2019 12:25:42 -0400 Subject: [PATCH 07/17] Create README.md --- pcbdevice/README.md | 43 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 pcbdevice/README.md diff --git a/pcbdevice/README.md b/pcbdevice/README.md new file mode 100644 index 0000000..2feefed --- /dev/null +++ b/pcbdevice/README.md @@ -0,0 +1,43 @@ +# PCB Device + +This program transforms a pcb drawing into a gcode + +## Getting started + +To use this program, you need to launch main.py with the proper parameters or launch UI.py in /UI/ and enter the parameters in the boxes. +The PCB drawing your are using needs to be of format *.pbm , +there are plenty of converters online to change your file format, such as [this converter we used](https://convertio.co/fr/pdf-pbm/) + +### Prerequisites + +Since the program is coded in python, you need to install python, we used python 3.7. +You can use an IDE or cmd to launch the program or the UI + +### Using the program + +After downloading the project, +You can launch UI.py in S4-P3-Projet/pcbdevice/UI/ for the user interface of the program. + +Enter the parameters in the proper boxes: + +''' +* Pcb drawing file path you want to convert into gcode, this file needs to be of type *.pbm +* Gcode file you want to save with the path where you want to save it. + If the file doesn't exist, a new file will be created, file type need to be *.gcode +* If the pbm file is of type binary or ascii. To find the type, you can open the file in a text editor, + ascii files will start with P1 while binary with P4. Also, birary type will contain unreadable characters. +* Width dimension of your pcb, units are entered later, +* Height dimension of your pcb, must be of the same units as width +* Radius of the tool you are using, units must be in mm. +* Units type for the width and height +''' +Once all parameters are entered, you can click on the **execute program* button. +If everything is good, you should read **SUCCESS* on the bottom of the UI, else and error code should appear. + +## People in this project + +**Ian Lalonde* +**Marc-Antoine Lafrenière* +**Maxime Laporte* +**Guillaume Pépin* +**Louis-Philippe Baillargeon* From 15fa1efb775b73f944956693b69221eb2f5ccaa2 Mon Sep 17 00:00:00 2001 From: IanLalonde <46567178+IanLalonde@users.noreply.github.com> Date: Thu, 4 Apr 2019 12:30:37 -0400 Subject: [PATCH 08/17] Update README.md --- pcbdevice/README.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/pcbdevice/README.md b/pcbdevice/README.md index 2feefed..f2d6ef4 100644 --- a/pcbdevice/README.md +++ b/pcbdevice/README.md @@ -20,7 +20,7 @@ You can launch UI.py in S4-P3-Projet/pcbdevice/UI/ for the user interface of the Enter the parameters in the proper boxes: -''' +``` * Pcb drawing file path you want to convert into gcode, this file needs to be of type *.pbm * Gcode file you want to save with the path where you want to save it. If the file doesn't exist, a new file will be created, file type need to be *.gcode @@ -30,14 +30,14 @@ Enter the parameters in the proper boxes: * Height dimension of your pcb, must be of the same units as width * Radius of the tool you are using, units must be in mm. * Units type for the width and height -''' -Once all parameters are entered, you can click on the **execute program* button. -If everything is good, you should read **SUCCESS* on the bottom of the UI, else and error code should appear. +``` +Once all parameters are entered, you can click on the **execute program** button. +If everything is good, you should read **SUCCESS** on the bottom of the UI, else and error code should appear. ## People in this project -**Ian Lalonde* -**Marc-Antoine Lafrenière* -**Maxime Laporte* -**Guillaume Pépin* -**Louis-Philippe Baillargeon* +**Ian Lalonde** +**Marc-Antoine Lafrenière** +**Maxime Laporte** +**Guillaume Pépin** +**Louis-Philippe Baillargeon** From 5a8d1b52bc85a4e6a3fda7911235fa027d85db78 Mon Sep 17 00:00:00 2001 From: IanLalonde <46567178+IanLalonde@users.noreply.github.com> Date: Thu, 4 Apr 2019 12:31:40 -0400 Subject: [PATCH 09/17] Update README.md --- pcbdevice/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/pcbdevice/README.md b/pcbdevice/README.md index f2d6ef4..0c1e612 100644 --- a/pcbdevice/README.md +++ b/pcbdevice/README.md @@ -36,8 +36,8 @@ If everything is good, you should read **SUCCESS** on the bottom of the UI, else ## People in this project -**Ian Lalonde** -**Marc-Antoine Lafrenière** -**Maxime Laporte** -**Guillaume Pépin** -**Louis-Philippe Baillargeon** +**Ian Lalonde** \n +**Marc-Antoine Lafrenière** \n +**Maxime Laporte** \n +**Guillaume Pépin** \n +**Louis-Philippe Baillargeon** \n From b5bf5b0c1ce69c00dc4c00bebebff675afd9ecc9 Mon Sep 17 00:00:00 2001 From: IanLalonde <46567178+IanLalonde@users.noreply.github.com> Date: Thu, 4 Apr 2019 12:33:03 -0400 Subject: [PATCH 10/17] Update README.md --- pcbdevice/README.md | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/pcbdevice/README.md b/pcbdevice/README.md index 0c1e612..6e15d27 100644 --- a/pcbdevice/README.md +++ b/pcbdevice/README.md @@ -21,23 +21,27 @@ You can launch UI.py in S4-P3-Projet/pcbdevice/UI/ for the user interface of the Enter the parameters in the proper boxes: ``` -* Pcb drawing file path you want to convert into gcode, this file needs to be of type *.pbm -* Gcode file you want to save with the path where you want to save it. +- Pcb drawing file path you want to convert into gcode, this file needs to be of type *.pbm +- Gcode file you want to save with the path where you want to save it. If the file doesn't exist, a new file will be created, file type need to be *.gcode -* If the pbm file is of type binary or ascii. To find the type, you can open the file in a text editor, +- If the pbm file is of type binary or ascii. To find the type, you can open the file in a text editor, ascii files will start with P1 while binary with P4. Also, birary type will contain unreadable characters. -* Width dimension of your pcb, units are entered later, -* Height dimension of your pcb, must be of the same units as width -* Radius of the tool you are using, units must be in mm. -* Units type for the width and height +- Width dimension of your pcb, units are entered later, +- Height dimension of your pcb, must be of the same units as width +- Radius of the tool you are using, units must be in mm. +- Units type for the width and height ``` Once all parameters are entered, you can click on the **execute program** button. If everything is good, you should read **SUCCESS** on the bottom of the UI, else and error code should appear. ## People in this project -**Ian Lalonde** \n -**Marc-Antoine Lafrenière** \n -**Maxime Laporte** \n -**Guillaume Pépin** \n -**Louis-Philippe Baillargeon** \n +**Ian Lalonde** + +**Marc-Antoine Lafrenière** + +**Maxime Laporte** + +**Guillaume Pépin** + +**Louis-Philippe Baillargeon** From b12875227358bd2d2967e5de6ea7175a24b49491 Mon Sep 17 00:00:00 2001 From: IanLalonde <46567178+IanLalonde@users.noreply.github.com> Date: Thu, 4 Apr 2019 12:34:19 -0400 Subject: [PATCH 11/17] Update README.md --- pcbdevice/README.md | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/pcbdevice/README.md b/pcbdevice/README.md index 6e15d27..4758fe3 100644 --- a/pcbdevice/README.md +++ b/pcbdevice/README.md @@ -22,10 +22,8 @@ Enter the parameters in the proper boxes: ``` - Pcb drawing file path you want to convert into gcode, this file needs to be of type *.pbm -- Gcode file you want to save with the path where you want to save it. - If the file doesn't exist, a new file will be created, file type need to be *.gcode -- If the pbm file is of type binary or ascii. To find the type, you can open the file in a text editor, - ascii files will start with P1 while binary with P4. Also, birary type will contain unreadable characters. +- Gcode file you want to save with the path where you want to save it.If the file doesn't exist, a new file will be created, file type need to be *.gcode +- If the pbm file is of type binary or ascii. To find the type, you can open the file in a text editor,ascii files will start with P1 while binary with P4. Also, birary type will contain unreadable characters. - Width dimension of your pcb, units are entered later, - Height dimension of your pcb, must be of the same units as width - Radius of the tool you are using, units must be in mm. From e92c46ed83e3aee4f1c2cf6c9581f8452cac1d6c Mon Sep 17 00:00:00 2001 From: IanLalonde <46567178+IanLalonde@users.noreply.github.com> Date: Thu, 4 Apr 2019 12:36:37 -0400 Subject: [PATCH 12/17] Update README.md --- pcbdevice/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pcbdevice/README.md b/pcbdevice/README.md index 4758fe3..753e5d9 100644 --- a/pcbdevice/README.md +++ b/pcbdevice/README.md @@ -23,7 +23,8 @@ Enter the parameters in the proper boxes: ``` - Pcb drawing file path you want to convert into gcode, this file needs to be of type *.pbm - Gcode file you want to save with the path where you want to save it.If the file doesn't exist, a new file will be created, file type need to be *.gcode -- If the pbm file is of type binary or ascii. To find the type, you can open the file in a text editor,ascii files will start with P1 while binary with P4. Also, birary type will contain unreadable characters. +- If the pbm file is of type binary or ascii. To find the type, you can open the file in a text editor, + ascii files will start with P1 while binary with P4. Also, birary type will contain unreadable characters. - Width dimension of your pcb, units are entered later, - Height dimension of your pcb, must be of the same units as width - Radius of the tool you are using, units must be in mm. From ddf75db254ae5a22798b839be3d72370ffadadc0 Mon Sep 17 00:00:00 2001 From: IanLalonde <46567178+IanLalonde@users.noreply.github.com> Date: Thu, 4 Apr 2019 12:38:26 -0400 Subject: [PATCH 13/17] Update README.md --- pcbdevice/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pcbdevice/README.md b/pcbdevice/README.md index 753e5d9..6e15d27 100644 --- a/pcbdevice/README.md +++ b/pcbdevice/README.md @@ -22,7 +22,8 @@ Enter the parameters in the proper boxes: ``` - Pcb drawing file path you want to convert into gcode, this file needs to be of type *.pbm -- Gcode file you want to save with the path where you want to save it.If the file doesn't exist, a new file will be created, file type need to be *.gcode +- Gcode file you want to save with the path where you want to save it. + If the file doesn't exist, a new file will be created, file type need to be *.gcode - If the pbm file is of type binary or ascii. To find the type, you can open the file in a text editor, ascii files will start with P1 while binary with P4. Also, birary type will contain unreadable characters. - Width dimension of your pcb, units are entered later, From 7f0756ec0f2fe78a66a47110c019e917e13cbd2c Mon Sep 17 00:00:00 2001 From: IanLalonde <46567178+IanLalonde@users.noreply.github.com> Date: Thu, 4 Apr 2019 12:39:07 -0400 Subject: [PATCH 14/17] Update README.md --- pcbdevice/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pcbdevice/README.md b/pcbdevice/README.md index 6e15d27..72ebd77 100644 --- a/pcbdevice/README.md +++ b/pcbdevice/README.md @@ -1,17 +1,17 @@ # PCB Device -This program transforms a pcb drawing into a gcode +This program transforms a pcb drawing into a gcode. ## Getting started To use this program, you need to launch main.py with the proper parameters or launch UI.py in /UI/ and enter the parameters in the boxes. The PCB drawing your are using needs to be of format *.pbm , -there are plenty of converters online to change your file format, such as [this converter we used](https://convertio.co/fr/pdf-pbm/) +there are plenty of converters online to change your file format, such as [this converter we used](https://convertio.co/fr/pdf-pbm/). ### Prerequisites Since the program is coded in python, you need to install python, we used python 3.7. -You can use an IDE or cmd to launch the program or the UI +You can use an IDE or cmd to launch the program or the UI. ### Using the program From 98c01ad36e9c720e5d975ae22b97a171d509b4ab Mon Sep 17 00:00:00 2001 From: Marc-Antoine Lafreniere Date: Sat, 6 Apr 2019 11:17:55 -0400 Subject: [PATCH 15/17] Refactor and bug fixes Fix homing issue Add motor direction as constant --- .../opencrcontroler/GcodeInterpreter.ino | 430 ------------------ .../GcodeInterpreter/GcodeInterpreter.ino | 389 ++++++++++++++++ 2 files changed, 389 insertions(+), 430 deletions(-) delete mode 100644 pcbdevice/opencrcontroler/GcodeInterpreter.ino create mode 100644 pcbdevice/opencrcontroler/GcodeInterpreter/GcodeInterpreter.ino diff --git a/pcbdevice/opencrcontroler/GcodeInterpreter.ino b/pcbdevice/opencrcontroler/GcodeInterpreter.ino deleted file mode 100644 index b5faf92..0000000 --- a/pcbdevice/opencrcontroler/GcodeInterpreter.ino +++ /dev/null @@ -1,430 +0,0 @@ -/******************************************************************************* -Titre : OpenCR -Date : 6 février 2019 -Auteur : Maxime Desmarais-Laporte -Descritpion : - -Specifications : -Baud for motors : 57600 b/s -Adress for motors : 11 and 12 - -List of function creation : - - Functions |Function State / test / Implementation / test - help | - begin (BAUD) |Done / yes - scan (RANGE) |Done / yes - ping (ID) |Done / yes - control_table (ID) | - id (ID) (NEW_ID) | - baud (ID) (NEW_BAUD) | - torque_on (ID) | - torque_off (ID) | - joint (ID) (GOAL_POSITION) | - wheel (ID) (GOAL_VELOCITY) |Done / yes - write (ID) (ADDRESS_NAME) (DATA) | - read (ID) (ADDRESS_NAME) | - sync_write_handler (Ref_ID) (ADDRESS_NAME) | - sync_write (ID_1) (ID_2) (HANDLER_INDEX) (PARAM_1) (PARAM_2)| - sync_read_handler (Ref_ID) (ADDRESS_NAME) | - sync_read (ID_1) (ID_2) (HANDLER_INDEX) | - bulk_write_handler | - bulk_write_param (ID) (ADDRESS_NAME) (PARAM) | - bulk_write | - bulk_read_handler | - bulk_read_param (ID) (ADDRESS_NAME) | - bulk_read | - reboot (ID) | - reset (ID) | - end | - -*******************************************************************************/ - -#include - -#if defined(__OPENCM904__) - #define DEVICE_NAME "3" //Dynamixel on Serial3(USART3) <-OpenCM 485EXP -#elif defined(__OPENCR__) - #define DEVICE_NAME "" -#endif - -#define STRING_BUF_NUM 64 -String cmd[STRING_BUF_NUM]; - -DynamixelWorkbench dxl_wb; -uint8_t get_id[16]; -uint8_t scan_cnt = 0; -uint8_t ping_cnt = 0; - -const char *NULL_POINTER = NULL; - -// Motors Propertys : -uint8_t idX = 11; -uint8_t idY = 12; -uint8_t idZ = 13; - -// Mecanicals propertys: -float pouleyPitch = 2; //mm -float nbTheets = 20; -int resMotorTick = 4096; //Ticks per revolution - -// Limit Switch propertys -int xSwitchPin = 8; -int ySwitchPin = 9; -int zSwitchPin = 10; -int emergencySwitchPin = 2; // intrupt pin - -// Homing variables -bool homing = 0; -bool homingX = 0; -bool homingY = 0; -bool homingZ = 0; - -int offsetX = 10; //mm -int offsetY = 10; //mm -int offsetZ = 10; //mm - -// 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 EmergencyStop(); -void HomeAxe(uint8_t id, int offset); -void LimiteSwitch(); - -int mmToTick(float value_mm, float pouleyPitch, float nbTheets, int resMotorTick); -int MovingTick(uint8_t id, int32_t value); -int MovingDistance(float value_mm, uint8_t id); -int Homing(); - -// Initialisation : -void setup() -{ - - // Initialisation des pins : - pinMode(xSwitchPin, INPUT_PULLUP); - pinMode(ySwitchPin, INPUT_PULLUP); - pinMode(zSwitchPin, INPUT_PULLUP); - attachInterrupt(digitalPinToInterrupt(emergencySwitchPin), LimiteSwitch, RISING); - - Serial.begin(57600); - while(!Serial); // Open a Serial Monitor - -//Motor Initialisation Section : - Begin((uint32_t)57600); - Ping(idX); - Ping(idY); - Ping(idZ); - Scan(); - - delay(1000); - - Torque_off(idX); - Torque_off(idY); - Torque_off(idZ); - - Write(idX, (uint32_t)4,"Operating_Mode"); - Write(idY, (uint32_t)4,"Operating_Mode"); - Write(idZ, (uint32_t)4,"Operating_Mode"); - - Torque_on(idX); - Torque_on(idY); - Torque_on(idZ); -} - -// Main Program -void loop() { - -if (Serial.available()) - { - String read_string = Serial.readStringUntil('\n'); - //Serial.println("[CMD] : " + String(read_string)); - - String words[] = {"", "", ""}; - - int start = 0; - int wordIndex = 0; - int statut[3] = {0, 0, 0}; - - for(int i = 1; i < read_string.length(); i++) - { - if(read_string.charAt(i) == ' ') - { - words[wordIndex++] = read_string.substring(start, i); - start = i+1; - } - } - words[wordIndex] = read_string.substring(start, read_string.length()); - - // Absolute Move G0 - // Call exemple : G0 X100 Y200 Z10 - if(words[0] == "G0") - { - Serial.println("2"); - for(int i = 1; i < wordIndex+1; i++) - { - float value = words[i].substring(1, words[i].length()).toFloat(); - - switch ((char)words[i].charAt(0)){ - - case 'X' : - statut[i] = MovingDistance(value, idX); - break; - - case 'Y' : - statut[i] = MovingDistance(value, idY); - break; - - case 'Z' : - statut[i] = MovingDistance(value, idZ); - break; - - default : - Serial.println("-1"); - break; - } - - - } - if((statut[0] == 1 || statut[0] == 0) && (statut[1] == 1 || statut[1] == 0) && (statut[2] == 1|| statut[2] == 0)){ - Serial.println("1"); - } - else{ - Serial.println("-1"); - } - } - - // Homing G28 (home all axes) - // Call exemple : G28 - else if(words[0] == "G28"){ - Serial.println("2"); - Serial.println(Homing()); - } - - // Torque OFF all M18 - // Call exemple : M18 - else if(words[0] == "M18"){ - Serial.println("2"); - EmergencyStop(); - Serial.println("1"); - } - - // Emergency state M112 - // Call exemple : M112 - else if(words[0] == "M112"){ - Serial.println("2"); - EmergencyStop(); - - for(int i=0; i<10; i++){ - Led(idX, 1); - Led(idY, 1); - Led(idZ, 1); - delay(200); - Led(idX, 0); - Led(idY, 0); - Led(idZ, 0); - delay(200); - } - Serial.println("1"); - } - - // Request not recognized - else{ - Serial.println("-1"); - } - } -} - -//--------------------------------------------------------------------------------------------------------------------------------------------------------------------- -// Functions : -//--------------------------------------------------------------------------------------------------------------------------------------------------------------------- - -//--------------------------------------------------------------------------------------------------------------------------------------------------------------------- -// Functions of Robotics Engeneering UdeS : -//--------------------------------------------------------------------------------------------------------------------------------------------------------------------- - -//######################## -// High level Functions : -//######################## - -int Homing(){ - homing = 1; - // home Z : - homingZ = true; - homingY = false; - homingX = false; - Torque_on(idZ); - Wheel(idZ, -50); - - // home y : - homingZ = false; - homingY = true; - homingX = false; - Torque_on(idY); - Wheel(idY, -100); - - // home x : - homingZ = false; - homingY = false; - homingX = true; - Torque_on(idX); - Wheel(idX, -100); - - return 1; -} - -int MovingDistance(float value_mm, uint8_t id){ - return MovingTick(id, mmToTick(value_mm, pouleyPitch, nbTheets, resMotorTick)); -} - -int mmToTick(float value_mm, float pouleyPitch, float nbTheets, int resMotorTick){ - int32_t value = ((value_mm/(pouleyPitch*nbTheets))/resMotorTick); - return value; -} - -int MovingTick(uint8_t id, int32_t value){ - String commande = "Goal_Position"; - int32_t CurrentPosition = Read(id, "Present_Position"); - int MaxTick = 1048575; - int MinTick = 0; - - bool Front_Back = 0; - - if(value > Read(id, "Present_Position")){ - Front_Back = 1; - } - else if(value < Read(id, "Present_Position")){ - Front_Back = 0; - } - - if((Front_Back && (CurrentPosition < MaxTick)) || (!Front_Back && (CurrentPosition > MinTick))) - { - Torque_on(id); - Write(id, value, commande); - } - else - { - Torque_off(id); - Serial.println(CurrentPosition); - return -1; - } - - // Moving to front request - if(Front_Back){ - while(CurrentPosition < value-1){ - CurrentPosition = Read(id, "Present_Position"); - if(CurrentPosition >= MaxTick){ - Torque_off(id); - return -1; - } - } - } - - // Moving to back request - else { - while(CurrentPosition > value+1){ - CurrentPosition = Read(id, "Present_Position"); - if(CurrentPosition <= MinTick){ - Torque_off(id); - return -1; - } - } - } - - return 1; -} - -//######################## -// Low Levels functions : -//######################## - -void Begin(uint32_t baud){ - if (cmd[1] == '\0') - cmd[1] = String("57600"); - - dxl_wb.init(DEVICE_NAME, baud); -} - -void Ping(int identification){ - get_id[ping_cnt] = identification; - uint16_t model_number = 0; - dxl_wb.ping(get_id[ping_cnt], &model_number, &NULL_POINTER); -} - -void Scan(){ - uint8_t range = 253; - dxl_wb.scan(get_id, &scan_cnt, range); -} - -void Joint(uint8_t id, int32_t goal){ - dxl_wb.jointMode(id, 0, 0, &NULL_POINTER); - dxl_wb.goalPosition(id, goal, &NULL_POINTER); -} - -void Wheel(uint8_t id, int32_t goal){ - dxl_wb.wheelMode(id, 0, &NULL_POINTER); - dxl_wb.goalVelocity(id, goal, &NULL_POINTER); -} - -void Torque_on(uint8_t id){ - dxl_wb.torqueOn(id, &NULL_POINTER); -} - -void Torque_off(uint8_t id){ - dxl_wb.torqueOff(id, &NULL_POINTER); -} - -void Write(uint8_t id, uint32_t value, String commande){ - dxl_wb.writeRegister(id, commande.c_str(), value, &NULL_POINTER); -} - -int32_t Read(uint8_t id, String commande){ - int32_t data = 0; - dxl_wb.readRegister(id, commande.c_str(), &data, &NULL_POINTER); - return data; -} - -void Led(uint8_t id, bool state){ - Write(id, (uint32_t)state, "LED"); -} - -void EmergencyStop(){ - Torque_off(idX); - Torque_off(idY); - Torque_off(idZ); - Serial.println(-1); -} - -void HomeAxe(uint8_t id, int offset){ - Torque_off(id); - Torque_on(id); - MovingDistance(offset, id); - int homingPosition = Read(id, "Present_Position"); - Write(id, (uint32_t)homingPosition, "Homing_Offset"); -} - -void LimiteSwitch(){ - if(homing){ - if(homingZ == HIGH && digitalRead(zSwitchPin) == HIGH){ - HomeAxe(idZ, offsetZ); - } - else if(homingY == HIGH && digitalRead(ySwitchPin) == HIGH){ - HomeAxe(idY, offsetY); - } - else if(homingX == HIGH && digitalRead(xSwitchPin) == HIGH){ - HomeAxe(idX, offsetX); - } - } - else{ - EmergencyStop(); - Serial.flush(); - Serial.println("-1"); - } -} \ No newline at end of file diff --git a/pcbdevice/opencrcontroler/GcodeInterpreter/GcodeInterpreter.ino b/pcbdevice/opencrcontroler/GcodeInterpreter/GcodeInterpreter.ino new file mode 100644 index 0000000..098703d --- /dev/null +++ b/pcbdevice/opencrcontroler/GcodeInterpreter/GcodeInterpreter.ino @@ -0,0 +1,389 @@ +/******************************************************************************* +Titre : OpenCR +Date : 6 février 2019 +Auteur : Maxime Desmarais-Laporte +Descritpion : + +Specifications : +Baud for motors : 57600 b/s +Adress for motors : 11 and 12 and 13 +*******************************************************************************/ +#include + +#if defined(__OPENCM904__) + #define DEVICE_NAME "3" +#elif defined(__OPENCR__) + #define DEVICE_NAME "" +#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 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]; + +DynamixelWorkbench dxl_wb; +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; +uint8_t idZ = 13; + +// Mecanicals propertys: +const float pouleyPitch = 2; //mm +const int nbTheets = 20; +const int resMotorTick = 4096; //Ticks per revolution + +int32_t tickFromMm = resMotorTick/(pouleyPitch*nbTheets); + +// Limit Switch propertys +const int xSwitchPin = 8; +const int ySwitchPin = 9; +const int zSwitchPin = 10; +const int emergencySwitchPin = 2; // intrupt pin + +// Homing variables +bool homing = false; + +const int homeOffsetX = 10*tickFromMm; +const int homeOffsetY = 10*tickFromMm; +const int homeOffsetZ = 10*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); + +// Initialisation : +void setup() +{ + + // Initialisation des pins : + pinMode(xSwitchPin, INPUT_PULLUP); + pinMode(ySwitchPin, INPUT_PULLUP); + pinMode(zSwitchPin, INPUT_PULLUP); + attachInterrupt(digitalPinToInterrupt(emergencySwitchPin), LimiteSwitch, FALLING); + + Serial.begin(57600); + while(!Serial); // Open a Serial Monitor + + //Motor Initialisation Section : + Begin((uint32_t)57600); + Ping(idX); + Ping(idY); + Ping(idZ); + Scan(); + + delay(1000); + + Torque_off(idX); + Torque_off(idY); + Torque_off(idZ); + + Write(idX, 4, OPERATING_MODE); + Write(idY, 4, OPERATING_MODE); + Write(idZ, 4, OPERATING_MODE); + + Write(idX, 0, HOMING_OFFSET); + Write(idY, 0, HOMING_OFFSET); + Write(idZ, 0, HOMING_OFFSET); + + 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); +} + +// Main Program +void loop() +{ + if(isEmegencyState) + setEmergency(); + + if (Serial.available()) + { + if(!isEmegencyState) + { + String read_string = Serial.readStringUntil('\n'); + String words[] = {"", "", ""}; + + int start = 0; + int wordIndex = 0; + int statut[3] = {0, 0, 0}; + + for(int i = 1; i < read_string.length(); i++) + { + if(read_string.charAt(i) == ' ') + { + words[wordIndex++] = read_string.substring(start, i); + start = i+1; + } + } + words[wordIndex] = read_string.substring(start, read_string.length()); + + 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 + 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{ + Serial.println("-1"); + } + } + + else if(words[0] == "G28"){ + Serial.println(Homing()); + } + else if(words[0] == "M18"){ + TorqueOffAll(); + Serial.println("1"); + } + else if(words[0] == "M112"){ + setEmergency(); + + Led(idX, 1); + Led(idY, 1); + Led(idZ, 1); + + 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 + { + Serial.println("-1"); + } + } + else + { + Serial.println("-2"); + } + } +} + +uint8_t getIdFromChar(char letter) +{ + if(letter == 'X') + return idX; + else if(letter == 'Y') + return idY; + else if(letter == 'Z') + return idZ; + else + return -1; +} + +int Homing() +{ + 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; +} + +int HomingAxis(uint8_t id, int speed, int switchPin, int offset) +{ + 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); + 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; +} + +void LimiteSwitch(){ + if(!homing){ + TorqueOffAll(); + + Led(idX, !digitalRead(xSwitchPin)); + Led(idY, !digitalRead(ySwitchPin)); + Led(idZ, !digitalRead(zSwitchPin)); + + isEmegencyState = true; + } +} + +void setEmergency() +{ + TorqueOffAll(); + Led(idX, !digitalRead(xSwitchPin)); + Led(idY, !digitalRead(ySwitchPin)); + Led(idZ, !digitalRead(zSwitchPin)); +} + + +//######################## +// Low Levels functions : +//######################## + +void Begin(uint32_t baud){ + if (cmd[1] == '\0') + cmd[1] = String("57600"); + + dxl_wb.init(DEVICE_NAME, baud); +} + +void Ping(int identification){ + get_id[ping_cnt] = identification; + uint16_t model_number = 0; + dxl_wb.ping(get_id[ping_cnt], &model_number, &NULL_POINTER); +} + +void Scan(){ + uint8_t range = 253; + dxl_wb.scan(get_id, &scan_cnt, range); +} + +void Joint(uint8_t id, int32_t goal){ + dxl_wb.jointMode(id, 0, 0, &NULL_POINTER); + dxl_wb.goalPosition(id, goal, &NULL_POINTER); +} + +void Wheel(uint8_t id, int32_t goal){ + dxl_wb.wheelMode(id, 0, &NULL_POINTER); + dxl_wb.goalVelocity(id, goal, &NULL_POINTER); +} + +void Torque_on(uint8_t id){ + dxl_wb.torqueOn(id, &NULL_POINTER); +} + +void Torque_off(uint8_t id){ + dxl_wb.torqueOff(id, &NULL_POINTER); +} + +void Write(uint8_t id, uint32_t value, String commande){ + dxl_wb.writeRegister(id, commande.c_str(), value, &NULL_POINTER); +} + +int32_t Read(uint8_t id, String commande){ + int32_t data = 0; + dxl_wb.readRegister(id, commande.c_str(), &data, &NULL_POINTER); + return data; +} + +void Led(uint8_t id, bool state){ + Write(id, (uint32_t)state, "LED"); +} + +void TorqueOffAll(){ + Torque_off(idX); + Torque_off(idY); + Torque_off(idZ); +} + +void TorqueOnAll(){ + Torque_on(idX); + Torque_on(idY); + Torque_on(idZ); +} From 06da9b8bb405e5b0b4e2d76ec189b0efd200d0f5 Mon Sep 17 00:00:00 2001 From: Marc-Antoine Lafreniere Date: Sat, 6 Apr 2019 12:47:06 -0400 Subject: [PATCH 16/17] Bug fixes Fix emergency state issue --- .../GcodeInterpreter/GcodeInterpreter.ino | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/pcbdevice/opencrcontroler/GcodeInterpreter/GcodeInterpreter.ino b/pcbdevice/opencrcontroler/GcodeInterpreter/GcodeInterpreter.ino index 098703d..407c010 100644 --- a/pcbdevice/opencrcontroler/GcodeInterpreter/GcodeInterpreter.ino +++ b/pcbdevice/opencrcontroler/GcodeInterpreter/GcodeInterpreter.ino @@ -93,7 +93,8 @@ void setup() pinMode(xSwitchPin, INPUT_PULLUP); pinMode(ySwitchPin, INPUT_PULLUP); pinMode(zSwitchPin, INPUT_PULLUP); - attachInterrupt(digitalPinToInterrupt(emergencySwitchPin), LimiteSwitch, FALLING); + pinMode(emergencySwitchPin, INPUT_PULLUP); + attachInterrupt(digitalPinToInterrupt(emergencySwitchPin), LimiteSwitch, RISING); Serial.begin(57600); while(!Serial); // Open a Serial Monitor @@ -136,9 +137,9 @@ void loop() if (Serial.available()) { + String read_string = Serial.readStringUntil('\n'); if(!isEmegencyState) { - String read_string = Serial.readStringUntil('\n'); String words[] = {"", "", ""}; int start = 0; @@ -303,12 +304,6 @@ int MovingTick(uint8_t id, int32_t value){ void LimiteSwitch(){ if(!homing){ - TorqueOffAll(); - - Led(idX, !digitalRead(xSwitchPin)); - Led(idY, !digitalRead(ySwitchPin)); - Led(idZ, !digitalRead(zSwitchPin)); - isEmegencyState = true; } } From 63152a59912cbaebda6960dab0dfabff103ec234 Mon Sep 17 00:00:00 2001 From: Marc-Antoine Lafreniere Date: Sat, 6 Apr 2019 12:50:15 -0400 Subject: [PATCH 17/17] Refactor serial read Ease usage of the serialread if statements --- arduinosender/CommunicationMain.py | 28 +++++++++++++++++----------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/arduinosender/CommunicationMain.py b/arduinosender/CommunicationMain.py index c66ec2f..0e837ca 100644 --- a/arduinosender/CommunicationMain.py +++ b/arduinosender/CommunicationMain.py @@ -58,20 +58,26 @@ def sendWithAck(gcodeCommand, timeoutCom): commandTimeout = 0 serial.write(gcodeCommand.encode('UTF-8')) - if not serial.readline().decode('UTF-8').startswith('2'): + + received = serial.readline().decode('UTF-8') + + if received.startswith('-2'): + raise RuntimeError('Device error, please reset the device') + elif not received.startswith('2'): raise RuntimeError('Communication lost') while True: received = serial.readline().decode('UTF-8') - if not received.startswith('1') or received.startswith('-1'): + + if received.startswith('1'): + break + elif received.startswith('-2'): + raise RuntimeError('Device error, please reset the device') + elif received.startswith('-1'): + raise RuntimeError('Command error') + else: commandTimeout += 1 if commandTimeout > timeoutCom * 10: raise RuntimeError('Command not executed') - if received.startswith('-1'): - raise RuntimeError('Command error') - if received.startswith('-2'): - raise RuntimeError('Device error, please reset the device') - else: - break def sendFileToPort(comPort, baudRate, communicationTimeout, waitTime, gcodePath): @@ -141,13 +147,13 @@ if __name__ == "__main__": gcodeEntry = LabelWithEntry(autoMainFrame, 'GCode file', padXLeft = (0, padding)) comEntry = LabelWithEntry(autoMainFrame, 'COM port', padXLeft = (0, padding)) baudrateEntry = LabelWithEntry(autoMainFrame, 'Baudrate', padXLeft = (0, padding)) - baudrateEntry.setText('9600') + baudrateEntry.setText('57600') comTimoutEntry = LabelWithEntry(autoMainFrame, 'Com timout (s)', padXLeft = (0, padding)) - comTimoutEntry.setText('1') + comTimoutEntry.setText('2') sleepEntry = LabelWithEntry(autoMainFrame, 'Sleep init (s)', padXLeft = (0, padding)) - sleepEntry.setText('1') + sleepEntry.setText('2') Button(autoMainFrame, text = 'Start', command = lambda: callWithParameters(comEntry, baudrateEntry, comTimoutEntry, sleepEntry, gcodeEntry)) \