Diff: wheelchair.cpp

Revision:

24:6c5b4b82f874

Parent:

23:58ec657a44f2

Child:

25:987f3bcd769b

Child:

27:17783779a954

--- a/wheelchair.cpp	Sat Oct 27 16:47:50 2018 +0000
+++ b/wheelchair.cpp	Fri Nov 02 02:15:30 2018 +0000
@@ -13,6 +13,8 @@
 double z_angular, dist_old, curr_pos;
 double x_position = 0;
 double y_position = 0;
+
+
  
 PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT);             // Angle PID object constructor
 PID myPIDDistance(&Input, &Output, &Setpoint, 5.5, .00, 0.002, P_ON_E, DIRECT);       // Distance PID object constructor
@@ -29,13 +31,31 @@
      curr_velS = wheelS->getVelosity();
      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
 {
     //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
@@ -264,7 +284,7 @@
     y->write(def);
     wheel->reset();
     PIDVelosity.SetTunings(.00005,0, 0.00);                                                    // Sets the constants for P and D
-    PIDSlaveV.SetTunings(.007,0.000001, 0.000001);                                                    // Sets the constants for P and D
+    PIDSlaveV.SetTunings(.004,0.000001, 0.000001);                                                    // Sets the constants for P and D
     PIDVelosity.SetOutputLimits(-.005, .005);                                              // Limits to the differnce between def and low
     PIDSlaveV.SetOutputLimits(-.002, .002);                                              // Limits to the differnce between def and low
     PIDVelosity.SetControllerDirection(DIRECT);