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

Fixing bugs

Browse Source 
Removed blocking whiles
This commit is contained in:
Marc-Antoine Lafreniere
2019-04-09 21:15:02 -04:00
parent 15d61836bb
commit a12c9fe083
5 changed files with 191 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
devicecommunication/CommunicationMain.py
View File

@@ -52,6 +52,8 @@ def sendAllLines(lines, timeoutCom):
for line in lines: for line in lines:
if not line == '\n': if not line == '\n':
sendWithAck(line, timeoutCom) sendWithAck(line, timeoutCom)
if line == 'G28':
sleep(1)
def sendWithAck(gcodeCommand, timeoutCom): def sendWithAck(gcodeCommand, timeoutCom):
global serial global serial

241
devicesoftware/GcodeInterpreter/GcodeInterpreter.ino
View File

@@ -9,6 +9,8 @@ Baud for motors : 57600 b/s
Adress for motors : 11 and 12 and 13 Adress for motors : 11 and 12 and 13
*******************************************************************************/ *******************************************************************************/
#include <DynamixelWorkbench.h> #include <DynamixelWorkbench.h>
#include "functions.h"
#include "models.h"
#if defined(__OPENCM904__) #if defined(__OPENCM904__)
#define DEVICE_NAME "3" #define DEVICE_NAME "3"
@@ -19,6 +21,7 @@ Adress for motors : 11 and 12 and 13
#define STRING_BUF_NUM 64 #define STRING_BUF_NUM 64
#define MINTICK 0 #define MINTICK 0
#define MAXTICK 1048575 #define MAXTICK 1048575
const int ACCEPTABLE_RANGE[3] = { 2, 2, 3 };
// 0 = not reverse, 1 = reverse // 0 = not reverse, 1 = reverse
uint8_t X_REVERSE = 0; uint8_t X_REVERSE = 0;
@@ -66,9 +69,9 @@ bool homingY = false;
bool homingZ = false; bool homingZ = false;
int homingState = 0; int homingState = 0;
const int homeOffsetX = 10*tickFromMm; const int homeOffsetX = /*28*/48*tickFromMm;
const int homeOffsetY = 10*tickFromMm; const int homeOffsetY = /*19.5*/40*tickFromMm;
const int homeOffsetZ = 5*tickFromMm; const int homeOffsetZ = 2.5*tickFromMm;
// Debounce timer variables // Debounce timer variables
bool isFalling = false; bool isFalling = false;
@@ -87,23 +90,14 @@ bool isXSwitchPress = false;
bool isYSwitchPress = false; bool isYSwitchPress = false;
bool isZSwitchPress = false; bool isZSwitchPress = false;
// Fonctions prototypes : // Moving variables
void Begin(uint32_t baud); bool isMoving = false;
void Ping(int identification); bool currentMoveDone = false;
void Scan(); bool hasFailed = false;
void Joint(uint8_t id, uint16_t goal); int currentMove = 0;
void Wheel(uint8_t id, int32_t goal); int nbsMovements = 0;
void Torque_on(uint8_t id); int32_t currentPosition = 0;
void Torque_off(uint8_t id); MovingCommand commands[3];
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);
void HomingAxis(uint8_t id, int speed);
uint8_t getIdFromChar(char letter);
// Initialisation : // Initialisation :
void setup() void setup()
@@ -142,9 +136,12 @@ void setup()
dxl_wb.writeRegister(idY, 10, 1, &Y_REVERSE, &NULL_POINTER); dxl_wb.writeRegister(idY, 10, 1, &Y_REVERSE, &NULL_POINTER);
dxl_wb.writeRegister(idZ, 10, 1, &Z_REVERSE, &NULL_POINTER); dxl_wb.writeRegister(idZ, 10, 1, &Z_REVERSE, &NULL_POINTER);
Torque_on(idX); Torque_on(idX);
Torque_on(idY); Torque_on(idY);
Torque_on(idZ); Torque_on(idZ);
resetMovingVariables();
} }
// Main Program // Main Program
@@ -164,12 +161,16 @@ void loop()
{ {
homingX = false; homingX = false;
OffsetAxe(idX, homeOffsetX); OffsetAxe(idX, homeOffsetX);
Torque_off(idX);
Write(idX, 4, OPERATING_MODE); Write(idX, 4, OPERATING_MODE);
homingState += MovingTick(idX, 0); Torque_on(idX);
Serial.println(homingState == 3 ? "1" : "-1"); currentMove = 0;
homingState = 0; nbsMovements = 1;
homing = false; commands[currentMove]._motorId = idX;
commands[currentMove]._goalPosition = 0;
isMoving = true;
Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION);
} }
else else
isEmegencyState = true; isEmegencyState = true;
@@ -193,11 +194,16 @@ void loop()
{ {
homingY = false; homingY = false;
OffsetAxe(idY, homeOffsetY); OffsetAxe(idY, homeOffsetY);
Torque_off(idY);
Write(idY, 4, OPERATING_MODE); Write(idY, 4, OPERATING_MODE);
homingState += MovingTick(idY, 0); Torque_on(idY);
homingX = true; currentMove = 0;
HomingAxis(idX, -100); nbsMovements = 1;
commands[currentMove]._motorId = idY;
commands[currentMove]._goalPosition = 0;
isMoving = true;
Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION);
} }
else else
isEmegencyState = true; isEmegencyState = true;
@@ -221,11 +227,16 @@ void loop()
{ {
homingZ = false; homingZ = false;
OffsetAxe(idZ, homeOffsetZ); OffsetAxe(idZ, homeOffsetZ);
Torque_off(idZ);
Write(idZ, 4, OPERATING_MODE); Write(idZ, 4, OPERATING_MODE);
homingState += MovingTick(idZ, 0); Torque_on(idZ);
homingY = true; currentMove = 0;
HomingAxis(idY, -100); nbsMovements = 1;
commands[currentMove]._motorId = idZ;
commands[currentMove]._goalPosition = 0;
isMoving = true;
Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION);
} }
else else
isEmegencyState = true; isEmegencyState = true;
@@ -285,11 +296,60 @@ void loop()
} }
} }
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;
}
resetMovingVariables();
}
else
{
resetMovingVariables();
Serial.println("1");
}
}
else
{
currentMoveDone = false;
currentMove++;
Write(commands[currentMove]._motorId, commands[currentMove]._goalPosition, GOAL_POSITION);
}
}
if(isEmegencyState) if(isEmegencyState)
setEmergency(); setEmergency();
if (!homing && Serial.available()) if (!homing && !isMoving && Serial.available())
{ {
String read_string = Serial.readStringUntil('\n'); String read_string = Serial.readStringUntil('\n');
if(!isEmegencyState) if(!isEmegencyState)
@@ -309,31 +369,42 @@ void loop()
} }
} }
words[wordIndex] = read_string.substring(start, read_string.length()); words[wordIndex] = read_string.substring(start, read_string.length());
nbsMovements = wordIndex;
Serial.println("2"); Serial.println("2");
if(words[0] == "G0") 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"); 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"); 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")
@@ -366,6 +437,10 @@ void loop()
TorqueOnAll(); TorqueOnAll();
Serial.println("1"); Serial.println("1");
} }
else if(words[0] == "G90")
{
Serial.println("1");
}
else else
{ {
Serial.println("-1"); Serial.println("-1");
@@ -390,6 +465,27 @@ uint8_t getIdFromChar(char letter)
return -1; return -1;
} }
bool isInRange(int goal, int curPos, uint8_t id)
{
return (abs(goal - curPos) <= ACCEPTABLE_RANGE[id-idX]);
}
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) void HomingAxis(uint8_t id, int speed)
{ {
Torque_off(id); Torque_off(id);
@@ -411,43 +507,7 @@ void OffsetAxe(uint8_t id, int offset){
homePosition *= -1; homePosition *= -1;
Write(id, homePosition, HOMING_OFFSET); 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(){ void LimiteSwitch(){
@@ -459,10 +519,11 @@ void LimiteSwitch(){
void setEmergency() void setEmergency()
{ {
isMoving = false;
TorqueOffAll(); TorqueOffAll();
Led(idX, !digitalRead(xSwitchPin)); Led(idX, digitalRead(xSwitchPin));
Led(idY, !digitalRead(ySwitchPin)); Led(idY, digitalRead(ySwitchPin));
Led(idZ, !digitalRead(zSwitchPin)); Led(idZ, digitalRead(zSwitchPin));
} }

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

@@ -0,0 +1,22 @@
#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();
#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: else:
gcodeCommand.append('G0 X' + str(round(coord.getX()*pWidth, 2)) + ' Y' + str(round(coord.getY()*pHeight, 2))) gcodeCommand.append('G0 X' + str(round(coord.getX()*pWidth, 2)) + ' Y' + str(round(coord.getY()*pHeight, 2)))
if toolUp: if toolUp:
gcodeCommand.append('G0 Z3') gcodeCommand.append('G0 Z10.5')
toolUp = False toolUp = False
# FOOTER # FOOTER
gcodeCommand.append('\nG0 Z0') gcodeCommand.append('\nG0 Z0')
gcodeCommand.append('G28') gcodeCommand.append('G0 X0 Y0')
gcodeCommand.append('M18') gcodeCommand.append('M18')
return gcodeCommand return gcodeCommand