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

pull request review and changes.

Browse Source 
- Refactoring following pull requests.
This commit is contained in:
Maxime
2019-03-27 10:59:08 -04:00
parent bf1dfbaf7d
commit 4753c2486b
1 changed files with 81 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files

154
pcbdevice/opencrcontroler/GcodeInterpreter.ino
View File

@@ -57,23 +57,22 @@ uint8_t scan_cnt = 0;
uint8_t ping_cnt = 0; uint8_t ping_cnt = 0;
const char *NULL_POINTER = NULL; const char *NULL_POINTER = NULL;
bool HOMING = 0;
// Motors Propertys : // Motors Propertys :
uint8_t idX = 11; uint8_t idX = 11;
uint8_t idY = 12; uint8_t idY = 12;
uint8_t idZ = 13; uint8_t idZ = 13;
// Positions Propertys : // Mecanicals propertys:
int XPosition = 0; float pouleyPitch = 2; //mm
int yPosition = 0; float nbTheets = 20;
int ZPosition = 0; int resMotorTick = 4096; //Ticks per revolution
// Limit Switch propertys // Limit Switch propertys
int XSwitchPin = 8; int xSwitchPin = 8;
int YSwitchPin = 9; int ySwitchPin = 9;
int ZSwitchPin = 10; int zSwitchPin = 10;
int EmergencySwitchPin = 2; int emergencySwitchPin = 2; // intrupt pin
// Homing variables // Homing variables
bool homing = 0; bool homing = 0;
@@ -95,23 +94,25 @@ void Torque_on(uint8_t id);
void Torque_off(uint8_t id); void Torque_off(uint8_t id);
void Write(uint8_t id, uint32_t value, String commande); void Write(uint8_t id, uint32_t value, String commande);
int32_t Read(uint8_t id, String commande); int32_t Read(uint8_t id, String commande);
void Led(uint8_t ID, bool state); void Led(uint8_t id, bool state);
void EmergencyStop(); void EmergencyStop();
void HomeAxe(String axe, uint8_t ID, int offset); void HomeAxe(uint8_t id, int offset);
void LimiteSwitch(uint8_t IDX, uint8_t IDY, uint8_t IDZ, bool homing); void LimiteSwitch();
int MovingDistance(String axe, float value_mm, uint8_t IDX, uint8_t IDY, uint8_t IDZ); int mmToTick(float value_mm, float pouleyPitch, float nbTheets, int resMotorTick);
Homing(uint8_t IDX, uint8_t IDY, uint8_t IDZ); int MovingTick(uint8_t id, int32_t value);
int MovingDistance(float value_mm, uint8_t id);
int Homing();
// Initialisation : // Initialisation :
void setup() void setup()
{ {
// Initialisation des pins : // Initialisation des pins :
pinMode(XSwitchPin, INPUT_PULLUP); pinMode(xSwitchPin, INPUT_PULLUP);
pinMode(YSwitchPin, INPUT_PULLUP); pinMode(ySwitchPin, INPUT_PULLUP);
pinMode(ZSwitchPin, INPUT_PULLUP); pinMode(zSwitchPin, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(EmergencySwitchPin), LimiteSwitch, RISING); attachInterrupt(digitalPinToInterrupt(emergencySwitchPin), LimiteSwitch, RISING);
Serial.begin(57600); Serial.begin(57600);
while(!Serial); // Open a Serial Monitor while(!Serial); // Open a Serial Monitor
@@ -136,7 +137,6 @@ void setup()
Torque_on(idX); Torque_on(idX);
Torque_on(idY); Torque_on(idY);
Torque_on(idZ); Torque_on(idZ);
//Serial.println("Ready");
} }
// Main Program // Main Program
@@ -171,7 +171,27 @@ if (Serial.available())
for(int i = 1; i < wordIndex+1; i++) for(int i = 1; i < wordIndex+1; i++)
{ {
float value = words[i].substring(1, words[i].length()).toFloat(); float value = words[i].substring(1, words[i].length()).toFloat();
statut[i] = MovingDistance((String)words[i].charAt(0), value, idX, idY, idZ);
switch ((char)words[i].charAt(0)){
case 'X' :
statut[i] = MovingDistance(value, idX);
break;
case 'Y' :
statut[i] = MovingDistance(value, idY);
break;
case 'Z' :
statut[i] = MovingDistance(value, idZ);
break;
default :
Serial.println("-1");
break;
}
} }
if((statut[0] == 1 || statut[0] == 0) && (statut[1] == 1 || statut[1] == 0) && (statut[2] == 1|| statut[2] == 0)){ if((statut[0] == 1 || statut[0] == 0) && (statut[1] == 1 || statut[1] == 0) && (statut[2] == 1|| statut[2] == 0)){
Serial.println("1"); Serial.println("1");
@@ -185,8 +205,7 @@ if (Serial.available())
// Call exemple : G28 // Call exemple : G28
else if(words[0] == "G28"){ else if(words[0] == "G28"){
Serial.println("2"); Serial.println("2");
Homing(idX, idY, idZ); Serial.println(Homing());
Serial.println("1");
} }
// Torque OFF all M18 // Torque OFF all M18
@@ -231,60 +250,43 @@ if (Serial.available())
// Functions of Robotics Engeneering UdeS : // Functions of Robotics Engeneering UdeS :
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------- //---------------------------------------------------------------------------------------------------------------------------------------------------------------------
//##################
// Gcode Functions :
//##################
//######################## //########################
// High level Functions : // High level Functions :
//######################## //########################
int Homing(uint8_t IDX, uint8_t IDY, uint8_t IDZ){ int Homing(){
homing = 1; homing = 1;
// home Z : // home Z :
homingZ = HIGH; homingZ = true;
homingY = LOW; homingY = false;
homingX = LOW; homingX = false;
Torque_on(IDZ); Torque_on(idZ);
Wheel(IDZ, -50); Wheel(idZ, -50);
// home y : // home y :
homingZ = LOW; homingZ = false;
homingY = HIGH; homingY = true;
homingX = LOW; homingX = false;
Torque_on(IDY); Torque_on(idY);
Wheel(IDZ, -100); Wheel(idY, -100);
// home x : // home x :
homingZ = LOW; homingZ = false;
homingY = LOW; homingY = false;
homingX = HIGH; homingX = true;
Torque_on(IDX); Torque_on(idX);
Wheel(IDZ, -100); Wheel(idX, -100);
return 1;
} }
int MovingDistance(String axe, float value_mm, uint8_t IDX, uint8_t IDY, uint8_t IDZ){ int MovingDistance(float value_mm, uint8_t id){
return MovingTick(id, mmToTick(value_mm, pouleyPitch, nbTheets, resMotorTick));
// 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"){
return MovingTick(IDX, value);
}
else if(axe == "Y"){
return MovingTick(IDY, value);
}
else if(axe == "Z"){
return MovingTick(IDZ, value);
} }
int mmToTick(float value_mm, float pouleyPitch, float nbTheets, int resMotorTick){
int32_t value = ((value_mm/(pouleyPitch*nbTheets))/resMotorTick);
return value;
} }
int MovingTick(uint8_t id, int32_t value){ int MovingTick(uint8_t id, int32_t value){
@@ -389,8 +391,8 @@ int32_t Read(uint8_t id, String commande){
return data; return data;
} }
void Led(uint8_t ID, bool state){ void Led(uint8_t id, bool state){
Write(ID, (uint32_t)state, "LED"); Write(id, (uint32_t)state, "LED");
} }
void EmergencyStop(){ void EmergencyStop(){
@@ -400,24 +402,24 @@ void EmergencyStop(){
Serial.println(-1); Serial.println(-1);
} }
void HomeAxe(String axe, uint8_t ID, int offset){ void HomeAxe(uint8_t id, int offset){
Torque_off(ID); Torque_off(id);
Torque_on(ID); Torque_on(id);
MovingDistance(axe, offset, idX, idY, idZ); MovingDistance(offset, id);
int homingPosition = Read(ID, "Present_Position"); int homingPosition = Read(id, "Present_Position");
Write(ID, (uint32_t)homingPosition, "Homing_Offset"); Write(id, (uint32_t)homingPosition, "Homing_Offset");
} }
void LimiteSwitch(){ void LimiteSwitch(){
if(homing){ if(homing){
if(homingZ == HIGH && digitalRead(ZSwitchPin) == HIGH){ if(homingZ == HIGH && digitalRead(zSwitchPin) == HIGH){
HomeAxe("Z", idZ, offsetZ); HomeAxe(idZ, offsetZ);
} }
else if(homingY == HIGH && digitalRead(YSwitchPin) == HIGH){ else if(homingY == HIGH && digitalRead(ySwitchPin) == HIGH){
HomeAxe("Y", idY, offsetY); HomeAxe(idY, offsetY);
} }
else if(homingX == HIGH && digitalRead(XSwitchPin) == HIGH){ else if(homingX == HIGH && digitalRead(xSwitchPin) == HIGH){
HomeAxe("X", idX, offsetX); HomeAxe(idX, offsetX);
} }
} }
else{ else{