fixed angle issue on mapping

IllinoisRoboticsInSpace · Jan 31, 2017 · fbbcb4f · fbbcb4f
1 parent 5d7511f
commit fbbcb4f
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 4 deletions.
diff --git a/algorithms/src/checkboard_navigation_module.cpp b/algorithms/src/checkboard_navigation_module.cpp
@@ -319,7 +319,6 @@ void* init_chessboard_navigation(void * stop_flag_ptr )
                         webcam_angle = 5;
                         long_turn = true;
                     }
-                    FILE* file;
                    // std::string s = std::to_string(((int)webcam_angle)%180);
                     //char buffer[s.length()];
                    // for (int i = 0; i < s.length(); i++)
@@ -331,7 +330,7 @@ void* init_chessboard_navigation(void * stop_flag_ptr )
                     fprintf(file, "%d\n", (int)webcam_angle);
                     printf("Angle sent:%d\n", (int)webcam_angle);
 
-                    double vehicle_angle = fmod2pi(webcam_angle*M_PI / 180. - atan2(y, x) - M_PI);
+                    double vehicle_angle = -fmod2pi(webcam_angle*M_PI / 180. - atan2(y, x) - M_PI)-M_PI/2.;
 
                     cout << "webcam nav " << "x(cm) " << x << " y(cm) " << y << " "
                          << "x(in) " << x/2.54 << " y(in) " << y/2.54 << endl

diff --git a/algorithms/src/debug_ip_server.cpp b/algorithms/src/debug_ip_server.cpp
@@ -351,6 +351,7 @@ void *connection_handler(void * pointer)
     data.append( (char *) data_buffer , length);
 
     *read_image=true;
+    std::cout<<"\033[0;31m"<<"IP SERVER image_read"<<"\033[0m\n";
 
     std::string output;
 

diff --git a/algorithms/src/kinect_obstacle_module.cpp b/algorithms/src/kinect_obstacle_module.cpp
@@ -259,8 +259,8 @@ void* thread_depth(void* arg)
             /**CONVERT POINT CLOUD INTO HEIGHT MAP**/
             for(int i = 0; i<pointCount; ++i)
             {
-                if(height(pointCloud[i].x, pointCloud[i].y) < pointCloud[i].z)
-                    height(pointCloud[i].x, pointCloud[i].y) = pointCloud[i].z;
+                if(height(-pointCloud[i].x, pointCloud[i].y) < pointCloud[i].z)
+                    height(-pointCloud[i].x, pointCloud[i].y) = pointCloud[i].z;
             }
             /**REMOVE STRANGE VALUES FROM MAP**/
 
@@ -338,6 +338,8 @@ void* thread_depth(void* arg)
                     }
                 }
                 tcpip_map_used = false;
+                std::cout<<"\033[0;31m"<<"IP SERVER image_ready"<<"\033[0m\n";
+
             }
 
         }