Merge branch 'master' into #55_Create_UI_frame
This commit is contained in:
Ian
206
arduinosender/CommunicationMain.py
Normal file
206
arduinosender/CommunicationMain.py
Normal 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
0
arduinosender/__init__.py
Normal file
47
pcbdevice/README.md
Normal file
47
pcbdevice/README.md
Normal 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
384
pcbdevice/opencrcontroler/GcodeInterpreter/GcodeInterpreter.ino
Normal 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);
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user