Odometry communication
Dependencies: PID QEI chair_BNO055 ros_lib_kinetic3
Dependents: wheelchaircontrolrealtime1
Fork of wheelchaircontrol by
Diff: wheelchair.cpp
- Revision:
- 25:987f3bcd769b
- Parent:
- 24:6c5b4b82f874
diff -r 6c5b4b82f874 -r 987f3bcd769b wheelchair.cpp --- 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() {