/**
* Start simultaneously movement of all motors
**/
void MotorManager::startMovement(int motionSlot) {
if(!poweredOn) {
throw MotorException("motor manager is not powered on");
}
// Execute motion.
for(int i = 0; i < motors.size(); ++i) {
if(motors[i]->isPoweredOn())
{
motors[i]->waitTillReady();
}
}
long timer2 = rexos_utilities::timeNow();
modbus->writeU16(CRD514KD::Slaves::BROADCAST, CRD514KD::Registers::CMD_1, motionSlot | CRD514KD::CMD1Bits::EXCITEMENT_ON | CRD514KD::CMD1Bits::START);
modbus->writeU16(CRD514KD::Slaves::BROADCAST, CRD514KD::Registers::CMD_1, CRD514KD::CMD1Bits::EXCITEMENT_ON);
//REXOS_INFO_STREAM("modbus writeU16 time: " << (rexos_utilities::timeNow()-timer2));
for(int i = 0; i < motors.size(); ++i) {
if(motors[i]->isPoweredOn())
{
motors[i]->updateAngle();
}
}
}
/**
* Stops the motors & clears the motion queue.
*
* @note (un)locks queue_mutex.
**/
void StepperMotor::stop(void){
if(!poweredOn){
throw MotorException("motor drivers are not powered on");
}
try{
modbus->writeU16(motorIndex, CRD514KD::Registers::CMD_1, CRD514KD::CMD1Bits::STOP);
modbus->writeU16(motorIndex, CRD514KD::Registers::CMD_1, CRD514KD::CMD1Bits::EXCITEMENT_ON);
} catch(ModbusController::ModbusException& exception){
std::cerr << "steppermotor::stop failed: " << std::endl << "what(): " << exception.what() << std::endl;
}
}
/**
* Start the motor to move according to the set registers.
**/
void StepperMotor::startMovement(void){
if(!poweredOn){
throw MotorException("motor drivers are not powered on");
}
// Execute motion.
waitTillReady();
modbus->writeU16(motorIndex, CRD514KD::Registers::CMD_1, CRD514KD::CMD1Bits::EXCITEMENT_ON);
modbus->writeU16(motorIndex, CRD514KD::Registers::CMD_1, CRD514KD::CMD1Bits::EXCITEMENT_ON | CRD514KD::CMD1Bits::START);
modbus->writeU16(motorIndex, CRD514KD::Registers::CMD_1, CRD514KD::CMD1Bits::EXCITEMENT_ON);
updateAngle();
}
/**
* Start simultaneously movement of all motors
**/
void MotorManager::startMovement(void){
if(!poweredOn){
throw MotorException("motor manager is not powered on");
}
// Execute motion.
motors[0]->waitTillReady();
motors[1]->waitTillReady();
motors[2]->waitTillReady();
modbus->writeU16(CRD514KD::Slaves::BROADCAST, CRD514KD::Registers::CMD_1, CRD514KD::CMD1Bits::EXCITEMENT_ON);
modbus->writeU16(CRD514KD::Slaves::BROADCAST, CRD514KD::Registers::CMD_1, CRD514KD::CMD1Bits::EXCITEMENT_ON | CRD514KD::CMD1Bits::START);
modbus->writeU16(CRD514KD::Slaves::BROADCAST, CRD514KD::Registers::CMD_1, CRD514KD::CMD1Bits::EXCITEMENT_ON);
motors[0]->updateAngle();
motors[1]->updateAngle();
motors[2]->updateAngle();
}
/**
* Writes the rotation data into the motor controller.
*
* @param motorRotation A MotorRotation.
**/
void StepperMotor::writeRotationData(const DataTypes::MotorRotation<double>& motorRotation){
if(!poweredOn){
throw MotorException("motor drivers are not powered on");
}
if(anglesLimited && (motorRotation.angle <= minAngle || motorRotation.angle >= maxAngle)){
throw std::out_of_range("one or more angles out of range");
}
uint32_t motorSteps = (uint32_t)((motorRotation.angle + deviation) / CRD514KD::MOTOR_STEP_ANGLE);
uint32_t motorSpeed = (uint32_t)(motorRotation.speed / CRD514KD::MOTOR_STEP_ANGLE);
// TODO: figure out unknown magical variable 1000000000.0
uint32_t motorAcceleration = (uint32_t)(CRD514KD::MOTOR_STEP_ANGLE * 1000000000.0 / motorRotation.acceleration);
uint32_t motorDeceleration = (uint32_t)(CRD514KD::MOTOR_STEP_ANGLE * 1000000000.0 / motorRotation.deceleration);
modbus->writeU32(motorIndex, CRD514KD::Registers::OP_SPEED, motorSpeed, true);
modbus->writeU32(motorIndex, CRD514KD::Registers::OP_POS, motorSteps, true);
modbus->writeU32(motorIndex, CRD514KD::Registers::OP_ACC, motorAcceleration, true);
modbus->writeU32(motorIndex, CRD514KD::Registers::OP_DEC, motorDeceleration, true);
setAngle = motorRotation.angle;
}
