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

Homing issues

Browse Source 
Fix homing if axis is reverse
This commit is contained in:
Marc-Antoine Lafreniere
2019-04-07 12:22:28 -04:00
parent 6c790dd0c3
commit 1de6f3efcb
1 changed files with 14 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
devicesoftware/GcodeInterpreter/GcodeInterpreter.ino
View File

@@ -94,7 +94,7 @@ void setup()
pinMode(ySwitchPin, INPUT_PULLUP); pinMode(ySwitchPin, INPUT_PULLUP);
pinMode(zSwitchPin, INPUT_PULLUP); pinMode(zSwitchPin, INPUT_PULLUP);
pinMode(emergencySwitchPin, INPUT_PULLUP); pinMode(emergencySwitchPin, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(emergencySwitchPin), LimiteSwitch, RISING); attachInterrupt(digitalPinToInterrupt(emergencySwitchPin), LimiteSwitch, FALLING);
Serial.begin(57600); Serial.begin(57600);
while(!Serial); // Open a Serial Monitor while(!Serial); // Open a Serial Monitor
@@ -120,7 +120,7 @@ void setup()
Write(idY, 0, HOMING_OFFSET); Write(idY, 0, HOMING_OFFSET);
Write(idZ, 0, HOMING_OFFSET); Write(idZ, 0, HOMING_OFFSET);
dxl_wb.writeRegister(idX, 10, 1, &X_REVERSE, &NULL_POINTER); dxl_wb.writeRegister(idX, 10, 1, & , &NULL_POINTER);
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);
@@ -238,12 +238,12 @@ int Homing()
int state = 0; int state = 0;
homing = true; homing = true;
state += HomingAxis(idZ, -50, xSwitchPin, homeOffsetZ); //state += HomingAxis(idZ, -50, zSwitchPin, homeOffsetZ);
state += HomingAxis(idY, -100, xSwitchPin, homeOffsetY); state += HomingAxis(idY, -100, ySwitchPin, homeOffsetY);
state += HomingAxis(idX, -100, xSwitchPin, homeOffsetX); state += HomingAxis(idX, -100, xSwitchPin, homeOffsetX);
homing = false; homing = false;
return state == 3 ? 1 : -1; return state == 2 ? 1 : -1;
} }
int HomingAxis(uint8_t id, int speed, int switchPin, int offset) int HomingAxis(uint8_t id, int speed, int switchPin, int offset)
@@ -252,7 +252,7 @@ int HomingAxis(uint8_t id, int speed, int switchPin, int offset)
Write(id, 0, HOMING_OFFSET); Write(id, 0, HOMING_OFFSET);
Torque_on(id); Torque_on(id);
Wheel(id, speed); Wheel(id, speed);
while(digitalRead(switchPin)); while(!digitalRead(switchPin));
OffsetAxe(id, offset); OffsetAxe(id, offset);
Write(id, 4, OPERATING_MODE); Write(id, 4, OPERATING_MODE);
return MovingTick(id, 0); return MovingTick(id, 0);
@@ -262,6 +262,14 @@ void OffsetAxe(uint8_t id, int offset){
int32_t posPresent = Read(id, PRESENT_POSITION); int32_t posPresent = Read(id, PRESENT_POSITION);
int32_t homePosition = - posPresent - offset; int32_t homePosition = - posPresent - offset;
Torque_off(id); Torque_off(id);
if(id == idX && X_REVERSE)
homePosition *= -1;
else if(id == idY && Y_REVERSE)
homePosition *= -1;
else if(id == idZ && Z_REVERSE)
homePosition *= -1;
Write(id, homePosition, HOMING_OFFSET); Write(id, homePosition, HOMING_OFFSET);
} }