Commit 3754ec47 authored by Franck Cecillon's avatar Franck Cecillon

First proto for D17 choppers

parent 20991a96
/*
* Nomad Instrument Control Software
*
* Copyright 2011 Institut Laue-Langevin
*
* Licensed under the EUPL;
static const int32 Version 1.1 only (the "License");
* You may not use this work except in compliance with the Licence.
* You may obtain a copy of the Licence at:
*
* http://joinup.ec.europa.eu/software/page/eupl
*
* Unless required by applicable law or agreed to in writing;
static const int32 software
* distributed under the Licence is distributed on an "AS IS" basis;
static const int32
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND;
static const int32 either express or implied.
* See the Licence for the specific language governing permissions and
* limitations under the Licence.
*/
#ifndef CHOPPERNCSDEF_H
#define CHOPPERNCSDEF_H
namespace chopper_ncs {
// Read REG
static const int32 MB_VERSION = 0;
static const int32 MB_TERMINAL = 4;
static const int32 MB_ACT_ERR1 = 12;
static const int32 MB_ACT_ERR2 = 14;
static const int32 MB_ACT_ERR3 = 16;
static const int32 MB_ACT_POS1 = 18;
static const int32 MB_ACT_POS2 = 20;
static const int32 MB_ACT_POS3 = 22;
static const int32 MB_ACT_STAT1 = 24;
static const int32 MB_ACT_STAT2 = 26;
static const int32 MB_ACT_STAT3 = 28;
static const int32 MB_ACT_VEL1 = 30;
static const int32 MB_ACT_VEL2 = 32;
static const int32 MB_ACT_VEL3 = 34;
static const int32 MB_DES_ACCEPT1 = 36;
static const int32 MB_DES_ACCEPT2 = 38;
static const int32 MB_DES_ACCEPT3 = 40;
static const int32 MB_DES_CMD1 = 42;
static const int32 MB_DES_CMD2 = 44;
static const int32 MB_DES_CMD3 = 46;
static const int32 MB_DES_DIR1 = 48;
static const int32 MB_DES_DIR2 = 50;
static const int32 MB_DES_DIR3 = 52;
static const int32 MB_DES_GEAR1 = 54;
static const int32 MB_DES_GEAR2 = 56;
static const int32 MB_DES_GEAR3 = 58;
static const int32 MB_DES_POS1 = 60;
static const int32 MB_DES_POS2 = 62;
static const int32 MB_DES_POS3 = 64;
static const int32 MB_DES_VEL1 = 66;
static const int32 MB_DES_VEL2 = 68;
static const int32 MB_DES_VEL3 = 70;
static const int32 MB_DES_MASTER1 = 72;
static const int32 MB_DES_MASTER2 = 74;
static const int32 MB_DES_MASTER3 = 76;
// Staus Def
static const int32 MESS_INVVALID = 0x00000001;
static const int32 MESS_CNO_NOT = 0x00000002;
static const int32 MESS_INV_PARAM = 0x00000004;
static const int32 MESS_SOFT_PB = 0x00000080;
static const int32 MESS_COM_FAIL = 0x00000100;
static const int32 MESS_HOMING_FAIL = 0x00000200;
static const int32 MESS_CNT_DEV_EXCE = 0x00000400;
static const int32 MESS_OVVERTEMP = 0x00001000;
static const int32 MESS_INTERLOCK_ER = 0x00010000;
static const int32 MESS_EMER_BRAKE = 0x00100000;
static const int32 MESS_EMER_STO = 0x00200000;
static const int32 MESS_EXT_PUL_MIS = 0x04000000;
static const int32 MESS_MOTION_ERR = 0x10000000;
static const int32 MESS_EXE_ERR = 0x20000000;
static const int32 MESS_GEN_ERR = 0x40000000;
static const uint16 CMD_ON_RESET = 0;
static const uint16 CMD_ON_CALIBRATION = 1;
static const uint16 CMD_ON_BRAKE = 2;
static const uint16 CMD_ON_STOP = 3;
static const uint16 CMD_ON_MOVE = 4;
static const uint16 CMD_ON_ASYNCH = 5;
static const uint16 CMD_ON_SYNCH = 6;
static const uint16 CMD_ON_IDLE = 7;// Dont needed
static const uint16 CMD_ON_RESUME = 8;
static const uint16 CMD_ON_ESTOP = 9; // Dont needed
static const uint16 PAR_CLOCKWISE = 0;
static const uint16 PAR_COUNTERCLOCKWISE= 1;
static const uint16 SPEED_TOL = 1;
static const float64 PHASE_TOL = 0.01;
static const int32 IND_CH1 = 1;
static const int32 IND_CH2 = 1;
static const uint32 CHOPPER_NO_STATUS_DEVICE_CONTAINER = 0x0000;
static const uint32 CHOPPER_PHASED_DEVICE_CONTAINER = 0x0001;
static const uint32 CHOPPER_4_QUADRANS_DEVICE_CONTAINER = 0x0002;
static const uint32 CHOPPER_PHASE_SLOW_MOVING_DEVICE_CONTAINER = 0x0004;
static const uint32 CHOPPER_PHASE_FAST_MOVING_DEVICE_CONTAINER = 0x0008;
static const uint32 CHOPPER_CHANGE_FREQUENCY_DEVICE_CONTAINER = 0x0010;
static const uint32 CHOPPER_PREDAMPING_DEVICE_CONTAINER = 0x0020;
static const uint32 CHOPPER_STATIC_POSITIONNING_DEVICE_CONTAINER = 0x0040;
static const uint32 CHOPPER_CATCHING_RESCUE_DEVICE_CONTAINER = 0x0080;
static const uint32 CHOPPER_SPEED_OK_DEVICE_CONTAINER = 0x0100;
static const uint32 CHOPPER_GATE_OK_DEVICE_CONTAINER = 0x0200;
static const uint32 CHOPPER_BEARING_ON_DEVICE_CONTAINER = 0x0400;
static const uint32 CHOPPER_DC_ON_DEVICE_CONTAINER = 0x0800;
static const uint32 CHOPPER_DRIVE_ON_DEVICE_CONTAINER = 0x1000;
static const uint32 CHOPPER_PHASING_RUNNING_DEVICE_CONTAINER = 0x2000;
static const uint32 CHOPPER_EMERGENCY_STOP_DEVICE_CONTAINER = 0x4000;
static const uint32 CHOPPER_CALIBRATING_DEVICE_CONTAINER = 0x8000;
static const uint32 CHOPPER_BRAKING_DEVICE_CONTAINER = 0x10000;
static const int32 NO_STATUS = 1;
static const int32 DEFAULT = 2;
static const int32 MANU = 4;
static const int32 IDLE = 8;
static const int32 RUNNING = 16;
}
#endif //CHOPPERNCS_H
/*
* Nomad Instrument Control Software
*
* Copyright 2011 Institut Laue-Langevin
*
* Licensed under the EUPL, Version 1.1 only (the "License");
* You may not use this work except in compliance with the Licence.
* You may obtain a copy of the Licence at:
*
* http://joinup.ec.europa.eu/software/page/eupl
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the Licence is distributed on an "AS IS" basis,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the Licence for the specific language governing permissions and
* limitations under the Licence.
*/
#include <drivers/astrium/chopper_ncs/ChopperNCSDriver.h>
#include <drivers/astrium/chopper_ncs/ChopperNCSState.h>
#include <drivers/astrium/chopper_ncs/PerfectChopperNCSDriver.h>
#include <drivers/astrium/chopper_ncs/RealChopperNCSDriver.h>
#include <math.h>
#include "DeviceContainer/Def/CommandDef.h"
#include "drivers/legacy/def/FunctionDef.h"
#include "drivers/global/DriversCommands.h"
#include "controllers/common/axis/AxisCommon.h"
#include <drivers/astrium/chopper_ncs/ChopperNCSDef.h>
using namespace boost;
using namespace std;
namespace chopper_ncs {
const std::string chopperNCSDriver::TYPE = "chopperNCS";
//const int32 chopperNCSDriver::STOP_MODE = 0;
//const int32 chopperNCSDriver::SQUARE_MODE = 1;
//const int32 chopperNCSDriver::PULSE_MODE = 2;
//const int32 chopperNCSDriver::CONTINU_MODE = 3;
//const int32 chopperNCSDriver::MOVE_MODE = 4;
//const string chopperNCSDriver::RESET_POS_COMMAND = "resetPos";
chopperNCSDriver::chopperNCSDriver(const string& name) :
DeviceDriver(name)
{
driver::ModBusTCP::init(name);
registerStates(new RealchopperNCSDriver(this), new PerfectchopperNCSDriver(this), new PerfectchopperNCSDriver(this));
speed.init(this, SAVE, "actual_speed", "wanted_speed", "speed");
phase.init(this, SAVE, "actual_phase", "wanted_phase", "phase");
gear.init(this, SAVE, "actual_gear", "wanted_gear", "gear");
dire.init(this, SAVE, "actual_dire", "wanted_dire", "Dire");
isSync.init(this, SAVE, "isSync");
isMaster.init(this, SAVE, "isMaster");
indChopperActive.init(this, SAVE, "actual_indChopperActive", "wanted_indChopperActive", "indCAct");
speedStatus.init(this, SAVE, "speedStatus");
nbChopper.init(this, SAVE, "nbChopper");
currentSpeed.init(this, NOSAVE, "currentSpped");
currentPhase.init(this, NOSAVE, "currentPhase");
chopperStatus.init(this, NOSAVE, "chopperStatus");
status.init(this, NOSAVE, "status");
initCommand(driver::INIT_COMMAND);
initCommand(COMMAND_READ_DEVICE_CONTAINER);
initCommand(COMMAND_STATUS_DEVICE_CONTAINER);
initCommand(COMMAND_START_DEVICE_CONTAINER);
initCommand(COMMAND_STOP_DEVICE_CONTAINER);
initCommand(COMMAND_CALIBRATE_DEVICE_CONTAINER);
initCommand(COMMAND_SETDIR_DEVICE_CONTAINER);
initCommand(COMMAND_SETGEAR_DEVICE_CONTAINER);
initCommand(COMMAND_BRAKE_DEVICE_CONTAINER);
registerFunction(NONE_FUNCTION);
nbChopper = 2;
currentSpeed.resize(nbChopper());
currentPhase.resize(nbChopper());
updateValue(DEVICE_TYPE_DEVICE_CONTAINER, string(LEAF_DEVICE_TYPE_DEVICE_CONTAINER));
/*
* Register the Spy and Observer commands necessary to do the updates.
*/
registerSpyCommand(driver::READ_COMMAND, 4);
registerSpyCommand(driver::STATUS_COMMAND, 4);
registerObserverCommand(driver::READ_COMMAND, 50);
registerObserverCommand(driver::STATUS_COMMAND, 50);
}
chopperNCSDriver::~chopperNCSDriver() {
}
void chopperNCSDriver::execute(const string& aCommand) {
if ((aCommand != COMMAND_STATUS_DEVICE_CONTAINER) && (aCommand != COMMAND_READ_DEVICE_CONTAINER)) {
commandProgression = PROGRESSION_UNKNOWNSTATE_DEVICE_CONTAINER;
}
// cout << " chopperNCSDriver::execute aCommand " << aCommand << endl;
chopperNCSState * currentState = static_cast<chopperNCSState *>(getCurrentState());
// Check command
if (aCommand == COMMAND_INIT_DEVICE_CONTAINER) {
currentState->init();
} else if (aCommand == COMMAND_READ_DEVICE_CONTAINER) {
currentState->read();
} else if (aCommand == COMMAND_STATUS_DEVICE_CONTAINER) {
currentState->readStatus();
} else if (aCommand == COMMAND_START_DEVICE_CONTAINER) {
startActivated = true;
currentState->start();
} else if (aCommand == COMMAND_STOP_DEVICE_CONTAINER) {
currentState->stop();
} else if (aCommand == COMMAND_CALIBRATE_DEVICE_CONTAINER) {
currentState->calibrate();
} else if (aCommand == COMMAND_SETDIR_DEVICE_CONTAINER) {
currentState->setDir();
} else if (aCommand == COMMAND_SETGEAR_DEVICE_CONTAINER) {
currentState->setGear();
} else if (aCommand == COMMAND_BRAKE_DEVICE_CONTAINER) {
currentState->brake();
}else {
//Error bad command
}
}
//void chopperNCSDriver::refreshwantedContinuSeqNb(int32 number) throw (CannotSetValue) {
//
//}
void chopperNCSDriver::calculateProgression() {
}
}
/*
* Nomad Instrument Control Software
*
* Copyright 2011 Institut Laue-Langevin
*
* Licensed under the EUPL, Version 1.1 only (the "License");
* You may not use this work except in compliance with the Licence.
* You may obtain a copy of the Licence at:
*
* http://joinup.ec.europa.eu/software/page/eupl
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the Licence is distributed on an "AS IS" basis,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the Licence for the specific language governing permissions and
* limitations under the Licence.
*/
#ifndef CHOPPERNCS_H
#define CHOPPERNCS_H
#include <Driver.h>
#include "controllers/common/axis/SpeedCommon.h"
#include "controllers/common/axis/AxisCommon.h"
//#include "drivers/legacy/other/modbus/Modbus.h"
#include "drivers/global/modbus/ModBusTCP.h"
namespace chopper_ncs{
class chopperNCSDriver : public driver::ModBusTCP {
friend class RealchopperNCSDriver;
friend class PerfectchopperNCSDriver;
public:
//! Driver type value
static const std::string TYPE;
/**
* Constructor.
*
* @param the name of device driver
*/
chopperNCSDriver(const std::string& aName);
/**
* Destructor.
*/
virtual ~chopperNCSDriver();
/**
* Execute the command on the device.
* The DeviceActionDriver class gives the command to the DeviceDriver.
* Each type of device have to interpret its command and call
* the write method.
*
* @param the command to apply on the device
*/
virtual void execute(const std::string& aCommand);
/**
* Calculate the progression state.
*/
void calculateProgression();
/**
* Properties
*/
Property<int32, SETPOINT > speed; //! speed property
Property<float64, SETPOINT > phase; //! phase property
Property<int32, SETPOINT > gear; //! phase property
Property<std::string, SETPOINT> dire; //! dir property
Property<int32> isSync;
Property<int32> isMaster;
Property<int32, SETPOINT > indChopperActive; //! speed property
Property<int32> speedStatus; //! Status for the speed (0,1,2)
Property<int32> nbChopper; //!
DynamicProperty<int32> currentSpeed; // actual spped for chopper
DynamicProperty<float64> currentPhase; // actual phase for chopper
DynamicProperty<int32> chopperStatus; // actual phase for chopper
};
}
#endif
/*
* Nomad Instrument Control Software
*
* Copyright 2011 Institut Laue-Langevin
*
* Licensed under the EUPL, Version 1.1 only (the "License");
* You may not use this work except in compliance with the Licence.
* You may obtain a copy of the Licence at:
*
* http://joinup.ec.europa.eu/software/page/eupl
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the Licence is distributed on an "AS IS" basis,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the Licence for the specific language governing permissions and
* limitations under the Licence.
*/
#ifndef CHOPPERNCSSTATE_H
#define CHOPPERNCSSTATE_H
#include "ChopperNCSDriver.h"
#include <Driver.h>
namespace chopper_ncs {
/**
* The class is the interface class describes in the pattern state implemented the chopperNCS Modbus device.
*/
class chopperNCSState : public DriverState<chopperNCSDriver> {
public:
/**
* Constructor
*
* @param the class which used the state
*/
chopperNCSState(chopperNCSDriver* owner) : DriverState<chopperNCSDriver>(owner) {}
/**
* Destructor
*/
virtual ~chopperNCSState() {}
/**
* Read the value.
*/
virtual void read() = 0;
/**
* Start the execution.
*/
virtual void start() = 0;
/**
* Stop the execution.
*/
virtual void stop() = 0;
/**
* Stop the execution.
*/
virtual void calibrate() = 0;
/**
* Stop the execution.
*/
virtual void setDir() = 0;
/**
* Stop the execution.
*/
virtual void setGear() = 0;
/**
* Stop the execution.
*/
virtual void brake() = 0;
/**
* Read status.
*/
virtual void readStatus() = 0;
};
}
#endif
<module name="chopper_ncs">
<driver class="chopper_ncs::chopperNCSDriver"/>
</module>
\ No newline at end of file
/*
* Nomad Instrument Control Software
*
* Copyright 2011 Institut Laue-Langevin
*
* Licensed under the EUPL, Version 1.1 only (the "License");
* You may not use this work except in compliance with the Licence.
* You may obtain a copy of the Licence at:
*
* http://joinup.ec.europa.eu/software/page/eupl
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the Licence is distributed on an "AS IS" basis,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the Licence for the specific language governing permissions and
* limitations under the Licence.
*/
#include <drivers/astrium/chopper_ncs/PerfectChopperNCSDriver.h>
#include <drivers/astrium/chopper_ncs/ChopperNCSDef.h>
using namespace std;
namespace chopper_ncs {
PerfectchopperNCSDriver::PerfectchopperNCSDriver(chopperNCSDriver* owner) : chopperNCSState(owner) {
}
PerfectchopperNCSDriver::~PerfectchopperNCSDriver() {
}
void PerfectchopperNCSDriver::init() {
readStatus();
owner()->startActivated =false;
//owner()->status =0;
}
void PerfectchopperNCSDriver::read() {
int32 vals1 = readRegInt16(MB_ACT_VEL1);
owner()->currentSpeed.set(0, vals1);
int32 vals2 = readRegInt16(MB_ACT_VEL2);
owner()->currentSpeed.set(1, vals2);
// Test
vals1 = 1000;
vals2 = 999;
owner()->currentSpeed.set(0, vals1);
owner()->currentSpeed.set(1, vals2);
// End Test
if (owner()->indChopperActive.setpoint() == IND_CH1) {
owner()->speed = vals1;
} else if (owner()->indChopperActive.setpoint() == IND_CH2) {
owner()->speed = vals2;
}
int32 valp1 = readRegInt16(MB_ACT_POS1);
owner()->currentPhase.set(0, valp1);
int32 valp2 = readRegInt16(MB_ACT_POS1);
owner()->currentPhase.set(1, valp2);
// Test
valp1 = 2244;
valp2 = 12044;
owner()->currentSpeed.set(0, valp1);
owner()->currentSpeed.set(1, valp2);
// End Test
if (owner()->indChopperActive.setpoint() == IND_CH1) {
owner()->phase = valp1;
} else if (owner()->indChopperActive.setpoint() == IND_CH2) {
owner()->phase = valp2;
}
}
void PerfectchopperNCSDriver::start() {
int32 valMaster = 1 ;
if ( owner()->indChopperActive.setpoint() == IND_CH1){
valMaster = 1;
} else if (owner()->indChopperActive.setpoint() == IND_CH2){
valMaster = 2;
}
writeRegInt(MB_DES_MASTER1 , valMaster);
writeRegInt(MB_DES_MASTER2 , valMaster);
int32 valSpeed;
int32 valPhase;
if ( owner()->indChopperActive.setpoint() == IND_CH1){
valSpeed = owner()->speed.setpoint();
writeRegInt(MB_DES_VEL1 , valSpeed);
valPhase = int ( owner()->phase.setpoint() * 100. );
writeRegInt(MB_ACT_POS1 , valPhase);
writeRegInt(MB_DES_CMD1, CMD_ON_SYNCH);
} else if (owner()->indChopperActive.setpoint() == IND_CH2){
valSpeed = owner()->speed.setpoint();
writeRegInt(MB_DES_VEL2 , valSpeed);
valPhase = int ( owner()->phase.setpoint() * 100. );
writeRegInt(MB_ACT_POS2 , valPhase);
writeRegInt(MB_DES_CMD2, CMD_ON_SYNCH);
}
//owner()->commandProgression = PROGRESSION_END_DEVICE_CONTAINER;
}
void PerfectchopperNCSDriver::stop() {
if (owner()->indChopperActive.setpoint() == IND_CH1) {
writeRegInt(MB_DES_CMD1, CMD_ON_STOP);
} else if (owner()->indChopperActive.setpoint() == IND_CH2) {
writeRegInt(MB_DES_CMD2, CMD_ON_STOP);
}
//Note: there is not a stop
owner()->commandStatus.setIdle();
owner()->commandProgression = 100;
}
void PerfectchopperNCSDriver::readStatus() {
owner()->startActivated = false;
// Read Status Chooper1
int32 local_status = readRegInt16( MB_ACT_STAT1 );
if (owner()->startActivated()) {
if ( fabs( owner()->currentSpeed.get(0) - owner()->speed.setpoint() ) < SPEED_TOL ){
cout <<"Speed is OK" << endl;
if (fabs( owner()->currentPhase.get(0) - owner()->phase.setpoint() ) < PHASE_TOL ){
cout <<"Phase is OK" << endl;
owner()->status = CHOPPER_PHASED_DEVICE_CONTAINER;
owner()->startActivated = false;
owner()->commandProgression = PROGRESSION_END_DEVICE_CONTAINER;
}else {
owner()->status = CHOPPER_PHASING_RUNNING_DEVICE_CONTAINER;
}
}else {
owner()->status = CHOPPER_CHANGE_FREQUENCY_DEVICE_CONTAINER;
cout <<"Speed running " << endl;
}
}else {
if (local_status == CMD_ON_RESET) {
owner()->status = CHOPPER_NO_STATUS_DEVICE_CONTAINER;
} else if (local_status == CMD_ON_CALIBRATION) {
owner()->status = CHOPPER_CALIBRATING_DEVICE_CONTAINER;
} else if (local_status == CMD_ON_BRAKE) {
owner()->status = CHOPPER_BRAKING_DEVICE_CONTAINER;
} else if (local_status == CMD_ON_STOP) {
owner()->status = CHOPPER_EMERGENCY_STOP_DEVICE_CONTAINER;
} else if (local_status == CMD_ON_MOVE) {
owner()->status = CHOPPER_STATIC_POSITIONNING_DEVICE_CONTAINER;
} else if (local_status == CMD_ON_ASYNCH) {
owner()->status = CHOPPER_PHASING_RUNNING_DEVICE_CONTAINER;
} else if (local_status == CMD_ON_SYNCH) {
owner()->status = CHOPPER_PHASING_RUNNING_DEVICE_CONTAINER;
} else if (local_status == CMD_ON_IDLE) {
owner()->status = CHOPPER_NO_STATUS_DEVICE_CONTAINER;
} else if (local_status == CMD_ON_RESUME) {
// owner()->status =
} else if (local_status == CMD_ON_ESTOP) {
owner()->status = CHOPPER_NO_STATUS_DEVICE_CONTAINER;
}
}
owner()->currentSpeed.set(0 , 1000.);
owner()->currentSpeed.set(1 , 999.);
}
/**
* Stop the execution.
*/
void PerfectchopperNCSDriver::calibrate() {
if ( owner()->indChopperActive.setpoint() == IND_CH1){
writeRegInt(MB_DES_CMD1, CMD_ON_CALIBRATION);
} else if (owner()->indChopperActive.setpoint() == IND_CH2){