Odometry communication
Dependencies: PID QEI chair_BNO055 ros_lib_kinetic3
Dependents: wheelchaircontrolrealtime1
Fork of wheelchaircontrol by
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);