// Reset and initialize the motor controller.
bool Motor::Initialize()
{
std::vector<MotorCommand> commands;
// perform a software reset
if (!MotorCommand(MC_GENERAL_REG, MC_RESET).Send(_i2cDevice))
return false;
// wait for the reset to complete before sending any commands
// (that would otherwise be erased as part of the reset)
usleep(DELAY_AFTER_RESET_MSEC * 1000);
// set up parameters applying to all Z motions
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_STEP_ANGLE,
_settings.GetInt(Z_STEP_ANGLE)));
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_UNITS_PER_REV,
_settings.GetInt(Z_MICRONS_PER_REV)));
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_MICROSTEPPING,
_settings.GetInt(MICRO_STEPS_MODE)));
// set up parameters applying to all rotations
commands.push_back(MotorCommand(MC_ROT_SETTINGS_REG, MC_STEP_ANGLE,
_settings.GetInt(R_STEP_ANGLE)));
commands.push_back(MotorCommand(MC_ROT_SETTINGS_REG, MC_UNITS_PER_REV,
_settings.GetInt(R_MILLIDEGREES_PER_REV) / R_SCALE_FACTOR));
commands.push_back(MotorCommand(MC_ROT_SETTINGS_REG, MC_MICROSTEPPING,
_settings.GetInt(MICRO_STEPS_MODE)));
// enable the motors
commands.push_back(MotorCommand(MC_GENERAL_REG, MC_ENABLE));
// no interrupt is needed here since no movement was requested
return SendCommands(commands);
}
// Clear pending motor command(s). Used when canceling a print, after a pause.
// Interrupt should be requested if the motor may not have completed the
// preceding pause yet.
bool Motor::ClearPendingCommands(bool withInterrupt)
{
std::vector<MotorCommand> commands;
commands.push_back(MotorCommand(MC_GENERAL_REG, MC_CLEAR));
if (withInterrupt)
{
// request an interrupt, to avoid sending new motor commands before the
// clear has been completed
commands.push_back(MotorCommand(MC_GENERAL_REG, MC_INTERRUPT));
}
return SendCommands(commands);
}
void BarrettHand::smoothSpreadParams()
{
MotorCommand(BARRETT_SPREAD,"FSET SAMPLE 50");
this->SkipUntilPrompt();
fprintf(stderr,"SAMPLE 50\n");
MotorCommand(BARRETT_SPREAD,"FSET ACCEL 1");
this->SkipUntilPrompt();
fprintf(stderr,"ACCEL 1\n");
MotorCommand(BARRETT_SPREAD,"FSET MCV 18");
this->SkipUntilPrompt();
fprintf(stderr,"MCV 18\n");
MotorCommand(BARRETT_SPREAD,"FSET MOV 18");
this->SkipUntilPrompt();
fprintf(stderr,"MOV 18\n");
fprintf(stderr,"All smooth spread params sent to hand\n");
}
// Rotate the tray and (if CanInspect is true) lift the build head to inspect
// the print in progress.
bool Motor::PauseAndInspect(const CurrentLayerSettings& cls)
{
std::vector<MotorCommand> commands;
// use same speeds & jerks as used for homing, since we're already separated
commands.push_back(MotorCommand(MC_ROT_SETTINGS_REG, MC_JERK,
_settings.GetInt(R_HOMING_JERK)));
commands.push_back(MotorCommand(MC_ROT_SETTINGS_REG, MC_SPEED,
R_SPEED_FACTOR * _settings.GetInt(R_HOMING_SPEED)));
// rotate the tray to cover stray light from the projector
int rotation = cls.RotationMilliDegrees / R_SCALE_FACTOR;
if (rotation != 0)
commands.push_back(MotorCommand(MC_ROT_ACTION_REG, MC_MOVE, -rotation));
if (cls.CanInspect)
{
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_JERK,
_settings.GetInt(Z_HOMING_JERK)));
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_SPEED,
Z_SPEED_FACTOR * _settings.GetInt(Z_HOMING_SPEED)));
// lift the build head for inspection
commands.push_back(MotorCommand(MC_Z_ACTION_REG, MC_MOVE,
cls.InspectionHeightMicrons));
}
// request an interrupt when these commands are completed
commands.push_back(MotorCommand(MC_GENERAL_REG, MC_INTERRUPT));
return SendCommands(commands);
}
// Rotate the tray and (if CanInspect is true) lower the build head from the
// inspection position, to resume printing.
bool Motor::ResumeFromInspect(const CurrentLayerSettings& cls)
{
std::vector<MotorCommand> commands;
// use same speeds & jerks as used for moving to start position,
// since we're already calibrated
// set rotation parameters
commands.push_back(MotorCommand(MC_ROT_SETTINGS_REG, MC_JERK,
_settings.GetInt(R_START_PRINT_JERK)));
commands.push_back(MotorCommand(MC_ROT_SETTINGS_REG, MC_SPEED,
R_SPEED_FACTOR * _settings.GetInt(R_START_PRINT_SPEED)));
// rotate the tray back into exposing position
int rotation = cls.RotationMilliDegrees / R_SCALE_FACTOR;
if (rotation != 0)
commands.push_back(MotorCommand(MC_ROT_ACTION_REG, MC_MOVE, rotation));
if (cls.CanInspect)
{
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_JERK,
_settings.GetInt(Z_START_PRINT_JERK)));
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_SPEED,
Z_SPEED_FACTOR * _settings.GetInt(Z_START_PRINT_SPEED)));
// lower the build head for exposure
commands.push_back(MotorCommand(MC_Z_ACTION_REG, MC_MOVE,
-cls.InspectionHeightMicrons));
}
// request an interrupt when these commands are completed
commands.push_back(MotorCommand(MC_GENERAL_REG, MC_INTERRUPT));
return SendCommands(commands);
}
// Separate the current layer
bool Motor::Separate(const CurrentLayerSettings& cls)
{
std::vector<MotorCommand> commands;
// rotate the previous layer from the PDMS
commands.push_back(MotorCommand(MC_ROT_SETTINGS_REG, MC_JERK,
cls.SeparationRotJerk));
commands.push_back(MotorCommand(MC_ROT_SETTINGS_REG, MC_SPEED,
cls.SeparationRPM * R_SPEED_FACTOR));
int rotation = cls.RotationMilliDegrees / R_SCALE_FACTOR;
if (rotation != 0)
commands.push_back(MotorCommand(MC_ROT_ACTION_REG, MC_MOVE, -rotation));
// lift the build platform
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_JERK,
cls.SeparationZJerk));
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_SPEED,
cls.SeparationMicronsPerSec * Z_SPEED_FACTOR));
if (cls.ZLiftMicrons != 0)
commands.push_back(MotorCommand(MC_Z_ACTION_REG, MC_MOVE,
cls.ZLiftMicrons));
// request an interrupt when these commands are completed
commands.push_back(MotorCommand(MC_GENERAL_REG, MC_INTERRUPT));
return SendCommands(commands);
}
// Move the tray back up into position for exposing the next layer, allowing
// resin to fill in for the height of a full layer.
bool Motor::Unpress(const CurrentLayerSettings& cls)
{
// reuse existing jerk settings from approach
std::vector<MotorCommand> commands;
// lift up on the tray
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_SPEED,
cls.UnpressMicronsPerSec * Z_SPEED_FACTOR));
if (cls.PressMicrons != 0)
commands.push_back(MotorCommand(MC_Z_ACTION_REG, MC_MOVE,
cls.PressMicrons));
// request an interrupt when these commands are completed
commands.push_back(MotorCommand(MC_GENERAL_REG, MC_INTERRUPT));
return SendCommands(commands);
}
// Attempt to recover from a jam by homing the build tray. It's up to the
// caller to determine if the anti-jam sensor is successfully triggered
// during the attempt. This move (without the interrupt request)is also
// required before resuming after a manual recovery, in order first to
// align the tray correctly.
bool Motor::UnJam(const CurrentLayerSettings& cls, bool withInterrupt)
{
// assumes speed & jerk have already
// been set as needed for separation from the current layer type
std::vector<MotorCommand> commands;
// rotate to the home position (but no more than a full rotation)
commands.push_back(MotorCommand(MC_ROT_ACTION_REG, MC_HOME,
UNITS_PER_REVOLUTION));
int rotation = cls.RotationMilliDegrees / R_SCALE_FACTOR;
if (rotation != 0)
commands.push_back(MotorCommand(MC_ROT_ACTION_REG, MC_MOVE, -rotation));
if (withInterrupt)
{
// request an interrupt when these commands are completed
commands.push_back(MotorCommand(MC_GENERAL_REG, MC_INTERRUPT));
}
return SendCommands(commands);
}
// Go to the position for exposing the next layer (with optional jam recovery
// motion first).
bool Motor::Approach(const CurrentLayerSettings& cls, bool unJamFirst)
{
if (unJamFirst)
if (!UnJam(cls, false))
return false;
std::vector<MotorCommand> commands;
// rotate back to the PDMS
commands.push_back(MotorCommand(MC_ROT_SETTINGS_REG, MC_JERK,
cls.ApproachRotJerk));
commands.push_back(MotorCommand(MC_ROT_SETTINGS_REG, MC_SPEED,
cls.ApproachRPM * R_SPEED_FACTOR));
int rotation = cls.RotationMilliDegrees / R_SCALE_FACTOR;
if (rotation != 0)
{
// see if we should use homing on approach, to avoid not rotating far
// enough back when there's been drag (a partial jam) on separation
if (_settings.GetInt(HOME_ON_APPROACH) != 0)
commands.push_back(MotorCommand(MC_ROT_ACTION_REG, MC_HOME,
2 * rotation));
else
commands.push_back(MotorCommand(MC_ROT_ACTION_REG, MC_MOVE,
rotation));
}
// lower into position to expose the next layer
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_JERK,
cls.ApproachZJerk));
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_SPEED,
cls.ApproachMicronsPerSec * Z_SPEED_FACTOR));
int deltaZ = cls.LayerThicknessMicrons - cls.ZLiftMicrons;
if (deltaZ != 0)
commands.push_back(MotorCommand(MC_Z_ACTION_REG, MC_MOVE, deltaZ));
// request an interrupt when these commands are completed
commands.push_back(MotorCommand(MC_GENERAL_REG, MC_INTERRUPT));
return SendCommands(commands);
}
// Move the motors to their home position, with optional interrupt such that
// it may be chained with GoToStartPosition() with only a single interrupt at
// the end of both. Also with options to not do a rotation homing (to avoid
// crashing into a completed print, when there's been partial jams) and/or to
// keep the tray's window in the open position, in support of demo mode.
bool Motor::GoHome(bool withInterrupt, bool rotateHome, bool stayOpen)
{
std::vector<MotorCommand> commands;
// set rotation parameters
commands.push_back(MotorCommand(MC_ROT_SETTINGS_REG, MC_JERK,
_settings.GetInt(R_HOMING_JERK)));
commands.push_back(MotorCommand(MC_ROT_SETTINGS_REG, MC_SPEED,
R_SPEED_FACTOR * _settings.GetInt(R_HOMING_SPEED)));
if(rotateHome)
{
// rotate to the home position (but no more than a full rotation)
commands.push_back(MotorCommand(MC_ROT_ACTION_REG, MC_HOME,
UNITS_PER_REVOLUTION));
}
int homeAngle = _settings.GetInt(R_HOMING_ANGLE) / R_SCALE_FACTOR;
if (homeAngle != 0 && !stayOpen)
{
// rotate 60 degrees back
commands.push_back(MotorCommand(MC_ROT_ACTION_REG, MC_MOVE, homeAngle));
}
// set Z motion parameters
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_JERK,
_settings.GetInt(Z_HOMING_JERK)));
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_SPEED,
Z_SPEED_FACTOR * _settings.GetInt(Z_HOMING_SPEED)));
// go up to the Z home position (but no more than twice the max Z travel)
commands.push_back(MotorCommand(MC_Z_ACTION_REG, MC_HOME,
-2 * _settings.GetInt(Z_START_PRINT_POSITION)));
if (withInterrupt)
{
// request an interrupt when these commands are completed
commands.push_back(MotorCommand(MC_GENERAL_REG, MC_INTERRUPT));
}
return SendCommands(commands);
}
void StateMachine(unsigned int SetPoint[row][col]){
if ( (flagM1 == 1) && (flagM2 == 1) && (flagM3 == 1) && (flagM4 == 1) && (flagM5 == 1) && (j < row) ){
j++;
flagM1 = 0;
flagM2 = 0;
flagM3 = 0;
flagM4 = 0;
flagM5 = 0;
__delay_cycles(630000);
}
if ( j == row){
j=0;
}
SetTarget[i] = SetPoint[j][i]; // Seta posição destino do motor em questão.
MotorCommand(SetTarget, Gap); //lógica para acionamento do motor
}
// Goes to home position (without interrupt), then lowers the build platform to
// the PDMS in order to calibrate and/or start a print
bool Motor::GoToStartPosition()
{
EnableMotors();
GoHome(false);
std::vector<MotorCommand> commands;
// set rotation parameters
commands.push_back(MotorCommand(MC_ROT_SETTINGS_REG, MC_JERK,
_settings.GetInt(R_START_PRINT_JERK)));
commands.push_back(MotorCommand(MC_ROT_SETTINGS_REG, MC_SPEED,
R_SPEED_FACTOR * _settings.GetInt(R_START_PRINT_SPEED)));
int startAngle = _settings.GetInt(R_START_PRINT_ANGLE) / R_SCALE_FACTOR;
if (startAngle != 0)
{
// rotate to the start position
commands.push_back(MotorCommand(MC_ROT_ACTION_REG, MC_MOVE,
startAngle));
}
// set Z motion parameters
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_JERK,
_settings.GetInt(Z_START_PRINT_JERK)));
commands.push_back(MotorCommand(MC_Z_SETTINGS_REG, MC_SPEED,
Z_SPEED_FACTOR * _settings.GetInt(Z_START_PRINT_SPEED)));
// move down to the PDMS
commands.push_back(MotorCommand(MC_Z_ACTION_REG, MC_MOVE,
_settings.GetInt(Z_START_PRINT_POSITION)));
// request an interrupt when these commands are completed
commands.push_back(MotorCommand(MC_GENERAL_REG, MC_INTERRUPT));
return SendCommands(commands);
}
// Resume the motor command(s) pending at last pause (if any).
bool Motor::Resume()
{
return MotorCommand(MC_GENERAL_REG, MC_RESUME).Send(_i2cDevice);
}
// Pause the current motor command(s) in progress (if any).
bool Motor::Pause()
{
return MotorCommand(MC_GENERAL_REG, MC_PAUSE).Send(_i2cDevice);
}
// Disable (disengage) both motors. Return false if they can't be disabled.
bool Motor::DisableMotors()
{
return MotorCommand(MC_GENERAL_REG, MC_DISABLE).Send(_i2cDevice);
}
// Enable (engage) both motors. Return false if they can't be enabled.
bool Motor::EnableMotors()
{
return MotorCommand(MC_GENERAL_REG, MC_ENABLE).Send(_i2cDevice);
}