lefake/S4-P3-Projet
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request #69 from marc4492/issues_fixing

Browse Source 
Issues fixing
This commit is contained in:
Marc-Antoine
2019-04-10 20:55:12 -04:00
committed by GitHub
parent 16e36ef77b ea7f3906a6
commit a2ffccdbb3
6 changed files with 444 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
devicecommunication/CommunicationMain.py
View File

@@ -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')

528
devicesoftware/GcodeInterpreter/GcodeInterpreter.ino
View File

@@ -1,14 +1,60 @@
/*******************************************************************************
Titre : OpenCR
Date : 6 février 2019
Auteur : Maxime Desmarais-Laporte
Title : OpenCR
Date : April 10th 2019
Authors : Maxime Desmarais-Laporte and Marc-Antoine Lafreniere
Descritpion :
GCode interpretor, use in a PCB maker device.
The device has 3 limits switches (one for every axis) use for a homing of the device
These switches are attach to one interrupt and to 3 digital pins.
The interrupt was noizy so a filter is apply to the interrupt
Interrupt filter :
To filter the noize on the interrupt we use the digital pin connected to the NC of the switch
The filter simply looks if the digital pin is LOW for *debounceTimeFalling* time
If it stays LOW for the whole *debounceTimeFalling* then a flag is set to true
If it stays HIGH for the whole *debounceTimeRising* then a flag is set to false
GCode status :
G0 Xx Yy Zz Done
G28 Done
G90 Inactive
G91 Not implemeted
M18 Done
M112 Done
Commincations :
When a command is recived, "2" is printed to the Serial port
When the command is done executing, "1" is printed to the Serial port
If the command can't complete, "-1" is printed to the Serial port
If the device is in eStop mode, "-2" is printed to the Serial port on a commmand request
Electrical :
The 3 homing switches are connected the same way :
NC to a digital pin
NO to the interrupt
COM to the ground
Hence every digital and interrupt pin MUST be in INPUT_PULLUP
TODO :
Connect 2 switches on the other end of the X and Y axis as a fail-safe and add a filter if needed
Change some variable usage : at the moment everything works only if motors' ids are +1 between each (ex: x=9, y=10, z=11)
Specifications :
Baud for motors : 57600 b/s
Adress for motors : 11 and 12 and 13
Baud for motors : 57600 b/s
Dynamixel and OpenCR documentation :
http://emanual.robotis.com/docs/en/dxl/x/xm430-w350/
http://emanual.robotis.com/docs/en/parts/controller/opencr10/
*******************************************************************************/
#include <DynamixelWorkbench.h>
#include "functions.h"
#include "models.h"
#if defined(__OPENCM904__)
#define DEVICE_NAME "3"
@@ -19,28 +65,26 @@ Adress for motors : 11 and 12 and 13
#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 int ACCEPTABLE_RANGE[3] = { 2, 2, 4 };
const String HOMING_OFFSET = "Homing_Offset";
const String OPERATING_MODE = "Operating_Mode";
const String PRESENT_POSITION = "Present_Position";
const String GOAL_POSITION = "Goal_Position";
String cmd[STRING_BUF_NUM];
// 0 = not reverse, 1 = reverse
uint8_t X_REVERSE = 0;
uint8_t Y_REVERSE = 1;
uint8_t Z_REVERSE = 0;
// Dynamixel variables
DynamixelWorkbench dxl_wb;
String cmd[STRING_BUF_NUM];
uint8_t get_id[16];
uint8_t scan_cnt = 0;
uint8_t ping_cnt = 0;
const char *NULL_POINTER = NULL;
bool isEmegencyState = false;
// Motors Propertys :
uint8_t idX = 11;
uint8_t idY = 12;
@@ -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 homeOffsetY = 10*tickFromMm;
const int homeOffsetZ = 10*tickFromMm;
const int homeOffsetX = 28*tickFromMm;
const int homeOffsetY = 19.5*tickFromMm;
const int homeOffsetZ = 2.5*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);
// Debounce timer variables
bool isFalling = false;
long debounceTimeFalling = 250;
long lastTimeXFalling = 0;
long lastTimeYFalling = 0;
long lastTimeZFalling = 0;
bool isRising = false;
long debounceTimeRising = 250;
long lastTimeXRising = 0;
long lastTimeYRising = 0;
long lastTimeZRising = 0;
bool isXSwitchPress = false;
bool isYSwitchPress = false;
bool isZSwitchPress = false;
// 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");
if(words[0] == "G0")
{
for(int i = 1; i < wordIndex+1; i++)
{
if(words[i].length() > 1)
{
float value = words[i].substring(1, words[i].length()).toFloat();
uint8_t idMotor = getIdFromChar((char)words[i].charAt(0));
if(idMotor == -1)
Serial.println("-1");
else
statut[i] = MovingTick(idMotor, value*tickFromMm);
}
else
int i;
for(i = 1; i < wordIndex+1; i++)
{
if(words[i].length() > 1)
{
float value = words[i].substring(1, words[i].length()).toFloat();
uint8_t idMotor = getIdFromChar((char)words[i].charAt(0));
if(idMotor == -1)
{
Serial.println("-1");
}
if((statut[0] == 1 || statut[0] == 0) && (statut[1] == 1 || statut[1] == 0) && (statut[2] == 1|| statut[2] == 0)){
Serial.println("1");
}
else{
break;
}
else
{
commands[i-1]._motorId = idMotor;
commands[i-1]._goalPosition = value*tickFromMm;
}
}
else
{
Serial.println("-1");
}
break;
}
}
if(i == wordIndex+1)
{
isMoving = true;
currentMove = 0;
Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION);
}
}
else if(words[0] == "G28"){
Serial.println(Homing());
else if(words[0] == "G28")
{
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;
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;
return (abs(goal - curPos) <= ACCEPTABLE_RANGE[id-idX]);
}
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);
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);
}
int MovingTick(uint8_t id, int32_t value){
int32_t CurrentPosition = Read(id, PRESENT_POSITION);
bool Forward = value > CurrentPosition;
if((Forward && (CurrentPosition < MAXTICK)) || (!Forward && (CurrentPosition > MINTICK)))
{
Torque_on(id);
Write(id, value, GOAL_POSITION);
}
else
{
Torque_off(id);
return -1;
}
if(Forward){
while(CurrentPosition < value-1 && !isEmegencyState){
CurrentPosition = Read(id, PRESENT_POSITION);
if(CurrentPosition >= MAXTICK && !homing){
Torque_off(id);
return -1;
}
}
}
else {
while(CurrentPosition > value+1 && !isEmegencyState){
CurrentPosition = Read(id, PRESENT_POSITION);
if(CurrentPosition <= MINTICK && !homing){
Torque_off(id);
return -1;
}
}
}
return 1;
Torque_on(id);
}
void LimiteSwitch(){
if(!homing){
isEmegencyState = true;
}
if(!digitalRead(emergencySwitchPin))
isRising = true;
else
isFalling = true;
}
void setEmergency()
{
isMoving = false;
TorqueOffAll();
Led(idX, !digitalRead(xSwitchPin));
Led(idY, !digitalRead(ySwitchPin));
Led(idZ, !digitalRead(zSwitchPin));
Led(idX, ledX);
Led(idY, ledY);
Led(idZ, ledZ);
}
void changeMode(uint8_t id)
{
Write(id, 4, OPERATING_MODE);
}

23
devicesoftware/GcodeInterpreter/functions.h Normal file
View File

@@ -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

10
devicesoftware/GcodeInterpreter/models.h Normal file
View File

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

4
gcodeextractor/gcode/GcodeBuilder.py
View File

@@ -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

6
gcodeextractor/tests/gcode/test_listToGcode.py
View File

@@ -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')