Merge branch 'master' of https://github.com/TimEvWw/farmbot-arduino-controller
commit
4294d04437
|
@ -86,7 +86,7 @@ void CurrentState::setEndStopState(unsigned int axis, unsigned int position, boo
|
|||
endStopState[axis][position] = state;
|
||||
}
|
||||
|
||||
void CurrentState::setStepsPerMm(int stepsX, int stepsY, int stepsZ)
|
||||
void CurrentState::setStepsPerMm(long stepsX, long stepsY, long stepsZ)
|
||||
{
|
||||
stepsPerMmX = stepsX;
|
||||
stepsPerMmY = stepsY;
|
||||
|
|
|
@ -43,16 +43,16 @@ public:
|
|||
void resetEmergencyStop();
|
||||
bool isEmergencyStop();
|
||||
|
||||
void setStepsPerMm(int stepsX, int stepsY, int stepsZ);
|
||||
void setStepsPerMm(long stepsX, long stepsY, long stepsZ);
|
||||
|
||||
private:
|
||||
CurrentState();
|
||||
CurrentState(CurrentState const &);
|
||||
void operator=(CurrentState const &);
|
||||
|
||||
int stepsPerMmX;
|
||||
int stepsPerMmY;
|
||||
int stepsPerMmZ;
|
||||
long stepsPerMmX;
|
||||
long stepsPerMmY;
|
||||
long stepsPerMmZ;
|
||||
|
||||
bool emergencyStop = false;
|
||||
};
|
||||
|
|
|
@ -148,35 +148,60 @@ int ParameterList::paramChangeNumber()
|
|||
|
||||
// ===== eeprom handling ====
|
||||
|
||||
int ParameterList::readValueEeprom(int id)
|
||||
long ParameterList::readValueEeprom(int id)
|
||||
{
|
||||
|
||||
// Assume all values are ints and calculate address for that
|
||||
int address = id * 2;
|
||||
|
||||
//Read the 2 bytes from the eeprom memory.
|
||||
long two = EEPROM.read(address);
|
||||
long one = EEPROM.read(address + 1);
|
||||
long one = EEPROM.read(address + 0);
|
||||
long two = EEPROM.read(address + 1);
|
||||
|
||||
long three = 0;
|
||||
long four = 0;
|
||||
|
||||
if (id == 141 || id == 142 || id == 143)
|
||||
{
|
||||
three = EEPROM.read(address + 20);
|
||||
four = EEPROM.read(address + 21);
|
||||
}
|
||||
|
||||
// just in case there are non uninitialized EEPROM bytes
|
||||
// put them both to zero
|
||||
if (three == -1 && four == -1)
|
||||
{
|
||||
three = 0;
|
||||
four = 0;
|
||||
}
|
||||
|
||||
//Return the recomposed long by using bitshift.
|
||||
return ((two << 0) & 0xFF) + ((one << 8) & 0xFFFF);
|
||||
return ((one << 0) & 0xFF) + ((two << 8) & 0xFF00) + ((three << 16) & 0xFF0000); +((four << 24) & 0xFF000000);
|
||||
}
|
||||
|
||||
int ParameterList::writeValueEeprom(int id, int value)
|
||||
int ParameterList::writeValueEeprom(int id, long value)
|
||||
{
|
||||
|
||||
// Assume all values are ints and calculate address for that
|
||||
int address = id * 2;
|
||||
|
||||
//Decomposition from a int to 2 bytes by using bitshift.
|
||||
//One = Most significant -> Two = Least significant byte
|
||||
byte two = (value & 0xFF);
|
||||
byte one = ((value >> 8) & 0xFF);
|
||||
//One = Least significant -> Four = Most significant byte
|
||||
byte one= (value & 0xFF);
|
||||
byte two = ((value >> 8) & 0xFF);
|
||||
byte three = ((value >> 16) & 0xFF);
|
||||
byte four = ((value >> 24) & 0xFF);
|
||||
|
||||
//Write the 4 bytes into the eeprom memory.
|
||||
EEPROM.write(address, two);
|
||||
EEPROM.write(address + 1, one);
|
||||
EEPROM.write(address + 0, one);
|
||||
EEPROM.write(address + 1, two);
|
||||
|
||||
// Only this parameter needs a long value
|
||||
if (id == 141 || id == 142 || id == 143)
|
||||
{
|
||||
EEPROM.write(address + 20, one);
|
||||
EEPROM.write(address + 21, two);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
|
@ -105,11 +105,15 @@ enum ParamListEnum
|
|||
ENCODER_INVERT_Y = 132,
|
||||
ENCODER_INVERT_Z = 133,
|
||||
|
||||
// sizes of axis
|
||||
// sizes of axis, lower byte and higher byte
|
||||
MOVEMENT_AXIS_NR_STEPS_X = 141,
|
||||
MOVEMENT_AXIS_NR_STEPS_Y = 142,
|
||||
MOVEMENT_AXIS_NR_STEPS_Z = 143,
|
||||
|
||||
MOVEMENT_AXIS_NR_STEPS_H_X = 151,/**/
|
||||
MOVEMENT_AXIS_NR_STEPS_H_Y = 152,
|
||||
MOVEMENT_AXIS_NR_STEPS_H_Z = 153,
|
||||
|
||||
// stop at end of axis
|
||||
MOVEMENT_STOP_AT_MAX_X = 145,
|
||||
MOVEMENT_STOP_AT_MAX_Y = 146,
|
||||
|
@ -160,8 +164,8 @@ public:
|
|||
int writeAllValuesToEeprom();
|
||||
int setAllValuesToDefault();
|
||||
|
||||
int readValueEeprom(int id);
|
||||
int writeValueEeprom(int id, int value);
|
||||
long readValueEeprom(int id);
|
||||
int writeValueEeprom(int id, long value);
|
||||
|
||||
void sendConfigToModules();
|
||||
|
||||
|
|
|
@ -18,6 +18,9 @@ unsigned long currentTime;
|
|||
unsigned long cycleCounter = 0;
|
||||
bool previousEmergencyStop = false;
|
||||
|
||||
unsigned long pinGuardLastCheck;
|
||||
unsigned long pinGuardCurrentTime;
|
||||
|
||||
int lastParamChangeNr = 0;
|
||||
|
||||
String commandString = "";
|
||||
|
@ -50,11 +53,11 @@ void interrupt(void)
|
|||
{
|
||||
if (!debugInterrupt)
|
||||
{
|
||||
interruptSecondTimer++;
|
||||
//interruptSecondTimer++;
|
||||
|
||||
if (interruptBusy == false)
|
||||
{
|
||||
interruptStartTime = micros();
|
||||
//interruptStartTime = micros();
|
||||
|
||||
interruptBusy = true;
|
||||
StepperControl::getInstance()->handleMovementInterrupt();
|
||||
|
@ -67,17 +70,17 @@ void interrupt(void)
|
|||
// //blinkLed();
|
||||
//}
|
||||
|
||||
interruptStopTime = micros();
|
||||
//interruptStopTime = micros();
|
||||
|
||||
if (interruptStopTime > interruptStartTime)
|
||||
{
|
||||
interruptDuration = interruptStopTime - interruptStartTime;
|
||||
}
|
||||
//if (interruptStopTime > interruptStartTime)
|
||||
//{
|
||||
// interruptDuration = interruptStopTime - interruptStartTime;
|
||||
//}
|
||||
|
||||
if (interruptDuration > interruptDurationMax)
|
||||
{
|
||||
interruptDurationMax = interruptDuration;
|
||||
}
|
||||
//if (interruptDuration > interruptDurationMax)
|
||||
//{
|
||||
// interruptDurationMax = interruptDuration;
|
||||
//}
|
||||
|
||||
interruptBusy = false;
|
||||
}
|
||||
|
@ -170,6 +173,7 @@ void setup()
|
|||
pinMode(X_STEP_PIN, OUTPUT);
|
||||
pinMode(X_DIR_PIN, OUTPUT);
|
||||
pinMode(X_ENABLE_PIN, OUTPUT);
|
||||
|
||||
pinMode(E_STEP_PIN, OUTPUT);
|
||||
pinMode(E_DIR_PIN, OUTPUT);
|
||||
pinMode(E_ENABLE_PIN, OUTPUT);
|
||||
|
@ -253,6 +257,7 @@ void setup()
|
|||
|
||||
// Get the settings for the pin guard
|
||||
PinGuard::getInstance()->loadConfig();
|
||||
pinGuardLastCheck = millis();
|
||||
|
||||
// Start the interrupt used for moving
|
||||
// Interrupt management code library written by Paul Stoffregen
|
||||
|
@ -265,6 +270,9 @@ void setup()
|
|||
// Initialize the inactivity check
|
||||
lastAction = millis();
|
||||
|
||||
pinGuardCurrentTime = millis();
|
||||
pinGuardLastCheck = millis();
|
||||
|
||||
if (ParameterList::getInstance()->getValue(MOVEMENT_HOME_AT_BOOT_Z) == 1)
|
||||
{
|
||||
Serial.print("R99 HOME Z ON STARTUP\r\n");
|
||||
|
@ -298,6 +306,22 @@ void loop()
|
|||
StepperControl::getInstance()->handleMovementInterrupt();
|
||||
}
|
||||
|
||||
pinGuardCurrentTime = millis();
|
||||
if (pinGuardCurrentTime < pinGuardLastCheck)
|
||||
{
|
||||
pinGuardLastCheck = pinGuardCurrentTime;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (pinGuardCurrentTime - pinGuardLastCheck >= 1000)
|
||||
{
|
||||
pinGuardLastCheck += 1000;
|
||||
|
||||
// check the pins for timeouts
|
||||
PinGuard::getInstance()->checkPins();
|
||||
}
|
||||
}
|
||||
|
||||
if (Serial.available())
|
||||
{
|
||||
|
||||
|
|
|
@ -134,13 +134,16 @@
|
|||
#define X_STEP_PIN 26 // X1_STEP_PIN
|
||||
#define X_DIR_PIN 27 // X1_DIR_PIN
|
||||
#define X_ENABLE_PIN 25 // X1_ENABLE_PIN
|
||||
|
||||
#define E_STEP_PIN 15 // X2_STEP_PIN
|
||||
#define E_DIR_PIN 30 // X2_DIR_PIN
|
||||
#define E_ENABLE_PIN 14 // X2_ENABLE_PIN
|
||||
|
||||
#define X_MIN_PIN 69
|
||||
#define X_MAX_PIN 68
|
||||
#define X_ENCDR_A 16
|
||||
#define X_ENCDR_B 17
|
||||
|
||||
#define X2_ENCDR_A 22
|
||||
#define X2_ENCDR_B 39
|
||||
#define X_ENCDR_A_Q -1 // N/A
|
||||
|
|
|
@ -174,7 +174,7 @@
|
|||
</ImportGroup>
|
||||
<ProjectExtensions>
|
||||
<VisualStudio>
|
||||
<UserProperties vm.programmer_name="avrisp" arduino.upload.port="COM4" />
|
||||
<UserProperties vm.programmer_name="avrisp" arduino.upload.port="COM5" />
|
||||
</VisualStudio>
|
||||
</ProjectExtensions>
|
||||
</Project>
|
Loading…
Reference in New Issue