/**
 * 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();
	}
Exemplo n.º 4
0
	/**
	 * 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;
	}