void ContainerGrabber::retract() {
setControlMode(leftArm, "openLoop");
setControlMode(rightArm, "openLoop");
setPIDTarget(0);
leftArm->state = RETRACT;
rightArm->state = RETRACT;
}
void ContainerGrabber::cancelGrabSequence() {
setControlMode(leftArm, "openLoop");
setControlMode(rightArm, "openLoop");
setPIDTarget(0);
initHomeState(leftArm);
initHomeState(rightArm);
}
void ContainerGrabber::initGrabSequence() {
setControlMode(leftArm, "position");
setControlMode(rightArm, "position");
setPIDslot(leftArm, 0);
setPIDslot(rightArm, 0);
setPositionTarget(leftArm, dropTargetAngle);
setPositionTarget(rightArm, dropTargetAngle);
}
void pVehicle::updateControl(float steering, bool analogSteering, float acceleration, bool analogAcceleration, bool handBrake)
{
setControlState(E_VCS_ACCELERATION,acceleration);
setControlState(E_VCS_HANDBRAKE,handBrake);
setControlState(E_VCS_STEERING,steering);
setControlMode(E_VCS_ACCELERATION,analogAcceleration ? E_VCSM_ANALOG : E_VCSM_DIGITAL);
setControlMode(E_VCS_STEERING, analogSteering ? E_VCSM_ANALOG : E_VCSM_DIGITAL);
}
void surfacemesh_mode_arapdeform::update()
{
setControlMode();
points = mesh()->vertex_property<Vector3>(VPOINT);
fnormals = mesh()->face_property<Vector3>(FNORMAL);
}
Controller_MidLevel_SpeedLoop::Controller_MidLevel_SpeedLoop() : started(false) {
// Just in case, I set to zero all the internal variables of the controller object
vxci = 0.0; vyci = 0.0; yawci = 0.0; zci = 0.0;
vxs = 0.0; vys = 0.0; yaws = 0.0; zs = 0.0;
pitchd = 0.0; rolld = 0.0; eps_yaw = 0.0; eps_z = 0.0; vxd_int = 0.0; vyd_int = 0.0;
pitchc_int = 0.0; rollc_int = 0.0; dyawc_int = 0.0; dzc_int = 0.0;
pitchco = 0.0; rollco = 0.0; dyawco = 0.0; dzco = 0.0;
// Nacho's PID algorithm >>>> u[k] = Kp*e[k] + Ki*i[k] + Kd*de[k]
// Vx PID gains: Kp, Ki = Kp/Ti, Kd = Kp*Td
pid_vx.setGains( MULTIROTOR_SPEEDCONTROLLER_VX_KP,
MULTIROTOR_SPEEDCONTROLLER_VX_KI,
MULTIROTOR_SPEEDCONTROLLER_VX_KD); // set Kp, Ki = Kp/Ti, Kd = Kp*Td
#ifdef CONTROLLER_DEACTIVATE_NLF
// Saturation specified by config file
pid_vx.enableAntiWindup(true, MULTIROTOR_SPEEDCONTROLLER_MAX_PITCH);
pid_vx.enableMaxOutput( true, MULTIROTOR_SPEEDCONTROLLER_MAX_PITCH); // set enable, value. Maximum output is vx_max = 2.5-2.75 m/s
#else
// Saturation performed by non-linear function
// (Parrot 1) Maximum output is vx_max = 2.5-2.75 m/s
pid_vx.enableAntiWindup(true, MULTIROTOR_SPEEDCONTROLLER_VMAX);
pid_vx.enableMaxOutput( true, MULTIROTOR_SPEEDCONTROLLER_VMAX);
#endif
// Vy PID gains: Kp, Ki = Kp/Ti, Kd = Kp*Td
pid_vy.setGains( MULTIROTOR_SPEEDCONTROLLER_VY_KP,
MULTIROTOR_SPEEDCONTROLLER_VY_KI,
MULTIROTOR_SPEEDCONTROLLER_VY_KD);
#ifdef CONTROLLER_DEACTIVATE_NLF
// Saturation specified by config file
pid_vy.enableAntiWindup(true, MULTIROTOR_SPEEDCONTROLLER_MAX_ROLL);
pid_vy.enableMaxOutput( true, MULTIROTOR_SPEEDCONTROLLER_MAX_ROLL);
#else
// Saturation performed by non-linear function
// (Parrot 1) Maximum output is vy_max = 2.0-2.30 m/s
pid_vy.enableAntiWindup(true, MULTIROTOR_SPEEDCONTROLLER_VMAX);
pid_vy.enableMaxOutput( true, MULTIROTOR_SPEEDCONTROLLER_VMAX);
#endif
// yaw PID gains: Kp, Ki = Kp/Ti, Kd = Kp*Td
pid_yaw.setGains( MULTIROTOR_SPEEDCONTROLLER_YAW_KP,
MULTIROTOR_SPEEDCONTROLLER_YAW_KI,
MULTIROTOR_SPEEDCONTROLLER_YAW_KD);
pid_yaw.enableMaxOutput(true, MULTIROTOR_SPEEDCONTROLLER_DYAWMAX);
pid_yaw.enableMaxOutput( true, MULTIROTOR_SPEEDCONTROLLER_DYAWMAX);
// z PID gains: Kp, Ki = Kp/Ti, Kd = Kp*Td
pid_z.setGains( MULTIROTOR_SPEEDCONTROLLER_Z_KP,
MULTIROTOR_SPEEDCONTROLLER_Z_KI,
MULTIROTOR_SPEEDCONTROLLER_Z_KD);
pid_z.enableMaxOutput( true, MULTIROTOR_SPEEDCONTROLLER_DZMAX);
pid_z.enableMaxOutput( true, MULTIROTOR_SPEEDCONTROLLER_DZMAX);
// Important: Default control mode is set in Controller_Multirotor::Controller_Multirotor() code.
control_mode = Controller_MidLevel_controlMode::POSITION_CONTROL;
setControlMode(control_mode);
}
void Shooter::setPID( float p , float i , float d ) {
m_P = p;
m_I = i;
m_D = d;
// Updates PID constants for PIDController object internally
setControlMode( getControlMode() );
}
void Private::Motor::setPwm(port_t port, const unsigned char &speed)
{
port = fixPort(port);
setControlMode(port, Private::Motor::Inactive);
Private::Kovan *kovan = Private::Kovan::instance();
const unsigned int adjustedSpeed = speed > 100 ? 100 : speed;
kovan->enqueueCommand(createWriteCommand(motorRegisters[port], adjustedSpeed * 2600 / 100));
}
bool Robot::setRobotController(RobotControllerItem * controllerItem)
{
if(!currentControllerItem->locked){
controllerItem->radioButton->setChecked(true);
setControlMode(controllerItem->mode);
currentControllerItem = controllerItem;
return true;
} else
return false;
}
void Parameter::loadJSONDataInternal(var data)
{
Controllable::loadJSONDataInternal(data);
if (!saveValueOnly) setRange(data.getProperty("minValue", minimumValue), data.getProperty("maxValue", maximumValue));
if (data.getDynamicObject()->hasProperty("value")) setValue(data.getProperty("value", 0));
if (data.getDynamicObject()->hasProperty("controlMode")) setControlMode((ControlMode)(int)data.getProperty("controlMode", MANUAL));
if (data.getDynamicObject()->hasProperty("expression")) setControlExpression(data.getProperty("expression", ""));
if (data.getDynamicObject()->hasProperty("editable")) isEditable = data.getProperty("editable", true);
}
bool AMCSubNode::start() {
setControlMode(mode_);
enterStateNoCheck(ShutDown);
if (position_valid_ && (mode_ == CyclicSynchronousPosition)) {
int size;
int32_t pos;
size = sizeof(pos);
ecx_SDOread(contextt_ptr_, slave_id_, SUBNODE_ADDR(0x6064), 0x00, FALSE, &size, &pos, EC_TIMEOUTRXM);
setPosition(pos);
}
return true;
}
void ContainerGrabber::startGrabSequence(float speed, bool teleop) {
if (!teleop) {
if (speed < 1.0) {
limitSpeed = speed;
goinSlow = true;
setControlMode(leftArm, "openLoop");
setControlMode(rightArm, "openLoop");
driveAngle = slowDriveAngle;
initSlowState(leftArm);
initSlowState(rightArm);
} else {
leftArm->state = DROP;
rightArm->state = DROP;
driveAngle = fastDriveAngle;
}
} else {
grabberPID->setTarget(speed);
limitSpeed = 0.2;
leftArm->state = TELEOP;
rightArm->state = TELEOP;
}
dropTimer->Reset();
}
void Controller_MidLevel_SpeedLoop::reset() {
// Just in case, I set to zero all the variables of the controller object
// setYawAndZ2ActualValues(); // yawci = 0.0; zci = 0.0;
vxci = 0.0; vyci = 0.0; vxs = 0.0; vys = 0.0; yaws = 0.0; zs = 0.0;
pitchd = 0.0; rolld = 0.0; eps_yaw = 0.0; eps_z = 0.0; vxd_int = 0.0; vyd_int = 0.0;
pitchc_int = 0.0; rollc_int = 0.0; dyawc_int = 0.0; dzc_int = 0.0;
pitchco = 0.0; rollco = 0.0; dyawco = 0.0; dzco = 0.0;
started = false;
pid_vx.reset();
pid_vy.reset();
pid_z.reset();
pid_yaw.reset();
setControlMode(init_control_mode);
}
void Private::Motor::setPwmDirection(port_t port, const Motor::Direction &dir)
{
// FIXME: This assumes that our current state is the latest.
// If somebody has altered the motor drive codes in the mean time,
// this will undo their work.
Private::Kovan *kovan = Private::Kovan::instance();
setControlMode(port, Private::Motor::Inactive);
port = fixPort(port);
const unsigned short offset = (3 - port) << 1;
unsigned short &dcs = kovan->currentState().t[MOTOR_DRIVE_CODE_T];
// Clear old drive code
dcs &= ~(0x3 << offset);
// Add new drive code
dcs |= dir << offset;
#ifdef LIBKOVAN_DEBUG
std::cout << "PWM Directions: " << std::hex << dcs << std::endl;
#endif
kovan->enqueueCommand(createWriteCommand(MOTOR_DRIVE_CODE_T, dcs));
}
void Robot::init(myData * _data, QVBoxLayout * boxLayout)
{
initFlag = true;
addBaseGui(boxLayout);
data = _data;
industrialRobot = new ofxIndustrialRobot(data->baseApp);
nameid = "Robot";
QTabWidget *robotTab = new QTabWidget();
QWidget * robotGeneralTab = new QWidget();
QGridLayout *robotGeneralTabLayout = new QGridLayout();
//
//Motor control
//
QGroupBox * robotMotorArea = new QGroupBox("Motors");
QGridLayout *robotMotorLayout = new QGridLayout;
// if(industrialRobot->thread.lock()){
for(int i=0;i<5;i++){
motorSlider[i] = new QSlider(Qt::Horizontal);
// motorSlider[i]->setValue(industrialRobot->thread.coreData.arms[i].rotation);
// motorSlider[i]->setFixedWidth(220);
motorLabel[i] = new QLabel("");
motorLabel[i]->setFixedWidth(55);
motorLabel2[i] = new QLabel("");
motorLabel2[i]->setFixedWidth(55);
setValue[i] = new QPushButton("Set");
robotMotorLayout->addWidget( new QLabel(("Motor "+ofToString(i, 0)).c_str()),i,0);
robotMotorLayout->addWidget(motorSlider[i],i,1);
robotMotorLayout->addWidget(motorLabel[i],i,2);
robotMotorLayout->addWidget(motorLabel2[i],i,3);
robotMotorLayout->addWidget(setValue[i],i,4);
}
loadXmlButton = new QPushButton("Load xml");
robotMotorLayout->addWidget(loadXmlButton,6,0);
variantSlider = new QSlider(Qt::Horizontal);
variantSlider->setEnabled(false);
variantSlider->setMinimum(0);
variantSlider->setMaximum(1000.0);
robotMotorLayout->addWidget( new QLabel("Variant"),5,0);
robotMotorLayout->addWidget(variantSlider,5,1);
// }
robotMotorLayout->setAlignment(Qt::AlignTop);
robotMotorArea->setLayout(robotMotorLayout);
robotMotorArea->setMinimumWidth(460);
robotGeneralTabLayout->addWidget(robotMotorArea,0,0);
//
//Mode/input selector
//
QWidget * robotModePositionArea = new QWidget();
QGridLayout *robotModePositionLayout = new QGridLayout;
QGroupBox * robotModeArea = new QGroupBox("Mode");
QGridLayout *robotModeLayout = new QGridLayout;
inputRadio[0] = new QRadioButton("Timeline", robotModeArea);
inputRadio[1] = new QRadioButton("Manual Motor", robotModeArea);
inputRadio[1]->setChecked(true);
inputRadio[2] = new QRadioButton("Gravity", robotModeArea);
inputRadio[3] = new QRadioButton("Manual position", robotModeArea);
// inputRadio[4] = new QRadioButton("Change Variant", robotModeArea);
setControlMode(CONTROL_MODE_MOTOR);
robotModeLayout->addWidget(inputRadio[0],0,0);
robotModeLayout->addWidget(inputRadio[1],1,0);
robotModeLayout->addWidget(inputRadio[2],2,0);
robotModeLayout->addWidget(inputRadio[3],3,0);
// robotModeLayout->addWidget(inputRadio[4],4,0);
// robotModeArea->setMinimumWidth(460);
robotModeArea->setLayout(robotModeLayout);
robotModePositionLayout->addWidget(robotModeArea,0,0);
QGroupBox * robotPositionArea = new QGroupBox("Position");
QGridLayout *robotPositionLayout = new QGridLayout;
robotPositionArea->setLayout(robotPositionLayout);
robotModePositionLayout->addWidget(robotPositionArea,0,1);
int n = 0, l =0;
for(int i=0;i<6;i++){
manualPosition[i] = new QDoubleSpinBox();
robotPositionLayout->addWidget(manualPosition[i],l,1+n);
if(i>2){
manualPosition[i]->setSingleStep(0.1);
}
n++;
if(n>2){
n = 0;
l++;
}
}
manualPosition[0]->setMinimum(-300.0);
manualPosition[0]->setMaximum(300.0);
manualPosition[1]->setMinimum(00.0);
manualPosition[1]->setMaximum(300.0);
manualPosition[2]->setMinimum(-300.0);
manualPosition[2]->setMaximum(300.0);
for(int i=3;i<6;i++){
manualPosition[i]->setMinimum(-1.0);
manualPosition[i]->setMaximum(1.0);
}
robotPositionLayout->addWidget(new QLabel("Pos"),0,0);
robotPositionLayout->addWidget(new QLabel("Dir"),1,0);
robotModePositionArea->setLayout(robotModePositionLayout);
robotGeneralTabLayout->addWidget(robotModePositionArea,1,0);
//
//Serial control
//
QWidget * robotGeneralTabBottomWidget = new QWidget();
QGridLayout *robotGeneralTabBottomLayout = new QGridLayout;
QWidget * robotSerialControlArea = new QWidget();
QGridLayout *robotSerialControlLayout = new QGridLayout;
for(int i=0;i<5;i++){
motorStatusLabel[i] = new QLabel("");
robotSerialControlLayout->addWidget(motorStatusLabel[i],i,0);
}
panicStatus = new QLabel("Status: OK");
resetMotors = new QPushButton("Reset all motors");
unlockMotors = new QPushButton("Un&lock");
unlockMotors->setCheckable(true);
robotSerialControlLayout->addWidget(panicStatus,6,0);
robotSerialControlLayout->addWidget(resetMotors,7,0);
robotSerialControlLayout->addWidget(unlockMotors,8,0);
robotSerialControlArea->setLayout(robotSerialControlLayout);
robotGeneralTabBottomLayout->addWidget(robotSerialControlArea,0,0);
robotGeneralTabBottomWidget->setLayout(robotGeneralTabBottomLayout);
robotGeneralTabLayout->addWidget(robotGeneralTabBottomWidget,2,0);
QWidget * robotControllerWidget = new QWidget();
robotControllerLayout = new QVBoxLayout();
robotControllerLayout->setAlignment(Qt::AlignTop);
robotControllerWidget->setLayout(robotControllerLayout);
robotGeneralTabBottomLayout->addWidget(robotControllerWidget,0,1);
controllerItemLocked = new QCheckBox("Locked");
controllerItemLocked->setEnabled(false);
robotControllerLayout->addWidget(controllerItemLocked);
QWidget * robotSerialTab = new QWidget();
QGridLayout *robotSerialTabLayout = new QGridLayout();
for(int i=0;i<8;i++){
byteone[i] = new QCheckBox(("Byte 1-"+ofToString(i, 0)).c_str());
bytetwo[i] = new QCheckBox(("Byte 2-"+ofToString(i, 0)).c_str());
bytestatus[i] = new QCheckBox(("Byte status-"+ofToString(i, 0)).c_str());
robotSerialTabLayout->addWidget(byteone[i],i,0);
robotSerialTabLayout->addWidget(bytetwo[i],i,1);
robotSerialTabLayout->addWidget(bytestatus[i],i,2);
}
robotGeneralTabLayout->setAlignment(Qt::AlignTop);
robotGeneralTab->setMinimumWidth(500);
robotGeneralTab->setLayout(robotGeneralTabLayout);
robotSerialTabLayout->setAlignment(Qt::AlignTop);
robotSerialTab->setMinimumWidth(500);
robotSerialTab->setLayout(robotSerialTabLayout);
robotTab->addTab(robotGeneralTab,"General");
robotTab->addTab(robotSerialTab,"Serial");
//robotTab->addTab(robotControllerTab,"Robot controller");
robotGeneralTabLayout->setAlignment(Qt::AlignTop);
// robotInputTab->setLayout(robotInputTabLayout);
// robotTab->addTab(robotInputTab,"Inputs");
boxLayout->addWidget(robotTab);
boxLayout->setAlignment(Qt::AlignTop);
// lastReverseHead = true;
resetting = 0;
manualControllerItem =addRobotController("Manual", CONTROL_MODE_MOTOR);
currentControllerItem = manualControllerItem;
setRobotController(manualControllerItem);
warningShown = false;
}
void Private::Motor::stop(port_t port)
{
setControlMode(port, Private::Motor::Inactive);
setPwmDirection(port, PassiveStop);
}
void ContainerGrabber::initSettleState(Arm* arm) {
arm->state = SETTLE;
arm->contact = true;
setControlMode(arm, "openLoop");
setMotorsOpenLoop(arm, settleSpeed);
}
void ContainerGrabber::initSlowState(Arm* arm) {
arm->state = SLOW;
setControlMode(arm, "openLoop");
setMotorsOpenLoop(arm, 1.0);
}
/*
* updateDeviceState - Function that update the device state based on parameters passed from
* the user interface
*
* inputs - params_current - the current set of parameters
* params_update - the new set of parameters
* device0 - pointer to device informaiton
*
*/
int updateDeviceState(struct param_pass *currParams, struct param_pass *rcvdParams, struct device *device0)
{
for (int i = 0; i < NUM_MECH; i++)
{
currParams->xd[i].x = rcvdParams->xd[i].x;
currParams->xd[i].y = rcvdParams->xd[i].y;
currParams->xd[i].z = rcvdParams->xd[i].z;
currParams->rd[i].yaw = rcvdParams->rd[i].yaw;
currParams->rd[i].pitch = rcvdParams->rd[i].pitch * WRIST_SCALE_FACTOR;
currParams->rd[i].roll = rcvdParams->rd[i].roll;
currParams->rd[i].grasp = rcvdParams->rd[i].grasp;
for (int j=0; j < MAX_DOF_PER_MECH; j++) {
currParams->jpos_d[i*MAX_DOF_PER_MECH + j] = rcvdParams->jpos_d[i*MAX_DOF_PER_MECH + j];
}
for (int j=0; j < MAX_DOF_PER_MECH; j++) {
currParams->jvel_d[i*MAX_DOF_PER_MECH + j] = rcvdParams->jvel_d[i*MAX_DOF_PER_MECH + j];
}
for (int j=0; j < MAX_DOF_PER_MECH; j++) {
currParams->torque_vals[i*MAX_DOF_PER_MECH + j] = rcvdParams->torque_vals[i*MAX_DOF_PER_MECH + j];
}
}
/*
for (int i = 0; i < NUM_MECH; i++) {
for (int j=0; j < MAX_DOF_PER_MECH; j++) {
//device0->mech[i].joint[j].jvel_d = 0;
device0->mech[i].joint[j].jpos_d = rcvdParams->jpos_d[i*MAX_DOF_PER_MECH + j];
}
}
*/
// set desired mech position in pedal_down runlevel
RunLevel rl = RunLevel::get();
if (rl.isPedalDown()) {
for (int i = 0; i < NUM_MECH; i++)
{
device0->mech[i].pos_d.x = rcvdParams->xd[i].x;
device0->mech[i].pos_d.y = rcvdParams->xd[i].y;
device0->mech[i].pos_d.z = rcvdParams->xd[i].z;
device0->mech[i].ori_d.grasp = rcvdParams->rd[i].grasp;
for (int j=0;j<3;j++)
for (int k=0;k<3;k++)
device0->mech[i].ori_d.R[j][k] = rcvdParams->rd[i].R[j][k];
}
/*
for (int i = 0; i < NUM_MECH; i++) {
for (int j=0; j < MAX_DOF_PER_MECH; j++) {
int armId = armIdFromMechType(device0->mech[i].type);
float jvel_d = rcvdParams->jvel_d[getCombinedJointIndex(armId,j)];
device0->mech[i].joint[j].jvel_d = jvel_d;
if (jvel_d) {
//std::cout << "joint " << getJointIndexName(j) << " on " << getArmNameFromId(armId) << " is " << jvel_d << std::endl;
}
}
}
*/
}
// Switch control modes only in pedal up or init.
if (99 == (int)newRobotControlMode) {
t_controlmode currRobotControlMode = getControlMode();
//log_msg("Current control mode: %d",(int)currRobotControlMode);
newRobotControlMode = currRobotControlMode;
} else if (!rl.isPedalDown()) {
bool changed = setControlMode(newRobotControlMode);
if (changed) {
log_msg("Control mode updated: %s",controlModeToString(newRobotControlMode).c_str());
}
}
newRobotControlMode = (t_controlmode) 99;
// Set new torque command from console user input
if ( newDofTorqueSetting )
{
// reset all other joints to zero
for (unsigned int idx=0; idx<MAX_MECH_PER_DEV*MAX_DOF_PER_MECH; idx++)
{
if ( idx == MAX_DOF_PER_MECH*newDofTorqueMech + newDofTorqueDof )
currParams->torque_vals[idx] = newDofTorqueTorque;
else
currParams->torque_vals[idx] = 0;
}
newDofTorqueSetting = 0;
log_msg("DOF Torque updated\n");
}
// Set new surgeon mode
if ( device0->surgeon_mode != rcvdParams->surgeon_mode)
{
device0->surgeon_mode=rcvdParams->surgeon_mode; //store the surgeon_mode to DS0
}
return 0;
}
void Robot::update(){
if(industrialRobot->getEmergencyStop()){
unlockMotors->setChecked(false);
}
tjeckCurrentRobotController();
//Check for changes in control mode (tilføjet af mads)
for(int i = 0; i < 4;i++)
{
if(inputRadio[i]->isChecked() && controlMode != i)
{
setControlMode(i);
}
}
if(industrialRobot->thread.lock()){
if(loadXmlButton->isDown()){
industrialRobot->thread.converter->loadXml();
loadXmlButton->setDown(false);
}
industrialRobot->thread.unlock();
}/*
if(pause->isChecked()){
industrialRobot->thread.timeline->pause();
} else {
industrialRobot->thread.timeline->play();
}
*/
unsigned char b1=0;
for(int i=0;i<8;i++){
if(byteone[i]->isChecked()){
b1 |= 0x01<<i;
}
}
unsigned char b2=0;
for(int i=0;i<8;i++){
if(bytetwo[i]->isChecked()){
b2 |= 0x01<<i;
}
}
if(industrialRobot->thread.serial->lock()){
industrialRobot->thread.serial->outbuf[24-3] = b1;
// cout<<(unsigned char )b1<<" "<<(unsigned char )b2<<endl;
industrialRobot->thread.serial->outbuf[24-2] = b2;
for(int i=0;i<8;i++){
if(industrialRobot->thread.serial->returnedFlags[i])
bytestatus[i]->setChecked(true);
else
bytestatus[i]->setChecked(false);
}
if(resetting > 0){
switch (resetting) {
case 1:
unlockMotors->setChecked(false);
resetting++;
byteone[5]->setChecked(true);
ofSleepMillis(20);
break;
case 2:
/* industrialRobot->thread.controller->gotoResetPosition();
for(int i=0;i<5;i++){
motorSlider[i]->setValue(industrialRobot->thread.coreData.arms[i].rotation*100.0);
}*/
byteone[5]->setChecked(false);
resetting++;
break;
case 3:
if(bytetwo[1]->isChecked()){
if(bytestatus[1]->isChecked()){
resetting ++;
}
} else {
bytetwo[1]->setChecked(true);
// ofSleepMillis(10);
unlockMotors->setChecked(true);
}
break;
case 4:
resetting++;
break;
case 5:
if(bytetwo[3]->isChecked()){
if(bytestatus[3]->isChecked() && bytestatus[4]->isChecked()){
resetting ++;
}
} else {
bytetwo[3]->setChecked(true);
bytetwo[4]->setChecked(true);
// ofSleepMillis(10);
}
break;
case 6:
if(bytetwo[2]->isChecked()){
if(bytestatus[2]->isChecked()){
resetting ++;
}
} else {
bytetwo[2]->setChecked(true);
// ofSleepMillis(10);
}
break;
case 7:
if(bytetwo[0]->isChecked()){
if(bytestatus[0]->isChecked()){
resetting ++;
}
} else {
bytetwo[0]->setChecked(true);
// ofSleepMillis(10);
}
break;
case 8:
for(int i=0;i<5;i++){
bytetwo[i]->setChecked(false);
}
industrialRobot->thread.controller->gotoResetPosition();
for(int i=0;i<5;i++){
motorSlider[i]->setValue(industrialRobot->thread.coreData.arms[i].rotation*100.0);
}
resetting = 0;
break;
default:
break;
}
}
/*bool removeReset = true;
for(int i =0;i<5;i++){
if(!bytestatus[i]->isChecked() || !bytetwo[i]->isChecked()){
removeReset = false;
}
}
if(!removeReset){
if(resetting == 1){
resetting = 2;
}
}
if(removeReset){
if(resetting == 2){
for(int i =0;i<5;i++){
// bytetwo[i]->setChecked(false);
}
}
}
*/
// if(!allOk){
// industrialRobot->thread.timeline->pause();
// } else {
// industrialRobot->thread.timeline->play();
//
// }
if(industrialRobot->thread.serial->connected){
resetMotors->setEnabled(true);
unlockMotors->setEnabled(true);
} else {
resetMotors->setEnabled(false);
unlockMotors->setEnabled(false);
}
for(int i=0;i<5;i++){
string l;
if(industrialRobot->thread.serial->connected){
l = "Motor "+ofToString(i,0)+": ";
if(!byteone[7]->isChecked()){
l+= "LOCKED ";
}
if(bytetwo[i]->isChecked()){
if(bytestatus[i]->isChecked()){
l+= "Resetting done...";
} else {
l+= "Resetting...";
}
} else if(bytestatus[i]->isChecked()){
l+= "OK";
} else {
l+= "NOT OK";
}
} else {
l = " ";
}
motorStatusLabel[i]->setText((l).c_str());
}
if(!industrialRobot->thread.serial->connected){
panicStatus->setText("Status: Serial not connected");
}else if(bytestatus[7]->isChecked()){
panicStatus->setText("Status: PANIC!!!");
/*industrialRobot->thread.serial->unlock();
if(!warningShown){
QMessageBox msgBox;
msgBox.setText("Panic!");
msgBox.exec();
warningShown = true;
}
industrialRobot->thread.serial->lock();*/
} else if(bytestatus[6]->isChecked()){
panicStatus->setText("Status: EMERGENCY STOP");
/*industrialRobot->thread.serial->unlock();
if(!warningShown){
QMessageBox msgBox;
msgBox.setText("Emergency stop pressed. Release it, and unlock the robot.");
msgBox.exec();
warningShown = true;
}
industrialRobot->thread.serial->lock();*/
} else {
panicStatus->setText("Status: OK");
// warningShown = false;
}
if(resetMotors->isDown()){
resetting = 1;
/* for(int i=0;i<5;i++){
bytetwo[i]->setChecked(true);
}
resetMotors->setDown(false);
if(byteone[7]->isChecked()){
unlockMotors->setDown(true);
}
*/
// Mads HERTIL
}
if(unlockMotors->isChecked() && !byteone[7]->isChecked()){
byteone[7]->setChecked(true);
} else if(!unlockMotors->isChecked() && byteone[7]->isChecked()){
byteone[7]->setChecked(false);
}
industrialRobot->thread.serial->unlock();
}
if(resetting > 0){
ofSleepMillis(200);
}
/*
Fjernet af mads
int input = 0;
if(inputRadio[0]->isChecked()){
input = 0;
} else if(inputRadio[1]->isChecked()){
input = 1;
} else if(inputRadio[2]->isChecked()){
input = 2;
} else if(inputRadio[3]->isChecked()){
input = 3;
}
*/
/*
//Fjernet af mads
if(inputSet != input){
industrialRobot->thread.controller->setInput(input);
inputSet = input;
}*/
if(industrialRobot->thread.lock()){
if(industrialRobot->thread.panic){
unlockMotors->setChecked(false);
if(!warningShown){
industrialRobot->thread.unlock();
QMessageBox msgBox;
msgBox.setText("PANIC!!");
ofxVec3f curPos = industrialRobot->getCurrentTarget(false);
ofxVec3f curDir = industrialRobot->getCurrentDir(false);
msgBox.setInformativeText(QString((industrialRobot->thread.panicMessage+"\n\nCurrent position: "+ofToString(curPos.x,1)+" , "+ofToString(curPos.y,1)+" , "+ofToString(curPos.z,1)+"\nCurrent direction: "+ofToString(curDir.x,2)+" , "+ofToString(curDir.y,2)+" , "+ofToString(curDir.z,2)).c_str()));
msgBox.exec();
warningShown = true;
industrialRobot->thread.lock();
// industrialRobot->thread.panic = false;
// industrialRobot->thread.panicMessage = "No message";
}
} else {
// warningShown = false;
}
float rotations[5];
for(int i=0;i<5;i++){
rotations[i] = industrialRobot->thread.coreData.arms[i].rotation;
}
for(int i=0;i<5;i++){
motorSlider[i]->setMinimum(industrialRobot->thread.converter->getMinRotation(i, rotations)*100.0);
motorSlider[i]->setMaximum(industrialRobot->thread.converter->getMaxRotation(i, rotations)*100.0);
if(getControlMode() == 0 || getControlMode() >= 2 || initFlag){
// cout<<industrialRobot->thread.coreData.arms[i].rotation<<endl;
motorSlider[i]->setValue(industrialRobot->thread.coreData.arms[i].rotation*100.0);
motorSlider[i]->setEnabled(false);
} else if(getControlMode() == 1){
motorSlider[i]->setEnabled(true);
if(i == 2 && motorSlider[i]->value() == 10030){
industrialRobot->thread.coreData.arms[i].rotation = 100;
} else {
industrialRobot->thread.coreData.arms[i].rotation = motorSlider[i]->value() / 100.0;
}
motorLabel[i]->setText(ofToString(industrialRobot->thread.coreData.arms[i].rotation, 0).c_str());
}
float v1 = industrialRobot->thread.coreData.arms[i].rotation;
float v2 = industrialRobot->thread.converter->convertRotation(i);
motorLabel[i]->setText((ofToString(v1, 1)).c_str());
motorLabel2[i]->setText(("("+ofToString(v2, 1)+")").c_str());
if(setValue[i]->isDown()){
industrialRobot->thread.unlock();
bool ok;
double d = QInputDialog::getDouble(new QWidget, QString("Set angle"), "Angle", motorSlider[i]->value(), -10000, 10000, 2, &ok);
if (ok)
//doubleLabel->setText(QString("$%1").arg(d));
motorSlider[i]->setValue(d*100.0);
setValue[i]->setDown(false);
industrialRobot->thread.lock();
}
}
variantSlider->setValue(industrialRobot->thread.coreData.reverseHeadPercent*1000.0);
if(getControlMode() == 3){
for(int i=0;i<6;i++){
manualPosition[i]->setEnabled(true);
}
ofxVec3f target = ofxVec3f(manualPosition[0]->value()*10.0, manualPosition[1]->value()*10.0, manualPosition[2]->value()*10.0);
ofxVec3f targetDir = ofxVec3f(manualPosition[3]->value(), manualPosition[4]->value(), manualPosition[5]->value());
industrialRobot->thread.unlock();
bool couldAdd = industrialRobot->setGravityTarget(target, targetDir, 1.5, 3.0);
industrialRobot->thread.lock();
/* if(industrialRobot->thread.controller->legalPosition(target, targetDir, 1.0) || industrialRobot->thread.controller->legalPosition(target, targetDir, 0.0)){
industrialRobot->thread.controller->gravityTarget = target;
industrialRobot->thread.controller->gravityTargetDir = targetDir;
}
*/
manualPosition[0]->setValue(industrialRobot->thread.controller->gravityTarget.x*0.1);
manualPosition[1]->setValue(industrialRobot->thread.controller->gravityTarget.y*0.1);
manualPosition[2]->setValue(industrialRobot->thread.controller->gravityTarget.z*0.1);
manualPosition[3]->setValue(industrialRobot->thread.controller->gravityTargetDir.x);
manualPosition[4]->setValue(industrialRobot->thread.controller->gravityTargetDir.y);
manualPosition[5]->setValue(industrialRobot->thread.controller->gravityTargetDir.z);
} else {
for(int i=0;i<6;i++){
manualPosition[i]->setEnabled(false);
}
manualPosition[0]->setValue(industrialRobot->thread.controller->targetPosition.x*0.1);
manualPosition[1]->setValue(industrialRobot->thread.controller->targetPosition.y*0.1);
manualPosition[2]->setValue(industrialRobot->thread.controller->targetPosition.z*0.1);
manualPosition[3]->setValue(industrialRobot->thread.controller->targetDir.x);
manualPosition[4]->setValue(industrialRobot->thread.controller->targetDir.y);
manualPosition[5]->setValue(industrialRobot->thread.controller->targetDir.z);
}
if(currentControllerItem->locked){
controllerItemLocked->setChecked(true);
} else {
controllerItemLocked->setChecked(false);
}
industrialRobot->thread.unlock();
}
initFlag = false;
}
void AMCSubNode::update() {
double current, velocity, position;
processStatus();
if (getState() != AMCSubNode::OperationEnabled) {
setPosition(tpdo_ptr_->actual_position);
}
if (getState() == AMCSubNode::Fault) {
// go to ReadyToSwitchOn state
RTT::log(RTT::Warning) << "Fault" << RTT::endlog();
target_state_ = ResetFault;
} else if (getState() == AMCSubNode::QuickStopActive) {
// go to ReadyToSwitchOn state
RTT::log(RTT::Warning) << "QuickStop" << RTT::endlog();
target_state_ = DisableVoltage;
} else if (getState() == AMCSubNode::NotReadyToSwitchOn) {
// go to ReadyToSwitchOn state
//RTT::log(RTT::Warning) << "NotReadyToSwitchOn" << RTT::endlog();
target_state_ = ShutDown;
} else if (getState() == AMCSubNode::SwitchOnDisabled) {
// go to ReadyToSwitchOn state
//RTT::log(RTT::Warning) << "ReadyToSwitchOn" << RTT::endlog();
target_state_ = ShutDown;
} else if (getState() == AMCSubNode::ReadyToSwitchOn) {
// go to OperationDisabled state
//RTT::log(RTT::Warning) << "OperationDisabled" << RTT::endlog();
setPosition(tpdo_ptr_->actual_position);
target_state_ = SwitchOn;
} else if (getState() == AMCSubNode::SwitchedOn) {
//RTT::log(RTT::Warning) << "SwitchedOn" << RTT::endlog();
setPosition(tpdo_ptr_->actual_position);
if (position_valid_) {
RTT::log(RTT::Warning) << "enable" << RTT::endlog();
setControlMode(mode_);
target_state_ = EnableOperation;
} else {
RTT::log(RTT::Warning) << "homing" << RTT::endlog();
setControlMode(Homing);
target_state_ = BeginHoming;
homing_ = true;
}
} else if (getState() == AMCSubNode::OperationEnabled) {
//RTT::log(RTT::Warning) << "OperationEnabled" << RTT::endlog();
if (position_valid_) {
if (port_motor_position_command_.read(position) == RTT::NewData) {
setPosition(position);
}
if (port_motor_velocity_command_.read(velocity) == RTT::NewData) {
setVelocity(velocity);
}
if (port_motor_current_command_.read(current) == RTT::NewData) {
setCurrent(current);
}
// dio
bool dio;
if (port_do1_command_.read(dio) == RTT::NewData) {
if (dio) {
setDO(0);
} else {
resetDO(0);
}
}
if (port_do2_command_.read(dio) == RTT::NewData) {
if (dio) {
setDO(1);
} else {
resetDO(1);
}
}
if (port_do3_command_.read(dio) == RTT::NewData) {
if (dio) {
setDO(2);
} else {
resetDO(2);
}
}
if (port_do4_command_.read(dio) == RTT::NewData) {
if (dio) {
setDO(3);
} else {
resetDO(3);
}
}
} else {
if ((actualMode_ == Homing)) {
if (isHomingCompleted()) {
position_valid_ = true;
setPosition(tpdo_ptr_->actual_position);
setControlMode(mode_);
target_state_ = EnableOperation;
} else {
target_state_ = BeginHoming;
}
} else {
setControlMode(Homing);
target_state_ = BeginHoming;
}
}
}
enterStateNoCheck(target_state_);
if (position_valid_) {
port_motor_position_.write(getActualPosition());
port_motor_velocity_.write(getActualVelocity());
port_motor_current_.write(getActualCurrent());
}
}
void ContainerGrabber::runArmsFreeSpeed() {
setControlMode(leftArm, "openLoop");
setControlMode(rightArm, "openLoop");
setMotorsOpenLoop(leftArm, 1.0);
setMotorsOpenLoop(rightArm, 1.0);
}
Controller_MidLevel_SpeedLoop::Controller_MidLevel_SpeedLoop(int idDrone, const std::string &stackPath_in) /* throw(std::runtime_error) */ : started(false) {
std::cout << "Constructor: Controller_MidLevel_SpeedLoop" << std::endl;
try {
XMLFileReader my_xml_reader( stackPath_in+"configs/drone"+std::to_string(idDrone)+"/trajectory_controller_config.xml");
// Just in case, I set to zero all the internal variables of the controller object
vxci = 0.0; vyci = 0.0; yawci = 0.0; zci = 0.0;
vxs = 0.0; vys = 0.0; yaws = 0.0; zs = 0.0;
eps_vx = 0.0; eps_vy = 0.0; eps_yaw = 0.0; eps_z = 0.0;
vxd_int = 0.0; vyd_int = 0.0;
pitchd = 0.0; rolld = 0.0;
pitchc_int = 0.0; rollc_int = 0.0; dyawc_int = 0.0; dzc_int = 0.0;
pitchco = 0.0; rollco = 0.0; dyawco = 0.0; dzco = 0.0;
nlf_is_activated = false;
// Nacho's PID algorithm >>>> u[k] = Kp*e[k] + Ki*i[k] + Kd*de[k]
// read configuration file
double vxy_Kp, vxy_Ki, vxy_Kd, vxy_DeltaKp;
vxy_DeltaKp = my_xml_reader.readDoubleValue( {"trajectory_controller_config","middle_level_controller","speed_controller","vxy","DeltaKp"} );
vxy_Kp = vxy_DeltaKp*my_xml_reader.readDoubleValue( {"trajectory_controller_config","middle_level_controller","speed_controller","vxy","Kp"} );
vxy_Ki = vxy_DeltaKp*my_xml_reader.readDoubleValue( {"trajectory_controller_config","middle_level_controller","speed_controller","vxy","Ki"} );
vxy_Kd = vxy_DeltaKp*my_xml_reader.readDoubleValue( {"trajectory_controller_config","middle_level_controller","speed_controller","vxy","Kd"} );
nlf_is_activated = my_xml_reader.readIntValue( {"trajectory_controller_config","middle_level_controller","speed_controller","vxy","nlf_is_activated"} );
vxy_max = my_xml_reader.readDoubleValue( {"trajectory_controller_config","general_saturations","speed_control_loop","vxy_max"} );
double pitch_max, roll_max, dyaw_max, dz_at_ratio;
pitch_max = my_xml_reader.readDoubleValue( {"trajectory_controller_config","general_saturations","drone_commands","pitch_max"} );
roll_max = my_xml_reader.readDoubleValue( {"trajectory_controller_config","general_saturations","drone_commands","roll_max"} );
dyaw_max = my_xml_reader.readDoubleValue( {"trajectory_controller_config","general_saturations","drone_commands","dyaw_max"} );
dz_max = my_xml_reader.readDoubleValue( {"trajectory_controller_config","general_saturations","drone_commands","dz_max"} );
dz_at_ratio = my_xml_reader.readDoubleValue( {"trajectory_controller_config","general_saturations","speed_control_loop","dz_at_ratio"} );
dz_ct_max = (1-dz_at_ratio) * dz_max;
double yaw_Kp, yaw_Ki, yaw_Kd, yaw_DeltaKp;
yaw_DeltaKp = my_xml_reader.readDoubleValue( {"trajectory_controller_config","middle_level_controller","speed_controller","yaw","DeltaKp"} );
yaw_Kp = yaw_DeltaKp*my_xml_reader.readDoubleValue( {"trajectory_controller_config","middle_level_controller","speed_controller","yaw","Kp"} );
yaw_Ki = yaw_DeltaKp*my_xml_reader.readDoubleValue( {"trajectory_controller_config","middle_level_controller","speed_controller","yaw","Ki"} );
yaw_Kd = yaw_DeltaKp*my_xml_reader.readDoubleValue( {"trajectory_controller_config","middle_level_controller","speed_controller","yaw","Kd"} );
double z_Kp, z_Ki, z_Kd, z_DeltaKp;
z_DeltaKp = my_xml_reader.readDoubleValue( {"trajectory_controller_config","middle_level_controller","speed_controller","altitude","DeltaKp"} );
z_Kp = z_DeltaKp*my_xml_reader.readDoubleValue( {"trajectory_controller_config","middle_level_controller","speed_controller","altitude","Kp"} );
z_Ki = z_DeltaKp*my_xml_reader.readDoubleValue( {"trajectory_controller_config","middle_level_controller","speed_controller","altitude","Ki"} );
z_Kd = z_DeltaKp*my_xml_reader.readDoubleValue( {"trajectory_controller_config","middle_level_controller","speed_controller","altitude","Kd"} );
// Vx PID gains: Kp, Ki = Kp/Ti, Kd = Kp*Td
pid_vx.setGains( vxy_Kp, vxy_Ki, vxy_Kd); // set Kp, Ki = Kp/Ti, Kd = Kp*Td
if (!nlf_is_activated) {
// Saturation specified by config file
pid_vx.enableAntiWindup(true, pitch_max);
pid_vx.enableMaxOutput( true, pitch_max); // set enable, value. Maximum output is vx_max = 2.5-2.75 m/s
} else {
// Saturation performed by non-linear function
// (Parrot 1) Maximum output is vx_max = 2.5-2.75 m/s
pid_vx.enableAntiWindup(true, vxy_max);
pid_vx.enableMaxOutput( true, vxy_max);
}
// Vy PID gains: Kp, Ki = Kp/Ti, Kd = Kp*Td
pid_vy.setGains( vxy_Kp, vxy_Ki, vxy_Kd);
if (!nlf_is_activated) {
// Saturation specified by config file
pid_vy.enableAntiWindup(true, roll_max);
pid_vy.enableMaxOutput( true, roll_max);
} else {
// Saturation performed by non-linear function
// (Parrot 1) Maximum output is vy_max = 2.0-2.30 m/s
pid_vy.enableAntiWindup(true, vxy_max);
pid_vy.enableMaxOutput( true, vxy_max);
}
// yaw PID gains: Kp, Ki = Kp/Ti, Kd = Kp*Td
pid_yaw.setGains( yaw_Kp, yaw_Ki, yaw_Kd);
pid_yaw.enableMaxOutput(true, dyaw_max);
pid_yaw.enableMaxOutput( true, dyaw_max);
// z PID gains: Kp, Ki = Kp/Ti, Kd = Kp*Td
pid_z.setGains( z_Kp, z_Ki, z_Kd);
pid_z.enableMaxOutput( true, dz_ct_max);
pid_z.enableMaxOutput( true, dz_ct_max);
std::string init_control_mode_str = my_xml_reader.readStringValue( {"trajectory_controller_config","init_control_mode"} );
if ( init_control_mode_str.compare("speed") == 0 ) {
init_control_mode = Controller_MidLevel_controlMode::SPEED_CONTROL;
} else { if ( init_control_mode_str.compare("position") == 0 ) {
init_control_mode = Controller_MidLevel_controlMode::POSITION_CONTROL;
} else { // "trajectory"
init_control_mode = Controller_MidLevel_controlMode::SPEED_CONTROL;
throw std::runtime_error("Controller_MidLevel_SpeedLoop::Controller_MidLevel_SpeedLoop, inital control_mode cannot be TRAJECTORY_CONTROL");
return;
}
}
setControlMode(init_control_mode);
} catch ( cvg_XMLFileReader_exception &e) {
throw cvg_XMLFileReader_exception(std::string("[cvg_XMLFileReader_exception! caller_function: ") + BOOST_CURRENT_FUNCTION + e.what() + "]\n");
}
}
//
// Core Functions
//
int P2OSPtz::setup()
{
int err = 0;
int num_inits = 7;
is_on_ = true;
for (int i = 1; i < num_inits; i++) {
switch(i)
{
// case 0:
// do
// {
// ROS_DEBUG("Waiting for camera to power off.");
// err = setPower(POWER_OFF);
// } while (error_code_ == CAM_ERROR_BUSY);
// break;
case 1:
do
{
ROS_DEBUG("Waiting for camera to power on.");
err = setPower(POWER_ON);
} while (error_code_ == CAM_ERROR_BUSY);
break;
case 2:
do
{
ROS_DEBUG("Waiting for camera mode to set");
err = setControlMode();
} while (error_code_ == CAM_ERROR_BUSY);
break;
case 3:
do
{
ROS_DEBUG("Waiting for camera to initialize");
err = sendInit();
} while (error_code_ == CAM_ERROR_BUSY);
break;
case 4:
do
{
for(int i = 0; i < 3; i++)
{
ROS_DEBUG("Waiting for camera to set default tilt");
err = setDefaultTiltRange();
}
} while (error_code_ == CAM_ERROR_BUSY);
break;
case 5:
do
{
ROS_DEBUG("Waiting for camera to set initial pan and tilt");
err = sendAbsPanTilt(0, 0);
} while (error_code_ == CAM_ERROR_BUSY);
break;
case 6:
do
{
ROS_DEBUG("Waiting for camera to set initial zoom");
err = sendAbsZoom(0);
} while (error_code_ == CAM_ERROR_BUSY);
break;
default:
err = -7;
break;
}
// Check for erros after each attempt
if (err)
{
ROS_ERROR("Error initiliazing PTZ at stage %i", i);
switch(error_code_)
{
case CAM_ERROR_BUSY:
ROS_ERROR("Error: CAM_ERROR_BUSY");
break;
case CAM_ERROR_PARAM:
ROS_ERROR("Error: CAM_ERROR_PARAM");
break;
case CAM_ERROR_MODE:
ROS_ERROR("Error: CAM_ERROR_MODE");
break;
default:
ROS_ERROR("Error: Unknown error response from camera.");
break;
}
return(-1);
}
else
{
ROS_DEBUG("Passed stage %i of PTZ initialization.", i);
}
}
ROS_DEBUG("Finished initialization of the PTZ.");
return 0;
}
