Diff: wheelchair.cpp

Revision:

25:987f3bcd769b

Parent:

24:6c5b4b82f874

--- a/wheelchair.cpp	Fri Nov 02 02:15:30 2018 +0000
+++ b/wheelchair.cpp	Fri Nov 02 21:48:37 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
@@ -10,10 +10,7 @@
 volatile double vInS, vOutS, vDesiredS;
 volatile double yIn, yOut, yDesired;
 
-double z_angular, dist_old, curr_pos;
-double x_position = 0;
-double y_position = 0;
-
+double dist_old, curr_pos;
 
  
 PID myPID(&pid_yaw, &Output, &Setpoint, 5.5, .00, 0.0036, P_ON_E, DIRECT);             // Angle PID object constructor
@@ -31,25 +28,10 @@
      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
 {
+    x_position = 0;
+    y_position = 0;
     //Initializes X and Y variables to Pins
     x = new PwmOut(xPin);                                                               
     y = new PwmOut(yPin);
@@ -230,9 +212,10 @@
         }
     
 }   
-void Wheelchair::pid_reverse(double mm)
+double Wheelchair::getTwistZ()
 {
- 
+    return imu->gyro_z();
+
 }   
 void Wheelchair::pid_twistA()
 {