void test_particle_update() {
Vector3d object(3.35, -1.11, 0.8);
Arm::ArmType arm_type = Arm::ArmType::right;
bool view = true;
PR2System sys(object, arm_type, view);
PR2* brett = sys.get_brett();
Arm* arm = sys.get_arm();
rave::EnvironmentBasePtr env = brett->get_env();
sleep(2);
arm->set_posture(Arm::Posture::mantis);
// setup scenario
VectorJ j_t_real = arm->get_joint_values(), j_tp1_real;
VectorJ j_t = j_t_real, j_tp1; // i.e. no belief == actual
VectorU u_t = VectorU::Zero();
MatrixP P_t = setup_particles(env), P_tp1;
LOG_INFO("Origin particles");
std::vector<ParticleGaussian> particle_gmm_t;
sys.fit_gaussians_to_pf(P_t, particle_gmm_t);
sys.display(j_t, particle_gmm_t);
sys.execute_control_step(j_t_real, j_t, u_t, P_t, j_tp1_real, j_tp1, P_tp1);
LOG_INFO("New particles")
std::vector<ParticleGaussian> particle_gmm_tp1;
sys.fit_gaussians_to_pf(P_tp1, particle_gmm_tp1);
sys.display(j_tp1, particle_gmm_tp1);
}
void GamepadArmControl(Gamepad *gp)
{
// arm shoulder control
if ( gp->GetButton(Gamepad::kBottomButton) )
_arm->lowerShoulder();
else if ( gp->GetButton(Gamepad::kTopButton) )
_arm->raiseShoulder();
// claw control w/top right trigger
if ( gp->GetButton(Gamepad::kRightTopTrigger) )
_claw->open();
else
_claw->close();
double stick = gp->GetLeftY(); // FIXME: is the minus sign necessary????
SmartDashboard::Log(stick, "Elbow Control Stick");
double elbowPower = stick * -ELBOW_MAX_POWER;
_arm->_elbowController->Set(elbowPower);
}
void test_pr2_system() {
Vector3d object(3.35, -1.11, 0.8);
Arm::ArmType arm_type = Arm::ArmType::right;
bool view = true;
PR2System sys(object, arm_type, view);
PR2* brett = sys.get_brett();
Arm* arm = sys.get_arm();
rave::EnvironmentBasePtr env = brett->get_env();
sleep(2);
arm->set_posture(Arm::Posture::mantis);
// setup particles
MatrixP P = setup_particles(env);
// test plotting
VectorJ j = arm->get_joint_values();
std::vector<ParticleGaussian> particle_gmm;
particle_gmm.push_back(ParticleGaussian(Vector3d::Zero(), Matrix3d::Identity(), P.leftCols(M_DIM/2), 1));
particle_gmm.push_back(ParticleGaussian(Vector3d::Zero(), Matrix3d::Identity(), P.rightCols(M_DIM/2), 1));
sys.display(j, particle_gmm);
}
void test_fk() {
Vector3d object(3.35, -1.11, 0.8);
Arm::ArmType arm_type = Arm::ArmType::right;
bool view = true;
PR2System sys(object, arm_type, view);
PR2* brett = sys.get_brett();
Arm* arm = sys.get_arm();
Camera* cam = sys.get_camera();
rave::EnvironmentBasePtr env = brett->get_env();
arm->set_posture(Arm::Posture::mantis);
sleep(1);
VectorJ j = arm->get_joint_values();
Matrix4d actual_arm_pose = rave_utils::transform_from_to(brett->get_robot(), Matrix4d::Identity(), "r_gripper_tool_frame", "world");
Matrix4d fk_arm_pose = arm->get_pose(j);
std::cout << "actual_arm_pose:\n" << actual_arm_pose << "\n\n";
std::cout << "fk_arm_pose:\n" << fk_arm_pose << "\n\n";
Matrix4d actual_cam_pose = rave_utils::rave_to_eigen(cam->get_sensor()->GetTransform());
Matrix4d fk_cam_pose = cam->get_pose(j);
std::cout << "actual_cam_pose:\n" << actual_cam_pose << "\n\n";
std::cout << "fk_cam_pose:\n" << fk_cam_pose << "\n\n";
}
int main(int argc, char *argv[])
{
signal(SIGINT, stopsignal);
// Connect to the arm
start_arm();
sleep(1);
vec values = arm.sense();
arm.set_pose(values(0), values(1), values(2), values(3), values(4), values(5));
sense = values;
values += vec({ 90, 90, 0, 90, 90, 0 });
iValue[0] = (values(0));
iValue[1] = (values(1));
iValue[2] = (values(2));
iValue[3] = (values(3));
iValue[4] = (values(4));
iValue[5] = (values(5));
// Run loop
thread ui_thread(run_ui);
ui_thread.join(); // wait until the ui is finished
for (int i = 0; i < 100; i++)
{
arm.set_pose(0, 0, 0, 0, 0, 0, false);
}
stop_arm();
return 0;
}
void update2(double value) {
//goal = Point3f(cos(angle*PI / 180.0f), sin(angle*PI / 180.0f), (sin(zangle*PI / 180.0f)));
//Point3f goal(cos(angle*PI/180.0f), sin(angle*PI/180.0f), 0);
//goal.normalize();
//goal *= 5;
//goal += Vector3f(-3, 0, -1);
//goal = Vector3f(0, 0, 7);
//mainArm.solve(goal, 100);
//Point3f secGoal = Point3f(goal[0], goal[1], -goal[2]);
//secArm.solve(secGoal, 100);
//display();
for (int i = 0; i < 50; i++) {
if (i>20) {
goal = Point3f(3, 0, 1);
//goal += Vector3f(20, 4, 4);
mainArm.solve(goal, 100);
display();
}
}
for (int i = 0; i < 50;i++){
if(i>25){
goal = Vector3f(value+3, 0, 3);
mainArm.solve(goal, 100);
display();
}
}
}
void test_figtree() {
Vector3d object(3.35, -1.11, 0.8);
Arm::ArmType arm_type = Arm::ArmType::right;
bool view = true;
PR2System sys(object, arm_type, view);
PR2* brett = sys.get_brett();
Arm* arm = sys.get_arm();
rave::EnvironmentBasePtr env = brett->get_env();
arm->set_posture(Arm::Posture::mantis);
sleep(1);
MatrixP P = setup_particles(env);
std::vector<ParticleGaussian> particle_gmm;
tc.start("figtree");
sys.fit_gaussians_to_pf(P, particle_gmm);
tc.stop("figtree");
std::cout << "particle_gmm size: " << particle_gmm.size() << "\n";
for(int i=0; i < particle_gmm.size(); ++i) {
std::cout << "mean: " << particle_gmm[i].mean.transpose() << "\n";
std::cout << "cov diag: " << particle_gmm[i].cov.diagonal().transpose() << "\n";
std::cout << "pct: " << particle_gmm[i].pct << "\n";
}
tc.print_all_elapsed();
sys.display(arm->get_joint_values(), particle_gmm);
}
/* ************************************************************************** */
Pose2MobileArm::Pose2MobileArm(const Arm& arm, const gtsam::Pose3& base_T_arm) :
Base(arm.dof() + 3, arm.dof() + 1), base_T_arm_(base_T_arm), arm_(arm) {
// check arm base pose, warn if non-zero
if (!arm_.base_pose().equals(Pose3::identity(), 1e-6))
cout << "[Pose2MobileArm] WARNING: Arm has non-zero base pose, this base pose will be override. "
"Set the base_T_arm in Pose2MobileArm." << endl;
}
/* Move arm to various places */
void test_move(Arm &arm) {
arm.move_to( iPair(4,2) );
arm.move_to( iPair(3,0) );
arm.move_to( iPair(2,2) );
arm.move_to( iPair(0,0) );
}
/* ************************************************************************** */
Pose2MobileVetLinArm::Pose2MobileVetLinArm(const Arm& arm, const gtsam::Pose3& base_T_torso,
const gtsam::Pose3& torso_T_arm, bool reverse_linact) :
Base(arm.dof() + 4, arm.dof() + 2), base_T_torso_(base_T_torso), torso_T_arm_(torso_T_arm),
reverse_linact_(reverse_linact), arm_(arm) {
// check arm base pose, warn if non-zero
if (!arm_.base_pose().equals(Pose3::identity(), 1e-6))
cout << "[Pose2MobileArm] WARNING: Arm has non-zero base pose, this base pose will be override. "
"Set the base_T_torso and torso_T_arm in Pose2MobileArm." << endl;
}
bool mtsIntuitiveResearchKitConsole::AddArm(mtsComponent * genericArm, const mtsIntuitiveResearchKitConsole::Arm::ArmType CMN_UNUSED(armType))
{
// create new required interfaces to communicate with the components we created
Arm * newArm = new Arm(genericArm->GetName(), "");
if (SetupAndConnectInterfaces(newArm)) {
mArms.push_back(newArm);
return true;
}
CMN_LOG_CLASS_INIT_ERROR << GetName() << ": AddArm, unable to add new arm. Are you adding two arms with the same name? "
<< newArm->Name() << std::endl;
delete newArm;
return false;
}
void test_board_move(Arm &arm) {
Move move(iPair(4,1), iPair(4, 2));
/* Move arm according to move */
arm.execute_move(move);
/* Move arm to 0,0) */
arm.move_to(iPair(0,0));
/* move = Move(iPair(5, 0), iPair(4,0)); */
/* arm.execute_move(move); */
/* arm.move_to( iPair(3,0) ); */
/* arm.move_to(iPair(0,0) ); */
}
void dropInBox(int place)
{
arm.gotoPlace(place);
needle.down();
delay(500);
needle.up();
}
void run_ui(void)
{
string winname = "Press 'q' to quit";
namedWindow(winname);
createTrackbar("Joint0", winname, &iValue[0], 180, joint0callback, NULL);
createTrackbar("Joint1", winname, &iValue[1], 180, joint1callback, NULL);
createTrackbar("Joint2", winname, &iValue[2], 180, joint2callback, NULL);
createTrackbar("Joint3", winname, &iValue[3], 180, joint3callback, NULL);
createTrackbar("Joint4", winname, &iValue[4], 180, joint4callback, NULL);
createTrackbar("Claw", winname, &iValue[5], 180, joint5callback, NULL);
cv::Mat bufimage(480, 720, CV_8UC3);
char numbuf[64];
for (;;)
{
bufimage = Scalar(255, 255, 255);
vec values = sense + vec({ 90, 90, 0, 90, 90, 0 });
// Display the value bars
for (int i = 0; i < (int)values.n_elem; i++)
{
Point topleft(80, 20 + i * 80);
Point btmright(values(i) + 80, 60 + i * 80);
rectangle(bufimage, topleft, btmright, Scalar(255, 32, 10));
sprintf(numbuf, "%0.2lf", sense(i));
putText(bufimage, string(numbuf), Point(20, 60 + i * 80), FONT_HERSHEY_PLAIN, 1, Scalar(255, 32, 10), 1, 8, false);
}
// Display the forward kinematics
arm.arm_read = sense;
mat positions(3, 8, fill::zeros);
for (int i = 0; i <= 6; i++)
{
vec eepos = arm.get_end_effector_pos(i);
positions.col(i+1) = eepos;
sprintf(numbuf, "[%0.2lf %0.2lf %0.2lf]", eepos(0), eepos(1), eepos(2));
putText(bufimage, string(numbuf), Point(280, 60 + i * 60), FONT_HERSHEY_PLAIN, 1, Scalar(255, 32, 10), 1, 8, false);
}
// Draw shit
vector<Scalar> colors = { Scalar(0,0,0), Scalar(255,0,128), Scalar(255,0,0), Scalar(255,128,0), Scalar(0,128,0), Scalar(0,128,255), Scalar(0,0,255) };
for (int i = 0; i <= 6; i++)
{
vec pos1 = positions.col(i);
pos1 = vec({ pos1(1), -pos1(2) });
pos1 = 4 * pos1 + vec({ 560, 150 });
vec pos2 = positions.col(i+1);
pos2 = vec({ pos2(1), -pos2(2) });
pos2 = 4 * pos2 + vec({ 560, 150 });
line(bufimage, Point(pos1(0), pos1(1)), Point(pos2(0), pos2(1)), colors[i], 2);
}
imshow(winname, bufimage);
int k = waitKey(30);
if ((k & 0x7f) == 'q')
{
break;
}
}
}
bool mtsIntuitiveResearchKitConsole::AddArm(mtsComponent * genericArm, const mtsIntuitiveResearchKitConsole::Arm::ArmType CMN_UNUSED(armType))
{
// create new required interfaces to communicate with the components we created
Arm * newArm = new Arm(genericArm->GetName(), "");
if (AddArmInterfaces(newArm)) {
ArmList::iterator armIterator = mArms.find(newArm->mName);
if (armIterator != mArms.end()) {
mArms[newArm->mName] = newArm;
return true;
}
}
CMN_LOG_CLASS_INIT_ERROR << GetName() << ": AddArm, unable to add new arm. Are you adding two arms with the same name? "
<< newArm->Name() << std::endl;
delete newArm;
return false;
}
~Sling() {
arm.move_to( iPair(0,0) );
mav(motor_shoulder, -200);
mav(motor_arm, -200);
sleep(0.2);
ao();
}
bool ARM_module::initialize()
{
if(this->isInitialized() == true)
return 1;
else
return arm.initialize();
}
void pickup()
{
arm.gotoPickup();
needle.middle();
delay(2000);
needle.up();
}
void update() {
angle += 360.0 / 60.0;
zangle += 360.0f / 100.0f;
//goal = Point3f(cos(angle*PI / 180.0f), sin(angle*PI / 180.0f), (sin(zangle*PI / 180.0f)));
//Point3f goal(cos(angle*PI/180.0f), sin(angle*PI/180.0f), 0);
//goal.normalize();
//goal *= 5;
//goal += Vector3f(-3, 0, -1);
//goal = Vector3f(0, 0, 7);
//mainArm.solve(goal, 100);
//Point3f secGoal = Point3f(goal[0], goal[1], -goal[2]);
//secArm.solve(secGoal, 100);
//display();
goal = Vector3f(20, 4, 4);
mainArm.solve(goal, 100);
display();
}
int main() {
cout << "Begin" << endl;
//test_sensors();
/* Arm back to 0,0 position */
Arm arm;
arm.reset();
/* Move arm */
test_board_move(arm);
//test_move(arm);
/* Arm back to 0,0 position */
arm.reset();
cout << "Done." << endl;
return 0;
}
void LogToDashboard()
{
_arm->logInfo();
_lightSensors->logInfo();
// minibot deployment
SmartDashboard::Log(MinibotDeployable(), "Minibot Deployable");
SmartDashboard::Log(_minibotDeployerSolenoid->Get(), "Minibot Deployed");
}
void display() {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // clear the color buffer
// set camera parameters
GLdouble centerX = eyeX;
GLdouble centerY = eyeY + 1;
GLdouble centerZ = eyeZ;
GLdouble upX = 0.;
GLdouble upY = 0.;
GLdouble upZ = 1.;
glMatrixMode(GL_MODELVIEW); // indicate we are specifying camera transformations
glLoadIdentity();
gluLookAt(eyeX, eyeY, eyeZ,
centerX, centerY, centerZ,
upX, upY, upZ);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(60, 1, 1, 100);
// drawing is done here
mainArm.draw();
// secArm.draw();
float c = 0.2;
Point3f a0 = goal + Vector3f(-c, 0, c);
Point3f a1 = goal + Vector3f(0, 0, -c);
Point3f a2 = goal + Vector3f(c, 0, c);
Vector3f n2(0, -1, 0);
glBegin(GL_TRIANGLES);
glNormal3f(n2[0], n2[1], n2[2]);
glVertex3f(a0[0], a0[1], a0[2]);
glVertex3f(a1[0], a1[1], a1[2]);
glVertex3f(a2[0], a2[1], a2[2]);
glEnd();
/*Point3f secGoal = Point3f(goal[0], goal[1], -goal[2]);
a0 = secGoal + Vector3f(-c, 0, c);
a1 = secGoal + Vector3f(0, 0, -c);
a2 = secGoal + Vector3f(c, 0, c);
glBegin(GL_TRIANGLES);
glNormal3f(n2[0], n2[1], n2[2]);
glVertex3f(a0[0], a0[1], a0[2]);
glVertex3f(a1[0], a1[1], a1[2]);
glVertex3f(a2[0], a2[1], a2[2]);*/
glEnd();
// end drawing
glFlush();
glutSwapBuffers(); // swap buffers (we earlier set double buffer)
}
bool AngularMotor::moveArm(side s,Arm A){
if( s == right_side ){
effect << " (" << "rae1 " << A.getaj1() << ") ";
effect << " (" << "rae2 " << A.getaj2() << ") ";
effect << " (" << "rae3 " << A.getaj3() << ") ";
effect << " (" << "rae4 " << A.getaj4() << ") ";
}else if ( s == left_side ){
effect << " (" << "lae1 " << A.getaj1() << ") ";
effect << " (" << "lae2 " << A.getaj2() << ") ";
effect << " (" << "lae3 " << A.getaj3() << ") ";
effect << " (" << "lae4 " << A.getaj4() << ") ";
}
return true;
}
void AddArm()
{
cin.get();
cout << endl;
cout << "********************************************************" << endl;
cout << "* Add ARM vip *" << endl;
cout << "********************************************************" << endl;
cout << endl;
try
{
Arm* newArm = new Arm();
if(!newArm)
throw new Except("Add ARM vip failed!");
newArm->Init();
cin.get();
vip.push_back(newArm);
}
catch (Except* pEx)
{
cout<<endl;
cout << "********************************************************" << endl;
cout << "* It appear error as below : *" << endl;
cout << "* pEx->what() *" << endl;
cout << "********************************************************" << endl;
cout<<endl;
}
cout << endl;
cout << "****************************************************" << endl;
cout << "* Press ENTER to back to menu *" << endl;
cout << "****************************************************" << endl;
cin.get();
}
/**
* This function runs in the control thread, and continually sets the motors to the correct speed.
* The speed is determined by the difference in angle divided by a constant.
*/
void Arm::control_arm_motor( void *object )
{
Arm *instance = (Arm *) object;
instance->upperArmAngle = instance->GetTilt();
// Runs when the difference in angles is large enough
//TODO: fix kCloseEnough with real value
while ( fabs( instance->upperArmAngle - instance->arm_control_angle ) > kCloseEnough )
{
instance->upperArmAngle = instance->GetTilt();
if (((instance->upperArmAngle-instance->arm_control_angle ) / 700 < 0.5) && ((instance->upperArmAngle-instance->arm_control_angle ) / 2000 > 0) )
{
instance->armMotor.Set(0.5);
}
else if (((instance->upperArmAngle-instance->arm_control_angle ) / 700 > -0.5) && ((instance->upperArmAngle-instance->arm_control_angle ) / 2000 < 0) )
{
instance->armMotor.Set(-0.5);
}
else if ((instance->upperArmAngle-instance->arm_control_angle ) / 700 >= 1.0 )
{
instance->armMotor.Set(1.0);
}
else if ((instance->upperArmAngle-instance->arm_control_angle ) / 700 <= -1.0 )
{
instance->armMotor.Set(-1.0);
}
else
{
instance->armMotor.Set( ( instance->upperArmAngle-instance->arm_control_angle ) / 700 );
}
Wait( 0.05 );
}
instance->armMotor.Set( 0.0 );
}
void start_arm()
{
arm.connect();
if (!arm.connected())
{
printf("[ARM TEST] Not connected to anything, disconnecting...\n");
arm.disconnect();
exit(1);
}
arm.load_calibration_params("calib_params.json");
if (!arm.calibrated())
{
arm.disconnect();
exit(1);
}
}
int checkColor()
{
arm.gotoCheck();
delay(3000);
return Arm::Red;
}
void setup()
{
arm.init();
needle.init();
}
//--------------------------------------------------------------
void testApp::recordArmXML (Hand & hand){
if (bRecordingThisFrame){
Arm arm = hand.arm();
if (arm.isValid()){
float armWidth = arm.width();
XML.setValue("AW", armWidth, lastTagNumber);
ofPoint palmPt = leap.getofPoint ( hand.palmPosition());
ofPoint wristPt = leap.getofPoint ( arm.wristPosition());
ofPoint elbowPt = leap.getofPoint ( arm.elbowPosition());
ofPoint palmNorm = leap.getofPoint ( hand.palmNormal());
int palmPtTagNum = XML.addTag("PM");
XML.setValue("PM:X", palmPt.x, palmPtTagNum);
XML.setValue("PM:Y", palmPt.y, palmPtTagNum);
XML.setValue("PM:Z", palmPt.z, palmPtTagNum);
int wristPtTagNum = XML.addTag("W");
XML.setValue("W:X", wristPt.x, wristPtTagNum);
XML.setValue("W:Y", wristPt.y, wristPtTagNum);
XML.setValue("W:Z", wristPt.z, wristPtTagNum);
int elbowPtTagNum = XML.addTag("E");
XML.setValue("E:X", elbowPt.x, elbowPtTagNum);
XML.setValue("E:Y", elbowPt.y, elbowPtTagNum);
XML.setValue("E:Z", elbowPt.z, elbowPtTagNum);
int palmNormTagNum = XML.addTag("PN");
XML.setValue("PN:X", palmNorm.x, palmNormTagNum);
XML.setValue("PN:Y", palmNorm.y, palmNormTagNum);
XML.setValue("PN:Z", palmNorm.z, palmNormTagNum);
//---------------
// Export the hand basis matrix
Leap::Matrix handMatrix = hand.basis();
ofPoint handBasisX = leap.getofPoint( handMatrix.xBasis);
ofPoint handBasisY = leap.getofPoint( handMatrix.yBasis);
ofPoint handBasisZ = leap.getofPoint( handMatrix.zBasis);
int handMatrixTagNum = XML.addTag("HM");
if( XML.pushTag("HM", handMatrixTagNum) ){
XML.setValue("XX", handBasisX.x, handMatrixTagNum);
XML.setValue("XY", handBasisX.y, handMatrixTagNum);
XML.setValue("XZ", handBasisX.z, handMatrixTagNum);
XML.setValue("YX", handBasisY.x, handMatrixTagNum);
XML.setValue("YY", handBasisY.y, handMatrixTagNum);
XML.setValue("YZ", handBasisY.z, handMatrixTagNum);
XML.setValue("ZX", handBasisZ.x, handMatrixTagNum);
XML.setValue("ZY", handBasisZ.y, handMatrixTagNum);
XML.setValue("ZZ", handBasisZ.z, handMatrixTagNum);
XML.popTag(); // pop HM (Hand Matrix)
}
//---------------
// Export the arm basis matrix
Leap::Matrix armMatrix = arm.basis();
ofPoint armBasisX = leap.getofPoint( armMatrix.xBasis);
ofPoint armBasisY = leap.getofPoint( armMatrix.yBasis);
ofPoint armBasisZ = leap.getofPoint( armMatrix.zBasis);
int armMatrixTagNum = XML.addTag("AM");
if( XML.pushTag("AM", armMatrixTagNum) ){
XML.setValue("XX", armBasisX.x, armMatrixTagNum);
XML.setValue("XY", armBasisX.y, armMatrixTagNum);
XML.setValue("XZ", armBasisX.z, armMatrixTagNum);
XML.setValue("YX", armBasisY.x, armMatrixTagNum);
XML.setValue("YY", armBasisY.y, armMatrixTagNum);
XML.setValue("YZ", armBasisY.z, armMatrixTagNum);
XML.setValue("ZX", armBasisZ.x, armMatrixTagNum);
XML.setValue("ZY", armBasisZ.y, armMatrixTagNum);
XML.setValue("ZZ", armBasisZ.z, armMatrixTagNum);
XML.popTag(); // pop AM (Arm Matrix)
}
}
}
}
int main(int argc, const char **argv){
Cartesian c;
string url(HOST);
const string user(USER);
const string pass(PASS);
const string database(DB);
string input_string, tmp, ui_input;
vector<string> strs;
string command_str;
int speed, fWait;
float distance;
Logger logger;
logger.setDebugLevel(Logger::INFO);
PID GripPID(GripServo::MAX, GripServo::MIN);
PID WrotPID(WrotServo::MAX, WrotServo::MIN);
PID WristPID(WristServo::MAX, WristServo::MIN);
PID ElbowPID(ElbowServo::MAX, ElbowServo::MIN);
PID ShoulderPID(ShoulderServo::MAX, ShoulderServo::MIN);
PID BasePID(BaseServo::MAX, BaseServo::MIN);
Arm *currArm = new Arm();
Arm *prevArm = new Arm();
ForbiddenZone forbiddenZone;
try{
//connect to database
sql::Connection *con;
sql::mysql::MySQL_Driver * driver;
driver = sql::mysql::get_mysql_driver_instance();
con = driver->connect(url, user, pass);
con->setSchema(database);
logger.i("Database is connected");
std::auto_ptr< sql::Statement > stmt(con->createStatement());
std::auto_ptr< sql::ResultSet > res;
while(1){
stmt->execute("CALL get_xyz(@rs)");
res.reset(stmt->executeQuery("select @rs as _val"));
while (res->next()) {
tmp = res->getString("_val");
strs = split(tmp, ' ');
}
if(tmp.length() == 0){
usleep(SLEEP_TIME);
continue;
}
logger.i("Data available...");
if(strs.size() != XYZ_INPUT_COUNT){
logger.d("Ignore this bad input:" + emerge(strs, ' '));
continue;
}
logger.i("processing input:" + emerge(strs, ' '));
command_str = strs[0];
if(command_str == CONTROLLING_DISABLE) {
logger.i("Controlling is not enabled!");
continue;
}
//set goal XYZ Position
c.setX(atof(strs[1].c_str()) + 50);
c.setY(atof(strs[2].c_str()));
c.setZ(atof(strs[3].c_str()) - BASE_HEIGHT*2 - 50);
if(c.getZ() < -50 || forbiddenZone.contains(c)){
logger.d("Ignore input...");
logger.d(c.toString());
continue;
}
currArm->setPosition(c);
currArm->setWristAngle(atof(strs[4].c_str()));
currArm->setGripLength(atof(strs[8].c_str()));
currArm->setWrotAngle(atof(strs[5].c_str()));
if(!currArm->convertXYZToRaw()){
logger.d("Input ignore...");
logger.d(currArm->toString());
continue;
}
ui_input = (currArm->getElbowXYZ()).toUICommandString();
ui_input += " ";
ui_input += (currArm->getWristXYZ()).toUICommandString();
ui_input += " ";
ui_input += c.toUICommandString();
ui_input += " ";
ui_input += to_string(currArm->getWrotAngle());
ui_input += " ";
ui_input += to_string(currArm->getGripLength());
logger.i(currArm->toString());
fWait = atoi(strs[7].c_str());
int change = getBiggestChange(currArm,prevArm);
speed = change / PROPER_SPEED;
prevArm->setToArm(currArm);
input_string = "CALL add_input('";
input_string += command_str;
input_string += Helper::formatString(to_string(currArm->getBaseRaw()),3);
input_string += Helper::formatString(to_string(currArm->getShoulderRaw()),3);
input_string += Helper::formatString(to_string(currArm->getElbowRaw()),3);
input_string += Helper::formatString(to_string(currArm->getWristRaw()),3);
input_string += Helper::formatString(to_string(currArm->getWrotRaw()),3);
input_string += Helper::formatString(to_string(currArm->getGripRaw()),3);
input_string += Helper::formatString(to_string(speed),4);
input_string += Helper::formatString(to_string(fWait),1);
input_string += "')";
logger.i("Success:" + input_string);
stmt->execute(input_string);
stmt->execute("CALL add_ui_input('"+ui_input+"')");
usleep(speed);
}
}catch(sql::SQLException &e){
logger.e(e.what());
return EXIT_FAILURE;
}
}
