32 #define max(a,b) (((a)>(b))?(a):(b)) 38 void CKatana::inc(
long idx,
int dif,
bool wait,
int tolerance,
long timeout) {
42 void CKatana::dec(
long idx,
int dif,
bool wait,
int tolerance,
long timeout) {
46 void CKatana::mov(
long idx,
int tar,
bool wait,
int tolerance,
long timeout) {
67 if(!infos.
openFile(configurationFile)) {
97 std::cout <<
"\n\n!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n";
98 std::cout <<
"Exit: Incompatible Config File!\n";
99 std::cout <<
"Check whether you have a Katana 400 or 300 and choose the config file accordingly\n";
100 std::cout <<
"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\n.";
103 bool gripperIsPresent;
104 int gripperOpenEncoders, gripperCloseEncoders;
111 base->
init(gnl, mot, sct, eff, protocol);
117 std::cout <<
"Katana4xx calibration started\n";
149 std::cout <<
"...done with calibration.\n";
153 std::cout <<
"Katana300 calibration started\n";
236 double firstSpeedSample = 100, secondSpeedSample = 100;
244 if( (firstSpeedSample + secondSpeedSample) == 0.0 ) {
247 secondSpeedSample = firstSpeedSample;
322 if (axis < 1)
return;
328 int force = abs(limit);
338 p[3] = (char)(force >> 8);
339 p[4] = (char)(force);
347 char force = (char)r2;
368 std::vector<int>::iterator
372 std::vector<int>::iterator iter = start;
425 inc(number, encoders, waitUntilReached, waitTimeout);
427 dec(number, abs(encoders), waitUntilReached, 100, waitTimeout);
433 double degree = radianAngle/
M_PI*180;
439 mov(number, encoders, waitUntilReached, encTolerance, waitTimeout);
449 mov(number, encoders, waitUntilReached, 100, waitTimeout);
463 CKatana::moveRobotToEnc(std::vector<int>::const_iterator start, std::vector<int>::const_iterator end,
bool waitUntilReached,
int encTolerance,
int waitTimeout) {
472 std::vector<int>::const_iterator iter = start;
477 mov(i, *iter,
false, encTolerance, waitTimeout);
483 if(!waitUntilReached)
510 moveRobotToEnc(encoders.begin(), encoders.end(), waitUntilReached, encTolerance, waitTimeout);
516 int n, maxDistance = 0;
518 std::vector<int> diffMot,speed,oldSpeed;
523 maxDistance=
max(diffMot.at(n),maxDistance);
529 speed.push_back(
max(static_cast<int>((static_cast<double>(diffMot.at(n))/maxDistance) *
velocity),10));
int mKatanaType
The type of KatanaXXX (300 or 400)
short actpos
actual position
_encT rad2enc(_angleT const &angle, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
TMotCmdFlg mcf
motor flag after calibration
unsigned char byte
type specification (8 bit)
void mov(long idx, int tar, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
Moves the motor specified by an index to a given target position in encoders.
void setTolerance(int tolerance)
void sendSplineToMotor(short number, short targetPosition, short duration, short p1, short p2, short p3, short p4)
void moveMotorTo(short number, double radianAngle, bool waitUntilReached=false, int waitTimeout=0)
void decDegrees(long idx, double dif, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
Decrements the motor specified by an index postion in degree units.
[PVP] position, velocity, pulse width modulation
CKatBase * base
base katana
void freezeMotor(short number)
void setControllerParameters(byte kSpeed, byte kPos, byte kI)
Set the controller parameters.
static std::unique_ptr< CCplSerialCRC > protocol
virtual bool init(const TKatGNL _gnl, const TKatMOT _mot, const TKatSCT _sct, const TKatEFF _eff, CCplBase *_protocol)
void setCalibrated(bool calibrated)
void setGripperParameters(bool isPresent, int openEncoders, int closeEncoders)
void enableCrashLimits()
crash limits enable
TMotSCP getMotSCP(short number)
void setCrashLimit(long idx, int limit)
unblock robot after a crash
void setPwmLimits(byte maxppwm, byte maxnpwm)
Set the PWM limits (for the drive controller)
bool openFile(const char *filepath)
Abstract base class for protocol definiton.
const int POLLFREQUENCY
Polling position every POLLFREQUENCY milliseconds.
bool checkENLD(long idx, double degrees)
Check if the absolute position in degrees is out of range.
[SCT] every sens ctrl's attributes
bool enable
enable/disable
void sendTPS(const TMotTPS *_tps)
send data
void inc(int dif, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
Increments the motor specified by an index postion in encoder units.
void setAndStartPolyMovement(std::vector< short > polynomial, int exactflag, int moreflag)
void sendSpline(short targetPosition, short duration, short p1, short p2, short p3, short p4)
Initial motor parameters.
TMotDYL getMotDYL(short number)
void recvPVP()
receive data
void inc(long idx, int dif, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
Increments the motor specified by an index postion in encoders.
void decDegrees(double dif, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
Decrements the motor specified by an index postion in degrees.
void openGripper(bool waitUntilReached=false, int waitTimeout=100)
void startSplineMovement(int exactflag, int moreflag=1)
void waitFor(TMotStsFlg status, int waitTimeout)
CMotBase * arr
array of motors
void switchMotorOff(short number)
void setAccelerationLimit(short acceleration)
Set the acceleration limits.
void setRobotVelocityLimit(short velocity)
byte maxppwm_nmp
Max. value for positive voltage (0 => 0%, +70 => 100%)
void dec(long idx, int dif, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
Decrements the motor specified by an index postion in encoders.
TMotCLB getMotCLB(short number)
int flushMoveBuffers()
flush move buffers on all motors:
void disableCrashLimits()
crash limits disable
void setCalibrationParameters(bool doCalibration, short order, TSearchDir direction, TMotCmdFlg motorFlagAfter, int encoderPositionAfter)
CCplBase * getProtocol()
get a handle of the protocol, used in CKatana
void getParameterOrLimit(int subcommand, byte *R1, byte *R2, byte *R3)
int crash_limit_lin_nmp
Limit of error in position in linear movement.
void moveRobotToEnc4D(std::vector< int > target, int velocity=180, int acceleration=1, int encTolerance=100)
Move to robot to given target in the vector-container with the given velocity, acceleration and toler...
void setForceLimit(int axis, int limit)
Set the force limit for the current controller.
int readDigitalIO()
get digital I/O data from Katana400:
void incDegrees(long idx, double dif, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
Increments the motor specified by an index postion in degree units.
void moveMotorByEnc(short number, int encoders, bool waitUntilReached=false, int waitTimeout=0)
void WaitUntilElapsed() const
int getMotorEncoders(short number, bool refreshEncoders=true) const
void moveMotorBy(short number, double radianAngle, bool waitUntilReached=false, int waitTimeout=0)
void waitFor(TMotStsFlg status, int waitTimeout, bool gripper)
wait for motor on all motors
const TKatMOT * GetMOT()
Get a pointer to the desired structure.
void startSplineMovement(bool exactflag, int moreflag=1)
void setTolerance(long idx, int enc_tolerance)
Sets the tolerance range in encoder units for the robots movements.
Calibration structure for single motors.
void searchMechStop(long idx, TSearchDir dir, TMotSCP scp, TMotDYL dyl)
virtual void comm(const byte *pack, byte *buf, byte *size)=0
Base communication function.
void recvMPS()
read all motor positions simultaneously
Inverse Kinematics structure of the endeffektor.
[MOT] every motor's attributes
void movDegrees(long idx, double tar, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
Moves the motor specified by an index to a given target position in degree units. ...
int getCurrentControllerType(int axis)
Get the axis controller type.
void setSCP(TMotSCP _scp)
int mKinematics
The kinematics implementation: 0 for Analytical, 1 for RobAnaGuess.
void setPositionCollisionLimit(int limit)
Set the collision limit.
search direction for the meachanical stopper
short getForce(int axis)
Get the current force.
void mov(int tar, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
Moves the motor specified by an index to a given target position in encoder units.
byte kI_nmp
Integral factor (1/kI) of control output added to the final control output.
TMotInit getMotInit(short number)
void incDegrees(double dif, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
Increments the motor specified by an index postion in degrees.
void sendAPS(const TMotAPS *_aps)
send data
short getMotorVelocityLimit(short number) const
void recvGMS()
receive data
void setCrashLimitLinear(int limit_lin)
Set the crash limit linear.
bool checkAngleInRange(double angle)
check limits in encoder values
short tarpos
target position
void setCrashLimit(int limit)
Set the crash limit.
[SCP] static controller parameters
std::vector< int >::iterator getRobotEncoders(std::vector< int >::iterator start, std::vector< int >::const_iterator end, bool refreshEncoders=true) const
void unBlock()
unblock robot after a crash
void closeGripper(bool waitUntilReached=false, int waitTimeout=100)
void setSpeedLimit(short velocity)
void waitForMotor(short number, int encoders, int encTolerance=100, short mode=0, int waitTimeout=5000)
void getGripperParameters(bool &isPresent, int &openEncoders, int &closeEncoders)
void switchMotorOn(short number)
int crash_limit_nmp
Limit of error in position.
const TMotInit * GetInitialParameters()
short order
order in which this motor will be calibrated. range: 0..5
void setInitialParameters(double angleOffset, double angleRange, int encodersPerCycle, int encoderOffset, int rotationDirection)
void setMotorAccelerationLimit(short number, short acceleration)
void create(const char *configurationFile, CCplBase *protocol)
Create routine.
[GNL] general robot attributes
void setSpeedCollisionLimit(long idx, int limit)
set collision speed limits
TSearchDir dir
search direction for mech. stopper
void setMotorVelocityLimit(short number, short velocity)
void dec(int dif, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
Decrements the motor specified by an index postion in encoder units.
TMotStsFlg msf
motor status flag
void setCrashLimit(long idx, int limit)
set collision limits
int checkKatanaType(int type)
checks for a K300 or K400
short getNumberOfMotors() const
TMotCmdFlg mcfAPS
motor command flag
void flushMoveBuffers()
flush move buffers
byte maxaccel_nmp
Maximal acceleration and deceleration.
int _gripperCloseEncoders
void setPositionCollisionLimit(long idx, int limit)
set collision position limits
byte maxnpwm_nmp
Max. value for negative voltage (0 => 0%, +70 => 100%)
void waitForMotor(int tar, int encTolerance=100, short mode=0, int waitTimeout=TM_ENDLESS)
Waits until the Motor has reached the given targen position.
void unBlock()
unblock robot after a crash
void setDYL(TMotDYL _dyl)
void recvSFW()
receive data
TMotCmdFlg mcfTPS
motor command flag
void moveRobotToEnc(std::vector< int >::const_iterator start, std::vector< int >::const_iterator end, bool waitUntilReached=false, int encTolerance=100, int waitTimeout=0)
short maxpspeed_nmp
Max. allowed forward speed.
void setRobotAccelerationLimit(short acceleration)
void setSpeedCollisionLimit(int limit)
Set the collision limit.
void enableCrashLimits()
crash limits enable
byte kpos_nmp
Proportional factor of position compensator.
short maxnspeed_nmp
Max. allowed reverse speed.
std::vector< int > encoders
void disableCrashLimits()
crash limits disable
void setSpeedLimits(short positiveVelocity, short negativeVelocity)
Set speed limits.
void getGripperParameters(bool &isPresent, int &openEncoders, int &closeEncoders)
Get the gripper parameters.
byte kspeed_nmp
Proportional factor of speed compensator.
int readDigitalIO()
Read The Digital I/Os.
void moveMotorToEnc(short number, int encoders, bool waitUntilReached=false, int encTolerance=100, int waitTimeout=0)
short getMotorAccelerationLimit(short number) const
void setAndStartPolyMovement(std::vector< short > polynomial, bool exactflag, int moreflag)
void movDegrees(double tar, bool wait=false, int tolerance=100, long timeout=TM_ENDLESS)
Moves the motor specified by an index to a given target position in degrees.