From bf1dfbaf7de2c767a116f04b2023554b1c04815b Mon Sep 17 00:00:00 2001 From: Maxime Date: Tue, 26 Mar 2019 11:53:12 -0400 Subject: [PATCH 1/4] 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 2/4] 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 98c01ad36e9c720e5d975ae22b97a171d509b4ab Mon Sep 17 00:00:00 2001 From: Marc-Antoine Lafreniere Date: Sat, 6 Apr 2019 11:17:55 -0400 Subject: [PATCH 3/4] 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 4/4] 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; } }