Revision 28:889fc0e5f8c4, committed 2018-11-03

Comitter:

jvfausto

Date:

Sat Nov 03 20:33:55 2018 +0000

Parent:

27:17783779a954

Child:

29:a6a1ccf14c63

Commit message:

With twist and odom message

Changed in this revision

--- a/wheelchair.cpp	Fri Nov 02 21:52:29 2018 +0000
+++ b/wheelchair.cpp	Sat Nov 03 20:33:55 2018 +0000
@@ -1,7 +1,7 @@
 #include "wheelchair.h"
  
 bool manual_drive = false;                                                             // Variable Changes between joystick and auto drive
-double curr_yaw, curr_vel, curr_velS;                                                             // Variable that contains current relative angle
+double curr_yaw, curr_velS;                                                             // Variable that contains current relative angle
 double encoder_distance;                                                               // Keeps distanse due to original position
  
 volatile double Setpoint, Output, Input, Input2;                                       // Variables for PID
@@ -29,21 +29,6 @@
      curr_pos = wheel->getDistance(53.975);
      }
 void Wheelchair::rosCom_thread(){
-    //set and update array
-    odom_vector[0] = curr_pos;//x_position;
-    odom_vector[1] = y_position;
-    
-    //set the position with updated values
-/*    odom.pose.pose.position.x = odom_vector[0];
-    odom.pose.pose.position.y = odom_vector[1];
-    odom.pose.pose.position.z = 0;
-    //set the orientation
-    odom.pose.pose.orientation.z = z_angular;
-    odom.pose.pose.orientation.x = 0;
-    odom.pose.pose.orientation.y = 0;
-    chatter.publish(&odom);
-    nh.spinOnce();
-    */
 }
 Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS)   // Function Constructor for Wheelchair class
 {
@@ -52,9 +37,6 @@
     //Initializes X and Y variables to Pins
     x = new PwmOut(xPin);                                                               
     y = new PwmOut(yPin);
-    odom_vector[0] = 0;
-    odom_vector[1] = 0;
-    odom_vector[2] = 0;
     // Initializes IMU Library
     imu = new chair_BNO055(pc, time);
     Wheelchair::stop();                                                                 // Wheelchair is not moving when initializing
@@ -354,6 +336,11 @@
 void Wheelchair::resetDistance(){
     wheel->reset();
     }
+double Wheelchair::getTwistZ()
+{
+    return imu->gyro_z();
+
+}  
 /*Predetermined paths For Demmo*/    
 void Wheelchair::desk() {
     Wheelchair::pid_forward(5461);

--- a/wheelchair.h	Fri Nov 02 21:52:29 2018 +0000
+++ b/wheelchair.h	Sat Nov 03 20:33:55 2018 +0000
@@ -93,10 +93,15 @@
     void kitchen();
     void desk_to_kitchen();
     
+
+    double getTwistZ();
+
+    
     double x_position;
     double y_position;
     double z_angular;   
-    double odom_vector[3]; 
+    double curr_vel;
+    double z_twistA;
 private:
     /* Pointers for the joystick speed*/
     PwmOut* x;