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

Merge pull request #62 from marc4492/#51-BuildingofG-28request

Browse Source 
Building of G-28 request
This commit is contained in:
Marc-Antoine
2019-04-06 12:57:17 -04:00
committed by GitHub
parent 63152a5991 06da9b8bb4
commit d5d56968b8
2 changed files with 384 additions and 276 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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;
}