From 98c01ad36e9c720e5d975ae22b97a171d509b4ab Mon Sep 17 00:00:00 2001 From: Marc-Antoine Lafreniere Date: Sat, 6 Apr 2019 11:17:55 -0400 Subject: [PATCH] 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); +}