diff --git a/bin/r2_control b/bin/r2_control
index c4b0cf6ee831b56b23e9be76f1b8fb4137c218f9..ba75844628114500da2cb7bb9a35ff57d3c950a2 100755
Binary files a/bin/r2_control and b/bin/r2_control differ
diff --git a/run.sh b/run.sh
index 102768f45e765453d802c3d08fa3d9adb0c08438..d089c8a69667ecc9c0fbc4f4e0217362250569d5 100755
--- a/run.sh
+++ b/run.sh
@@ -1,4 +1,4 @@
-export ROS_PACKAGE_PATH=/opt/ros/indigo/share:/opt/ros/indigo/stacks:~/Documents/raven_2:~/Documents/raven_2/raven_visualization;
+export ROS_PACKAGE_PATH=/opt/ros/indigo/share:/opt/ros/indigo/stacks:~/homa_wksp/raven_2:~/homa_wksp/raven_2/raven_visualization;
diff --git a/src/raven/fwd_cable_coupling.cpp b/src/raven/fwd_cable_coupling.cpp
index 891d9e1602127d512bcba09bca79b34ecc8d996f..ea3c6a360317310776ae30ccfa6e5d615eed2788 100755
--- a/src/raven/fwd_cable_coupling.cpp
+++ b/src/raven/fwd_cable_coupling.cpp
@@ -180,6 +180,7 @@ void fwdMechCableCoupling(struct mechanism *mech)
th6 = (1.0/tr6) * (m6 - m4/GB_RATIO);
th7 = (1.0/tr7) * (m7 - m4/GB_RATIO);
}
+// Commented out
#ifndef simulator
// Now have solved for th1, th2, d3, th4, th5, th6
mech->joint[SHOULDER].jpos = th1;// - mech->joint[SHOULDER].jpos_off;
@@ -189,6 +190,8 @@ void fwdMechCableCoupling(struct mechanism *mech)
mech->joint[WRIST].jpos = th5;// - mech->joint[WRIST].jpos_off;
mech->joint[GRASP1].jpos = th6;// - mech->joint[GRASP1].jpos_off;
mech->joint[GRASP2].jpos = th7;// - mech->joint[GRASP2].jpos_off;
+
+// Commented out
#else
mech->joint[SHOULDER].jpos = mech->joint[SHOULDER].jpos_d;// - mech->joint[SHOULDER].jpos_off;
mech->joint[ELBOW].jpos = mech->joint[ELBOW].jpos_d;// - mech->joint[ELBOW].jpos_off;
diff --git a/src/raven/local_io.cpp b/src/raven/local_io.cpp
index 95e452a10e856ccb331605720e6229e4c26bcd21..018a1eb53619a9473557d56ce9b8c9e91253d4e9 100755
--- a/src/raven/local_io.cpp
+++ b/src/raven/local_io.cpp
@@ -199,6 +199,7 @@ void teleopIntoDS1(struct u_struct *us_t)
data1.rd[i].grasp -= us_t->grasp[armidx];
if (data1.rd[i].grasp>graspmax) data1.rd[i].grasp=graspmax;
else if(data1.rd[i].grasp<graspmin) data1.rd[i].grasp=graspmin;
+
#else
// Set Position command
data1.xd[i].x = us_t->delx[armidx];
@@ -222,6 +223,11 @@ void teleopIntoDS1(struct u_struct *us_t)
for (int k=0;k<3;k++)
data1.rd[1].R[j][k] = us_t->R_r[j][k];
}
+
+ // Get the encoder values
+ for (int ch=0;ch<16;ch++)
+ data1.enc_d[ch] = us_t->encVals[ch];
+
// Get the initial joint positions from input
/*if (us_t->sequence == 1)
for (int j=0;j<16;j++)
diff --git a/src/raven/log.cpp b/src/raven/log.cpp
index 5307841eadc7b2de1b639a35c57dad5422a0efd5..897bae51ec55f478e94f3781a5e7599e3c5509cf 100755
--- a/src/raven/log.cpp
+++ b/src/raven/log.cpp
@@ -36,7 +36,7 @@ const static size_t MAX_MSG_LEN =1024;
//#define simulator_logging
#ifdef save_logs
#include <fstream>
-extern char raven_path[];
+extern char* raven_path;
extern int inject_mode;
extern int logging;
/**\fn int log_file(const char* fmt,...)
diff --git a/src/raven/network_layer.cpp b/src/raven/network_layer.cpp
index 0939fe970a97dd966a269c5c86384283b177534b..bfd5fefcf3fb4271c8c0d5ec84aaddf279250eaa 100755
--- a/src/raven/network_layer.cpp
+++ b/src/raven/network_layer.cpp
@@ -326,8 +326,8 @@ void* network_process(void* param1)
#ifdef save_logs
log_file("NETWORK) Receieved Data on Socket, Size = %d", uSize);
- //log_msg("Pos Arm 0 = %d, %d, %d", u.delx[0], u.dely[0], u.delz[0]);
- //log_msg("Pos Arm 1 = %d, %d, %d", u.delx[1], u.dely[1], u.delz[1]);
+ //log_msg("Pos Arm 0 = %d, %d, %d", u.delx[0], u.dely[0], u.delz[0]);
+ //log_msg("Pos Arm 1 = %d, %d, %d", u.delx[1], u.dely[1], u.delz[1]);
#endif
diff --git a/src/raven/overdrive_detect.cpp b/src/raven/overdrive_detect.cpp
index 236604851a49db19f077befdf3a469abfd8ef0e6..9f19f53f2da94bad857af3029e266f34e4f4da78 100755
--- a/src/raven/overdrive_detect.cpp
+++ b/src/raven/overdrive_detect.cpp
@@ -59,7 +59,6 @@ int overdriveDetect(struct device *device0)
{
_joint = &(device0->mech[i].joint[j]);
int _dac_max = DOF_types[_joint->type].DAC_max;
-
// Kill current if greater than MAX_INST_DAC. Probably indicates a problem.
if (abs(_joint->current_cmd) > MAX_INST_DAC)
{
diff --git a/src/raven/rt_process_preempt.cpp b/src/raven/rt_process_preempt.cpp
index d13e2b8f8988336854ed60e2c8d12740bbad5677..3d5134e578732d3719587fdf6fb65921bc6e399c 100755
--- a/src/raven/rt_process_preempt.cpp
+++ b/src/raven/rt_process_preempt.cpp
@@ -95,10 +95,7 @@ int NUM_MECH=0; // Define NUM_MECH as a C variable, not a c++ variable
#ifdef save_logs
#include <fstream>
-//char* ROS_PACKAGE_PATH;
-//ROS_PACKAGE_PATH = getenv("ROS_PACKAGE_PATH");
-//if (ROS_PACKAGE_PATH != NULL)
-char raven_path[] = "/home/alemzad1/homa_wksp/raven_2";
+char* raven_path = new char[100];
int inject_mode;
int logging = 0;
int no_pack_cnt = 0;
@@ -353,9 +350,13 @@ static void *rt_process(void* )
//Update Atmel Output Pins
updateAtmelOutputs(&device0, currParams.runlevel);
-#ifdef simulator
+#ifndef simulator
//Fill USB Packet and send it out
putUSBPackets(&device0); //disable usb for par port test
+#else
+#ifdef log_USB
+ writeUSBPackets(&device0);
+#endif
#endif
//Publish current raven state
publish_ravenstate_ros(&device0,&currParams); // from local_io
@@ -452,16 +453,41 @@ int main(int argc, char **argv)
#endif
#ifdef save_logs
+ char* ROS_PACKAGE_PATH;
+ ROS_PACKAGE_PATH = getenv("ROS_PACKAGE_PATH");
+ if (ROS_PACKAGE_PATH!= NULL)
+ {
+ raven_path = strtok(ROS_PACKAGE_PATH,":");
+ while(raven_path!= NULL){
+ if (strstr(raven_path,"raven_2") != NULL){
+ printf("%s\n",raven_path);
+ break;
+ }
+ raven_path = strtok(NULL,":");
+ }
+ }
+
std::ofstream logfile;
log_msg("************** Inject mode = %d\n",inject_mode);
- char buff[50];
+ char buff[50];
if (inject_mode == 0)
sprintf(buff,"%s/sim_log.txt", raven_path);
else
sprintf(buff,"%s/fault_log_%d.txt", raven_path, inject_mode);
- logfile.open(buff,std::ofstream::out);
+ logfile.open(buff,std::ofstream::out);
+
+#ifdef log_USB
+ std::ofstream USBlogfile;
+ sprintf(buff,"%s/USB0_log.txt", raven_path);
+ USBlogfile.open(buff,std::ofstream::out);
+ USBlogfile.close();
+ sprintf(buff,"%s/USB1_log.txt", raven_path);
+ USBlogfile.open(buff,std::ofstream::out);
+ USBlogfile.close();
#endif
+#endif
+
// init reconfigure
dynamic_reconfigure::Server<raven_2::MyStuffConfig> srv;
diff --git a/src/raven/rt_raven.cpp b/src/raven/rt_raven.cpp
index ef5c586ca6ea97d21b3a9467c798466dcefc098d..59f984a222482bee5e2a04888bebcffee0cda67b 100755
--- a/src/raven/rt_raven.cpp
+++ b/src/raven/rt_raven.cpp
@@ -282,6 +282,9 @@ int raven_cartesian_space_command(struct device *device0, struct param_pass *cur
_mech = NULL; _joint = NULL;
while (loop_over_joints(device0, _mech, _joint, i,j) )
{
+#ifdef simulator_packetgen
+ mpos_PD_control(_joint);
+#else
if (currParams->runlevel != RL_PEDAL_DN)
{
_joint->tau_d=0;
@@ -290,6 +293,7 @@ int raven_cartesian_space_command(struct device *device0, struct param_pass *cur
{
mpos_PD_control(_joint);
}
+#endif
}
// Gravity compensation calculation
diff --git a/src/raven/state_estimate.cpp b/src/raven/state_estimate.cpp
index ea07dba7b0cee2841e13aa9978072eda8e7d4b2e..f06f5c7b6e63d6e120ff7bc9cc2fd4eabdf901dd 100755
--- a/src/raven/state_estimate.cpp
+++ b/src/raven/state_estimate.cpp
@@ -50,12 +50,10 @@ void stateEstimate(struct robot_device *device0)
for (j = 0; j < MAX_DOF_PER_MECH; j++)
{
_joint = &(device0->mech[i].joint[j]);
-
// if (_joint->type == TOOL_ROT_GOLD || _joint->type == TOOL_ROT_GOLD)
// encToMPos(_joint);
// else
getStateLPF(_joint, device0->mech[i].tool_type);
-
}
}
}
diff --git a/src/raven/t_to_DAC_val.cpp b/src/raven/t_to_DAC_val.cpp
index a3ceeeb8d2bea4ce005ba887d2469dcce142f7dd..5dcc427a42903221d14ab35eeca89d93223c869a 100755
--- a/src/raven/t_to_DAC_val.cpp
+++ b/src/raven/t_to_DAC_val.cpp
@@ -73,8 +73,6 @@ int TorqueToDAC(struct device *device0)
device0->mech[i].joint[j].current_cmd = tToDACVal( &(device0->mech[i].joint[j]) ); // Convert torque to DAC value
-
-
if ( soft_estopped )
device0->mech[i].joint[j].current_cmd = 0;
diff --git a/src/raven/update_device_state.cpp b/src/raven/update_device_state.cpp
index 33aae79c13513a50bffd6bd76a7ae32976ef4c54..77b595aba0b3c659b47099ebb4f2e48905e94714 100755
--- a/src/raven/update_device_state.cpp
+++ b/src/raven/update_device_state.cpp
@@ -99,27 +99,10 @@ int updateDeviceState(struct param_pass *currParams, struct param_pass *rcvdPara
// set desired mech position in pedal_down runlevel
if (currParams->runlevel == RL_PEDAL_DN)
{
+
#ifdef simulator
- //log_file("RT_PROCESS) Pedal is down. Update device state.\n");
- //currParams->robotControlMode = cartesian_space_control;////Added
if (currParams->last_sequence == 1)
{
- //for (int j=0;j<16;j++)
- // log_file("jpos %d = %f\n", j,rcvdParams->jpos_d[j]);
- /*device0->mech[0].joint[SHOULDER].jpos_d = rcvdParams->jpos_d[0];
- device0->mech[0].joint[ELBOW].jpos_d = rcvdParams->jpos_d[1];
- device0->mech[0].joint[Z_INS].jpos_d = rcvdParams->jpos_d[2];
- device0->mech[0].joint[TOOL_ROT].jpos_d = rcvdParams->jpos_d[4];
- device0->mech[0].joint[WRIST].jpos_d = rcvdParams->jpos_d[5];
- device0->mech[0].joint[GRASP1].jpos_d = rcvdParams->jpos_d[6];
- device0->mech[0].joint[GRASP2].jpos_d = rcvdParams->jpos_d[7];
- device0->mech[1].joint[SHOULDER].jpos_d = rcvdParams->jpos_d[8];
- device0->mech[1].joint[ELBOW].jpos_d = rcvdParams->jpos_d[9];
- device0->mech[1].joint[Z_INS].jpos_d = rcvdParams->jpos_d[10];
- device0->mech[1].joint[TOOL_ROT].jpos_d = rcvdParams->jpos_d[12];
- device0->mech[1].joint[WRIST].jpos_d = rcvdParams->jpos_d[13];
- device0->mech[1].joint[GRASP1].jpos_d = rcvdParams->jpos_d[14];
- device0->mech[1].joint[GRASP2].jpos_d = rcvdParams->jpos_d[15];*/
device0->mech[0].joint[SHOULDER].jpos_d = 0.521722;
device0->mech[0].joint[ELBOW].jpos_d = 1.585218;
device0->mech[0].joint[Z_INS].jpos_d = 0.400515;
@@ -135,6 +118,15 @@ int updateDeviceState(struct param_pass *currParams, struct param_pass *rcvdPara
device0->mech[1].joint[GRASP1].jpos_d = 0.702793;
device0->mech[1].joint[GRASP2].jpos_d = 0.729010;
}
+#endif
+#ifdef simulator_packetgen
+ for (int i = 0; i < NUM_MECH; i++)
+ {
+ for (int ch=0;ch<8;ch++)
+ {
+ device0->mech[i].joint[ch].enc_val = rcvdParams->enc_d[i*8+ch];
+ }
+ }
#endif
for (int i = 0; i < NUM_MECH; i++)
{