Merge pull request #69 from marc4492/issues_fixing

Issues fixing

devicecommunication/CommunicationMain.py

@@ -52,6 +52,8 @@ def sendAllLines(lines, timeoutCom):
 	for line in lines:
 		if not line == '\n':
 			sendWithAck(line, timeoutCom)
+			if line == 'G28':
+				sleep(1)
 
 def sendWithAck(gcodeCommand, timeoutCom):
 	global serial
@@ -73,10 +75,10 @@ def sendWithAck(gcodeCommand, timeoutCom):
 		elif received.startswith('-2'):
 			raise RuntimeError('Device error, please reset the device')
 		elif received.startswith('-1'):
-			raise RuntimeError('Command error')
+			raise RuntimeError('Command error : ' + gcodeCommand)
 		else:
 			commandTimeout += 1
-			if commandTimeout > timeoutCom * 10:
+			if commandTimeout > timeoutCom * 100:
 				raise RuntimeError('Command not executed')
 
 

devicesoftware/GcodeInterpreter/GcodeInterpreter.ino

@@ -1,46 +1,90 @@
 /*******************************************************************************
-Titre : OpenCR
+Title : OpenCR
-Date : 6 février 2019
+Date : April 10th 2019
-Auteur : Maxime Desmarais-Laporte
+Authors : Maxime Desmarais-Laporte and Marc-Antoine Lafreniere
+
 Descritpion : 
+  GCode interpretor, use in a PCB maker device.
+
+  The device has 3 limits switches (one for every axis) use for a homing of the device
+  These switches are attach to one interrupt and to 3 digital pins.
+  The interrupt was noizy so a filter is apply to the interrupt
+
+
+Interrupt filter :
+  To filter the noize on the interrupt we use the digital pin connected to the NC of the switch
+  The filter simply looks if the digital pin is LOW for *debounceTimeFalling* time
+  If it stays LOW for the whole *debounceTimeFalling* then a flag is set to true
+  If it stays HIGH for the whole *debounceTimeRising* then a flag is set to false
+
+GCode status :
+  G0 Xx Yy Zz       Done
+  G28               Done
+  G90               Inactive
+  G91               Not implemeted
+  M18               Done
+  M112              Done
+
+Commincations :
+  When a command is recived, "2" is printed to the Serial port
+  When the command is done executing, "1" is printed to the Serial port
+
+  If the command can't complete, "-1" is printed to the Serial port
+  If the device is in eStop mode, "-2" is printed to the Serial port on a commmand request
+
+
+Electrical :
+  The 3 homing switches are connected the same way :
+    NC to a digital pin
+    NO to the interrupt
+    COM to the ground
+  Hence every digital and interrupt pin MUST be in INPUT_PULLUP
+
+
+TODO :
+  Connect 2 switches on the other end of the X and Y axis as a fail-safe and add a filter if needed
+  Change some variable usage : at the moment everything works only if motors' ids are +1 between each (ex: x=9, y=10, z=11)
 
 Specifications :
-Baud for motors : 57600 b/s
+  Baud for motors : 57600 b/s
-Adress for motors : 11 and 12 and 13
+
+Dynamixel and OpenCR documentation :
+  http://emanual.robotis.com/docs/en/dxl/x/xm430-w350/
+  http://emanual.robotis.com/docs/en/parts/controller/opencr10/
 *******************************************************************************/
 #include <DynamixelWorkbench.h>
+#include "functions.h"
+#include "models.h"
 
 #if defined(__OPENCM904__)
   #define DEVICE_NAME "3"
 #elif defined(__OPENCR__)
   #define DEVICE_NAME ""
-#endif   
+#endif
 
 #define STRING_BUF_NUM 64
 #define MINTICK 0
 #define MAXTICK 1048575
-
+const int ACCEPTABLE_RANGE[3] = { 2, 2, 4 };
-// 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];
+// 0 = not reverse, 1 = reverse
+uint8_t X_REVERSE = 0;
+uint8_t Y_REVERSE = 1;
+uint8_t Z_REVERSE = 0;
 
+// Dynamixel variables
 DynamixelWorkbench dxl_wb;
+String cmd[STRING_BUF_NUM];
 uint8_t get_id[16];
 uint8_t scan_cnt = 0;
 uint8_t ping_cnt = 0;
-
 const char *NULL_POINTER = NULL;
 
-bool isEmegencyState = false;
-
 // Motors Propertys :
 uint8_t idX = 11;
 uint8_t idY = 12;
@@ -61,45 +105,60 @@ const int emergencySwitchPin = 2; // intrupt pin
 
 // Homing variables
 bool homing = false;
+bool homingX = false;
+bool homingY = false;
+bool homingZ = false;
+int homingState = 0;
 
-const int homeOffsetX = 10*tickFromMm;
+const int homeOffsetX = 28*tickFromMm;
-const int homeOffsetY = 10*tickFromMm;
+const int homeOffsetY = 19.5*tickFromMm;
-const int homeOffsetZ = 10*tickFromMm;
+const int homeOffsetZ = 2.5*tickFromMm;
 
-// Fonctions prototypes : 
+// Debounce timer variables
-void Begin(uint32_t baud);
+bool isFalling = false;
-void Ping(int identification);
+long debounceTimeFalling = 250;
-void Scan();
+long lastTimeXFalling = 0;
-void Joint(uint8_t id, uint16_t goal);
+long lastTimeYFalling = 0;
-void Wheel(uint8_t id, int32_t goal);
+long lastTimeZFalling = 0;
-void Torque_on(uint8_t id);
+
-void Torque_off(uint8_t id);
+bool isRising = false;
-void Write(uint8_t id, uint32_t value, String commande);
+long debounceTimeRising = 250;
-int32_t Read(uint8_t id, String commande);
+long lastTimeXRising = 0;
-void Led(uint8_t id, bool state);
+long lastTimeYRising = 0;
-void TorqueOffAll();
+long lastTimeZRising = 0;
-void OffsetAxe(uint8_t id, int offset);
+
-void LimiteSwitch();
+bool isXSwitchPress = false;
-int MovingTick(uint8_t id, int32_t value);
+bool isYSwitchPress = false;
-int Homing();
+bool isZSwitchPress = false;
-int HomingAxis(uint8_t id, int speed, int switchPin, int offset);
+
-uint8_t getIdFromChar(char letter);
+// eStop
+bool isEmegencyState = false;
+bool ledX = false;
+bool ledY = false;
+bool ledZ = false;
+
+// Moving variables
+bool isMoving = false;
+bool currentMoveDone = false;
+bool hasFailed = false;
+int currentMove = 0;
+int nbsMovements = 0;
+int32_t currentPosition = 0;
+int32_t movingSpeed[] = { 75, 75 };
+MovingCommand commands[3];
 
-// Initialisation :
 void setup() 
 {
-
   // Initialisation des pins :
   pinMode(xSwitchPin, INPUT_PULLUP);
   pinMode(ySwitchPin, INPUT_PULLUP);
   pinMode(zSwitchPin, INPUT_PULLUP);
   pinMode(emergencySwitchPin, INPUT_PULLUP);
-  attachInterrupt(digitalPinToInterrupt(emergencySwitchPin), LimiteSwitch, RISING);
+  attachInterrupt(digitalPinToInterrupt(emergencySwitchPin), LimiteSwitch, CHANGE);
   
   Serial.begin(57600);
-  while(!Serial); // Open a Serial Monitor  
 
-    //Motor Initialisation Section :
+  //Motor Initialisation Section :
   Begin((uint32_t)57600);
   Ping(idX);
   Ping(idY);
@@ -124,18 +183,239 @@ void setup()
   dxl_wb.writeRegister(idY, 10, 1, &Y_REVERSE, &NULL_POINTER);
   dxl_wb.writeRegister(idZ, 10, 1, &Z_REVERSE, &NULL_POINTER);
 
+  Write(idX, movingSpeed[0], "Profile_Velocity");
+  Write(idY, movingSpeed[1], "Profile_Velocity");
+
   Torque_on(idX);
   Torque_on(idY);
   Torque_on(idZ);
+
+  resetMovingVariables();
 }
 
-// Main Program
 void loop() 
 {
+  if(isFalling)
+  {
+    if(digitalRead(xSwitchPin))
+    {
+      if(millis() - lastTimeXFalling > debounceTimeFalling)
+      {
+        lastTimeXFalling = millis();
+        isFalling = false;
+        isXSwitchPress = true;
+
+        // Insert what to do if the button is press (executed on change)
+        if(homingX)
+        {
+          homingX = false;
+          OffsetAxe(idX, homeOffsetX);
+          Torque_off(idX);
+          Write(idX, 4, OPERATING_MODE);
+          Torque_on(idX);
+          
+          currentMove = 0;
+          nbsMovements = 1;
+          commands[currentMove]._motorId = idX;
+          commands[currentMove]._goalPosition = 0;
+          isMoving = true;
+          Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION); 
+        }
+        else
+        {
+          isEmegencyState = true;
+          ledX = true;
+        }
+      }
+    }
+    else
+    {
+      lastTimeXFalling = millis();
+      isXSwitchPress = false;
+    }
+
+    if(digitalRead(ySwitchPin))
+    {
+      if(millis() - lastTimeYFalling > debounceTimeFalling)
+      {
+        lastTimeYFalling = millis();
+        isFalling = false;
+        isYSwitchPress = true;
+
+        // Insert what to do if the button is press (executed on change)
+        if(homingY)
+        {
+          homingY = false;
+          OffsetAxe(idY, homeOffsetY);
+          Torque_off(idY);
+          Write(idY, 4, OPERATING_MODE);
+          Torque_on(idY);
+          
+          currentMove = 0;
+          nbsMovements = 1;
+          commands[currentMove]._motorId = idY;
+          commands[currentMove]._goalPosition = 0;
+          isMoving = true;
+          Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION);
+        }
+        else
+        {
+          isEmegencyState = true;
+          ledY = true;
+        }
+      }
+    }
+    else
+    {
+      lastTimeYFalling = millis();
+      isYSwitchPress = false;
+    }
+
+    if(digitalRead(zSwitchPin))
+    {
+      if(millis() - lastTimeZFalling > debounceTimeFalling)
+      {
+        lastTimeZFalling = millis();
+        isFalling = false;
+        isZSwitchPress = true;
+        
+        // Insert what to do if the button is press (executed on change)
+        if(homingZ)
+        {
+          homingZ = false;
+          OffsetAxe(idZ, homeOffsetZ);
+          Torque_off(idZ);
+          Write(idZ, 4, OPERATING_MODE);
+          Torque_on(idZ);
+
+          currentMove = 0;
+          nbsMovements = 1;
+          commands[currentMove]._motorId = idZ;
+          commands[currentMove]._goalPosition = 0;
+          isMoving = true;          
+          Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION);
+        }
+        else
+          {
+          isEmegencyState = true;
+          ledZ = true;
+        }
+      }
+    }
+    else
+    {
+      lastTimeZFalling = millis();
+      isZSwitchPress = false;
+    }
+
+    if(!digitalRead(xSwitchPin) && !digitalRead(ySwitchPin) && !digitalRead(zSwitchPin))
+      isFalling = false;
+  }
+
+  if(isRising)
+  {
+    if(!digitalRead(xSwitchPin))
+    {
+      if(millis() - lastTimeXRising > debounceTimeRising)
+      {
+        lastTimeXRising = millis();
+        isRising = false;
+        isXSwitchPress = false;
+        // Insert what to do if the button is release (executed on change)
+      }
+    }
+    else
+      lastTimeXRising = millis();
+
+    if(!digitalRead(ySwitchPin))
+    {
+      if(millis() - lastTimeYRising > debounceTimeRising)
+      {
+        lastTimeYRising = millis();
+        isRising = false;
+        isYSwitchPress = false;
+        // Insert what to do if the button is release (executed on change)
+      }
+    }
+    else
+      lastTimeYRising = millis();
+
+    if(!digitalRead(zSwitchPin))
+    {
+      if(millis() - lastTimeZRising > debounceTimeRising)
+      {
+        lastTimeZRising = millis();
+        isRising = false;
+        isZSwitchPress = false;
+        // Insert what to do if the button is release (executed on change)
+      }
+    }
+    else
+      lastTimeZRising = millis();
+
+    if(digitalRead(xSwitchPin) && digitalRead(ySwitchPin) && digitalRead(zSwitchPin))
+    {
+      isRising = false;
+    }
+  }
+
+  if(isMoving)
+  {
+    currentPosition = Read(commands[currentMove]._motorId, PRESENT_POSITION);
+    if(isInRange(currentPosition, commands[currentMove]._goalPosition, commands[currentMove]._motorId))
+    {
+      currentMoveDone = true;
+    }
+  }
+
+  if(isMoving && currentMoveDone)
+  {
+    if(currentMove+1 >= nbsMovements)
+    {
+      if(homing)
+      {
+        if(commands[currentMove]._motorId == idZ)
+        {
+          homingY = true;
+          HomingAxis(idY, -100);
+          homingState++;
+        }
+        else if(commands[currentMove]._motorId == idY)
+        {
+          homingX = true;
+          HomingAxis(idX, -100);
+          homingState++;
+        }
+        else if(commands[currentMove]._motorId == idX)
+        {
+          homingState++;
+          homing = false;
+          Serial.println(homingState == 3 ? "1" : "-1");
+          homingState = 0;
+
+          Write(idX, movingSpeed[0], "Profile_Velocity");
+          Write(idY, movingSpeed[1], "Profile_Velocity");
+        }
+        resetMovingVariables();
+      }
+      else
+      {
+        resetMovingVariables();
+        Serial.println("1");
+      }
+    }
+    else
+    {
+      currentMoveDone = false;
+      currentMove++;
+      Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION);
+    }
+  }
+  
   if(isEmegencyState)
     setEmergency();
   
-  if (Serial.available())
+  if (!homing && !isMoving && Serial.available())
   {
     String read_string = Serial.readStringUntil('\n');
     if(!isEmegencyState)
@@ -155,35 +435,49 @@ void loop()
         }
       }
       words[wordIndex] = read_string.substring(start, read_string.length());
-      
+      nbsMovements = wordIndex;
-      Serial.println("2"); 
+      Serial.println("2");
 
       if(words[0] == "G0")
       {
-          for(int i = 1; i < wordIndex+1; i++)
+        int i;
-          {
+        for(i = 1; i < wordIndex+1; i++)
-              if(words[i].length() > 1)
+        {
-              {
+            if(words[i].length() > 1)
-                  float value = words[i].substring(1, words[i].length()).toFloat();
+            {
-                  uint8_t idMotor = getIdFromChar((char)words[i].charAt(0));
+                float value = words[i].substring(1, words[i].length()).toFloat();
-                  if(idMotor == -1)
+                uint8_t idMotor = getIdFromChar((char)words[i].charAt(0));
-                      Serial.println("-1");
+                if(idMotor == -1)
-                  else
+                {
-                      statut[i] = MovingTick(idMotor, value*tickFromMm);
-              }
-              else
                   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
-          }
+                {
-          else{
+                  commands[i-1]._motorId = idMotor;
+                  commands[i-1]._goalPosition = value*tickFromMm;
+                }
+            }
+            else
+            {
               Serial.println("-1");
-          }
+              break;
+            }
+        }
+        
+        if(i == wordIndex+1)
+        {
+          isMoving = true;
+          currentMove = 0;
+          Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION);
+        }
       }
       
-      else if(words[0] == "G28"){
+      else if(words[0] == "G28")
-        Serial.println(Homing());
+      {
+        homing = true;
+        homingZ = true;
+        HomingAxis(idZ, -50);
       }
       else if(words[0] == "M18"){
           TorqueOffAll();
@@ -198,15 +492,8 @@ void loop()
 
           Serial.println("1");
       }
-      else if(words[0] == "M1")
+      else if(words[0] == "G90")
       {
-        TorqueOffAll();
-        
-        Write(idX, -Read(idX, PRESENT_POSITION), HOMING_OFFSET);
-        Write(idY, -Read(idY, PRESENT_POSITION), HOMING_OFFSET);
-        Write(idZ, -Read(idZ, PRESENT_POSITION), HOMING_OFFSET);
-        
-        TorqueOnAll();
         Serial.println("1");
       }
       else
@@ -233,87 +520,70 @@ uint8_t getIdFromChar(char letter)
         return -1;
 }
 
-int Homing()
+bool isInRange(int goal, int curPos, uint8_t id)
 {
-  int state = 0;
+  return (abs(goal - curPos) <= ACCEPTABLE_RANGE[id-idX]);
-  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)
+void resetMovingVariables()
+{
+  isMoving = false;
+  currentMoveDone = false;
+  currentMove = 0;
+  nbsMovements = 0;
+  currentPosition = 0;
+  hasFailed = false;
+
+  for(int i = 0; i < 3; i++)
+  {
+    commands[i]._motorId = i+idX;
+    commands[i]._goalPosition = Read(i+idX, PRESENT_POSITION);
+  }
+}
+
+void HomingAxis(uint8_t id, int speed)
 {
   Torque_off(id);
   Write(id, 0, HOMING_OFFSET);
   Torque_on(id);
   Wheel(id, speed);
-  while(digitalRead(switchPin));
-  OffsetAxe(id, offset);
-  Write(id, 4, OPERATING_MODE);
-  return MovingTick(id, 0);
 }
 
 void OffsetAxe(uint8_t id, int offset){
   int32_t posPresent = Read(id, PRESENT_POSITION);
   int32_t homePosition = - posPresent - offset;
-  Torque_off(id); 
+  Torque_off(id);
+
+  if(id == idX && X_REVERSE)
+    homePosition *= -1;
+  else if(id == idY && Y_REVERSE)
+    homePosition *= -1;
+  else if(id == idZ && Z_REVERSE)
+    homePosition *= -1;
+  
   Write(id, homePosition, HOMING_OFFSET);
-}
+  Torque_on(id);
-
-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){
+  if(!digitalRead(emergencySwitchPin))
-    isEmegencyState = true;
+    isRising = true;
-  }
+  else
+    isFalling = true;
 }
 
 void setEmergency()
 {
+  isMoving = false;
   TorqueOffAll();
-  Led(idX, !digitalRead(xSwitchPin));
+  Led(idX, ledX);
-  Led(idY, !digitalRead(ySwitchPin)); 
+  Led(idY, ledY); 
-  Led(idZ, !digitalRead(zSwitchPin)); 
+  Led(idZ, ledZ);
+}
+
+void changeMode(uint8_t id)
+{
+  Write(id, 4, OPERATING_MODE);
 }
 
 

devicesoftware/GcodeInterpreter/functions.h

@@ -0,0 +1,23 @@
+#ifndef FUNCTIONS_H
+#define FUNCTIONS_H
+
+void Begin(uint32_t baud);
+void Ping(int identification);
+void Scan();
+void Joint(uint8_t id, uint16_t goal);
+void Wheel(uint8_t id, int32_t goal);
+void Torque_on(uint8_t id);
+void Torque_off(uint8_t id);
+void Write(uint8_t id, uint32_t value, String commande);
+int32_t Read(uint8_t id, String commande);
+void Led(uint8_t id, bool state);
+void TorqueOffAll();
+void OffsetAxe(uint8_t id, int offset);
+void LimiteSwitch();
+void HomingAxis(uint8_t id, int speed);
+uint8_t getIdFromChar(char letter);
+bool isInRange(int curPos, int goal);
+void resetMovingVariables();
+void changeMode(uint8_t id);
+
+#endif

devicesoftware/GcodeInterpreter/models.h

@@ -0,0 +1,10 @@
+#ifndef MODELS_H
+#define MODELS_H
+
+typedef struct MovingCommand
+{
+  uint8_t _motorId = 0;
+  uint32_t _goalPosition = 0;
+};
+
+#endif

gcodeextractor/gcode/GcodeBuilder.py

@@ -24,12 +24,12 @@ def listToGCode(listIndex, pHeight, pWidth):
 		else:
 			gcodeCommand.append('G0 X' + str(round(coord.getX()*pWidth, 2)) + ' Y' + str(round(coord.getY()*pHeight, 2)))
 			if toolUp:
-				gcodeCommand.append('G0 Z3')
+				gcodeCommand.append('G0 Z10.5')
 				toolUp = False
 			
 	# FOOTER
 	gcodeCommand.append('\nG0 Z0')
-	gcodeCommand.append('G28')
+	gcodeCommand.append('G0 X0 Y0')
 	gcodeCommand.append('M18')
 	
 	return gcodeCommand

gcodeextractor/tests/gcode/test_listToGcode.py

@@ -52,10 +52,10 @@ class TestListToGCode(TestCase):
 
 def getExpected(coords, ySize, xSize):
 	header = ['G28', 'G90\n']
-	footer = ['\nG0 Z0', 'G28', 'M18']
+	footer = ['\nG0 Z0', 'G0 X0 Y0', 'M18']
 	
 	content = ['G0 X' + str(round(xSize * coords[0].getX(), 2)) + ' Y' + str(round(ySize * coords[0].getY(), 2)),
-	           'G0 Z3',
+	           'G0 Z10.5',
 	           ]
 	
 	for index, coord in enumerate(coords):
@@ -63,7 +63,7 @@ def getExpected(coords, ySize, xSize):
 			if coord.getX() != -1 and coord.getY() != -1:
 				content.append('G0 X' + str(xSize * coord.getX()) + ' Y' + str(ySize * coord.getY()))
 				if coords[index - 1].getX() == -1 and coords[index - 1].getX() == -1:
-					content.append('G0 Z3')
+					content.append('G0 Z10.5')
 			else:
 				content.append('G0 Z0')