bool teo::FakeControlboard::getControlModes(int *modes) {
CD_DEBUG("\n");
bool ok = true;
for(unsigned int i=0; i < axes; i++)
ok &= getControlMode(i,&(modes[i]));
return ok;
}
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() );
}
/// Returns the current internal state of the LocalAgent (Automaton) within the cell and received as parameter
int luaCell::getCurrentStateName( lua_State *L )
{
luaLocalAgent *agent = Luna<luaLocalAgent>::check(L, -1);
ControlMode* controlMode = getControlMode((LocalAgent*)agent);
if( controlMode) lua_pushstring( L, controlMode->getControlModeName( ).c_str() );
else lua_pushnil(L);
return 1;
}
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;
}
/*
* 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;
}
