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

Merge branch 'master' into #55_Create_UI_frame

Browse Source
This commit is contained in:
Ian
2019-04-06 14:56:57 -04:00
parent 89e751350c d5d56968b8
commit 3939b8d590
5 changed files with 637 additions and 276 deletions
Download Patch File Download Diff File Expand all files Collapse all files

206
arduinosender/CommunicationMain.py Normal file
View File

@@ -0,0 +1,206 @@
from serial import Serial
from serial import SerialException
from time import sleep
from tkinter import *
from tkinter.ttk import Notebook
global serial
class LabelWithEntry:
def __init__(self, parent, labelText, padXLeft = (0, 0), padYLeft = (0, 0), padXRight = (0, 0), padYRight = (0, 0)):
frame = Frame(parent)
frame.pack(side = TOP, fill = BOTH, expand = 1)
self.label = Label(frame, text = labelText)
self.label.pack(side = LEFT, padx = padXLeft, pady = padYLeft)
self.entry = Entry(frame)
self.entry.pack(side = RIGHT, padx = padXRight, pady = padYRight)
def get(self):
return self.entry.get()
def setText(self, text):
self.entry.insert(END, text)
def openSerial(com, baudRate, communicationTimeout, waitTime):
global serial
try:
serial = Serial(com, int(baudRate), timeout = int(communicationTimeout))
sleep(int(waitTime))
except SerialException as errSE:
raise errSE
def closeSerial():
global serial
try:
serial.close()
except SerialException as errSE:
raise errSE
def readFile(filePath):
file = open(filePath, 'r')
lines = file.readlines()
file.close()
return lines
def sendAllLines(lines, timeoutCom):
global serial
for line in lines:
if not line == '\n':
sendWithAck(line, timeoutCom)
def sendWithAck(gcodeCommand, timeoutCom):
global serial
commandTimeout = 0
serial.write(gcodeCommand.encode('UTF-8'))
received = serial.readline().decode('UTF-8')
if received.startswith('-2'):
raise RuntimeError('Device error, please reset the device')
elif not received.startswith('2'):
raise RuntimeError('Communication lost')
while True:
received = serial.readline().decode('UTF-8')
if received.startswith('1'):
break
elif received.startswith('-2'):
raise RuntimeError('Device error, please reset the device')
elif received.startswith('-1'):
raise RuntimeError('Command error')
else:
commandTimeout += 1
if commandTimeout > timeoutCom * 10:
raise RuntimeError('Command not executed')
def sendFileToPort(comPort, baudRate, communicationTimeout, waitTime, gcodePath):
global serial
try:
openSerial(comPort, baudRate, communicationTimeout, waitTime)
lines = readFile(gcodePath)
sendAllLines(lines, communicationTimeout)
closeSerial()
except SerialException as errSE:
print('Serial Exception' + str(errSE))
except RuntimeError as errRE:
print(str(errRE))
finally:
if serial is not None:
closeSerial()
def callWithParameters(comPortET, communicationTimeoutET, communicationBaudrateET, waitTimeET, gcodePathET):
timeoutCom = 2
baudRate = 9600
waitTime = 1
if communicationTimeoutET.get() != '':
timeoutCom = int(communicationTimeoutET.get())
if communicationBaudrateET.get() != '':
baudRate = int(communicationBaudrateET.get())
if waitTimeET.get() != '':
waitTime = int(waitTimeET.get())
sendFileToPort(comPortET.get(), baudRate, timeoutCom, waitTime, gcodePathET.get())
def sendX(mov, timeoutCom):
sendWithAck('G0 X' + str(mov), timeoutCom)
def sendY(mov, timeoutCom):
sendWithAck('G0 Y' + str(mov), timeoutCom)
def sendZ(mov, timeoutCom):
sendWithAck('G0 Z' + str(mov), timeoutCom)
def sendHome(timeoutCom):
sendWithAck('G28', timeoutCom)
def sendAbsPosition(timeoutCom):
sendWithAck('G90', timeoutCom)
def sendRelativePosition(timeoutCom):
sendWithAck('G91', timeoutCom)
if __name__ == "__main__":
mainWindow = Tk()
mainWindow.title('Hello World!')
tabControl = Notebook(mainWindow)
tabAuto = Frame(tabControl)
tabManual = Frame(tabControl)
# Frames
autoMainFrame = Frame(tabAuto, width = 300, height = 100)
autoMainFrame.pack(fill = None, expand = False)
manualMainFrame = Frame(tabManual, width = 300, height = 100)
manualMainFrame.pack(fill = None, expand = False)
# Auto mode
padding = 20
gcodeEntry = LabelWithEntry(autoMainFrame, 'GCode file', padXLeft = (0, padding))
comEntry = LabelWithEntry(autoMainFrame, 'COM port', padXLeft = (0, padding))
baudrateEntry = LabelWithEntry(autoMainFrame, 'Baudrate', padXLeft = (0, padding))
baudrateEntry.setText('57600')
comTimoutEntry = LabelWithEntry(autoMainFrame, 'Com timout (s)', padXLeft = (0, padding))
comTimoutEntry.setText('2')
sleepEntry = LabelWithEntry(autoMainFrame, 'Sleep init (s)', padXLeft = (0, padding))
sleepEntry.setText('2')
Button(autoMainFrame, text = 'Start',
command = lambda: callWithParameters(comEntry, baudrateEntry, comTimoutEntry, sleepEntry, gcodeEntry)) \
.pack(side = BOTTOM, fill = BOTH, expand = 1, pady = (15, 0))
# Manual mode
buttonsFrame = Frame(manualMainFrame)
buttonsFrame.pack(side = TOP)
openButton = Button(buttonsFrame, text = 'Open', command = lambda: openSerial(comEntry.get(), int(baudrateEntry.get()), int(comTimoutEntry.get()), int(sleepEntry.get())))
openButton.grid(row = 0, column = 0)
closeButton = Button(buttonsFrame, text = 'Close', command = lambda: closeSerial())
closeButton.grid(row = 0, column = 2)
sendXP5Button = Button(buttonsFrame, text = '+5X', command = lambda: sendX(5, int(comTimoutEntry.get())))
sendXP5Button.grid(row = 2, column = 2, padx = 5)
sendXM5Button = Button(buttonsFrame, text = '-5X', command = lambda: sendX(-5, int(comTimoutEntry.get())))
sendXM5Button.grid(row = 2, column = 0, padx = 5)
sendYP5Button = Button(buttonsFrame, text = '+5Y', command = lambda: sendY(5, int(comTimoutEntry.get())))
sendYP5Button.grid(row = 1, column = 1, pady = 5)
sendYM5Button = Button(buttonsFrame, text = '-5Y', command = lambda: sendY(-5, int(comTimoutEntry.get())))
sendYM5Button.grid(row = 3, column = 1, pady = 5)
sendHomeButton = Button(buttonsFrame, text = 'Home', command = lambda: sendHome(int(comTimoutEntry.get())))
sendHomeButton.grid(row = 2, column = 1)
sendZP5Button = Button(buttonsFrame, text = '+5Z', command = lambda: sendZ(5, int(comTimoutEntry.get())))
sendZP5Button.grid(row = 3, column = 2)
sendZM5Button = Button(buttonsFrame, text = '-5Z', command = lambda: sendZ(-5, int(comTimoutEntry.get())))
sendZM5Button.grid(row = 1, column = 2)
sendZP5Button = Button(buttonsFrame, text = 'G90', command = lambda: sendAbsPosition(int(comTimoutEntry.get())))
sendZP5Button.grid(row = 1, column = 0)
sendZM5Button = Button(buttonsFrame, text = 'G91', command = lambda: sendRelativePosition(int(comTimoutEntry.get())))
sendZM5Button.grid(row = 3, column = 0)
customCommandFrame = Frame(manualMainFrame)
customCommandFrame.pack(side = BOTTOM, fill = BOTH, expand = 1)
customCommand = Entry(customCommandFrame)
customCommand.pack(side = LEFT, fill = BOTH, expand = 1)
Button(customCommandFrame, text = 'Send', command = lambda: sendWithAck(customCommand.get(), int(comTimoutEntry.get()))).pack(side = RIGHT)
tabControl.add(tabAuto, text = 'Auto')
tabControl.add(tabManual, text = 'Manual')
tabControl.pack(expand = 1, fill = BOTH)
mainWindow.mainloop()
closeSerial()

0
arduinosender/__init__.py Normal file
View File

47
pcbdevice/README.md Normal file
View File

@@ -0,0 +1,47 @@
# PCB Device
This program transforms a pcb drawing into a gcode.
## Getting started
To use this program, you need to launch main.py with the proper parameters or launch UI.py in /UI/ and enter the parameters in the boxes.
The PCB drawing your are using needs to be of format *.pbm ,
there are plenty of converters online to change your file format, such as [this converter we used](https://convertio.co/fr/pdf-pbm/).
### Prerequisites
Since the program is coded in python, you need to install python, we used python 3.7.
You can use an IDE or cmd to launch the program or the UI.
### Using the program
After downloading the project,
You can launch UI.py in S4-P3-Projet/pcbdevice/UI/ for the user interface of the program.
Enter the parameters in the proper boxes:
```
- Pcb drawing file path you want to convert into gcode, this file needs to be of type *.pbm
- Gcode file you want to save with the path where you want to save it.
If the file doesn't exist, a new file will be created, file type need to be *.gcode
- If the pbm file is of type binary or ascii. To find the type, you can open the file in a text editor,
ascii files will start with P1 while binary with P4. Also, birary type will contain unreadable characters.
- Width dimension of your pcb, units are entered later,
- Height dimension of your pcb, must be of the same units as width
- Radius of the tool you are using, units must be in mm.
- Units type for the width and height
```
Once all parameters are entered, you can click on the **execute program** button.
If everything is good, you should read **SUCCESS** on the bottom of the UI, else and error code should appear.
## People in this project
**Ian Lalonde**
**Marc-Antoine Lafrenière**
**Maxime Laporte**
**Guillaume Pépin**
**Louis-Philippe Baillargeon**

384
pcbdevice/opencrcontroler/GcodeInterpreter/GcodeInterpreter.ino Normal file
View File

@@ -0,0 +1,384 @@
/*******************************************************************************
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 <DynamixelWorkbench.h>
#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);
pinMode(emergencySwitchPin, 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, 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())
{
String read_string = Serial.readStringUntil('\n');
if(!isEmegencyState)
{
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){
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);
}

276
pcbdevice/opencrcontroler/OpenCR_5.0.ino
View File

@@ -1,276 +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 <DynamixelWorkbench.h>
// 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__)
#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;
// Positions Propertys :
int XPosition = 0;
int yPosition = 0;
int ZPosition = 0;
// 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);
int MovingDistance(String axe, float value_mm, uint8_t IDX, uint8_t IDY, uint8_t IDZ);
void setup()
{
Serial.begin(57600);
while(!Serial); // Open a Serial Monitor
//Motor 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);
Serial.println("Ready");
}
void loop() {
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;
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());
if(words[0] == "G0")
{
for(int i = 1; i < 3; 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){
Serial.println("1");
}
else{
Serial.println("-1");
}
}
}
}
//---------------------------------------------------------------------------------------------------------------------------------------------------------------------
// Functions :
//---------------------------------------------------------------------------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------------------------------------------------------
// Functions of Robotics Engeneering UdeS :
//---------------------------------------------------------------------------------------------------------------------------------------------------------------------
//##################
// Gcode Functions :
//##################
//########################
// High level Functions :
//########################
int MovingDistance(String axe, float value_mm, uint8_t IDX, uint8_t IDY, uint8_t IDZ){
// 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"){
MovingTick(IDX, value);
//Serial.println("X");
return 1;
}
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;
}
}
}
}
int MovingTick(uint8_t id, int32_t value){
String commande = "Goal_Position";
int32_t CurrentPosition = 0;
int MaxTick = 1048575;
int Iter = 0;
Torque_on(id);
Write(id, value, commande);
while(CurrentPosition < value-1){
CurrentPosition = Read(id, "Present_Position");
Iter += 1;
if(Iter >= MaxTick){
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;
}