finished commenting on the .cpp file and changed the statistics library name
Dependencies: QEI2 PID IMU6050Ver11 Watchdog VL53L1X_Filter ros_lib_kinetic
Dependents: Version1-3 Version1-5
Diff: wheelchair.cpp
- Revision:
- 35:5a2fed4c2e9f
- Parent:
- 34:b89967adc86c
- Child:
- 37:e0e6d3fe06a2
diff -r b89967adc86c -r 5a2fed4c2e9f wheelchair.cpp --- a/wheelchair.cpp Tue Jul 02 17:49:18 2019 +0000 +++ b/wheelchair.cpp Tue Jul 09 21:18:01 2019 +0000 @@ -1,8 +1,14 @@ +/************************************************************************* +* Importing header into wheelchair.cpp * +**************************************************************************/ #include "wheelchair.h" +/************************************************************************* +* Defining global variables * +**************************************************************************/ bool manual_drive = false; // Variable changes between joystick and auto drive double encoder_distance; // Keeps distanse due to original position - + volatile double Setpoint, Output, Input, Input2; // Variables for PID volatile double pid_yaw, Distance, Setpoint2, Output2, encoder_distance2; // Variables for PID volatile double vIn, vOut, vDesired; // Variables for PID Velosity @@ -11,34 +17,40 @@ // int* ToFDataPointer1; // int* ToFDataPointer2; -int ledgeArrayLF[150]; -int ledgeArrayRF[150]; +int ledgeArrayLF[150]; +int ledgeArrayRF[150]; int* ToFDataPointer1 = ledgeArrayLF; int* ToFDataPointer2 = ledgeArrayRF; -statistics LFTStats(ToFDataPointer1, 149, 1); -statistics RFTStats(ToFDataPointer2, 149, 1); +Statistics LFTStats(ToFDataPointer1, 149, 1); +Statistics RFTStats(ToFDataPointer2, 149, 1); int k = 0; double dist_old, curr_pos; // Variables for odometry position double outlierToF[4]; - +/************************************************************************* +* Creating PID objects * +**************************************************************************/ 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 PID PIDVelosity(&vIn, &vOut, &vDesired, 5.5, .00, .002, P_ON_E, DIRECT); // Velosity PID Constructor PID PIDSlaveV(&vInS, &vOutS, &vDesiredS, 5.5, .00, .002, P_ON_E, DIRECT); // Slave Velosity PID Constructor PID PIDAngularV(&yIn, &yOut, &yDesired, 5.5, .00, .002, P_ON_E, DIRECT); // Angular Velosity PID Constructor - + -/* Thread measures current angular position */ -void Wheelchair::compass_thread() -{ - curr_yaw = imu->yaw(); - z_angular = curr_yaw; +/************************************************************************* +* Thread measures current angular position * +**************************************************************************/ +void Wheelchair::compass_thread() +{ + curr_yaw = imu->yaw(); + z_angular = curr_yaw; } -/* Thread measures velocity of wheels and distance traveled */ -void Wheelchair::velocity_thread() +/************************************************************************* +* Thread measures velocity of wheels and distance traveled * +**************************************************************************/ +void Wheelchair::velocity_thread() { curr_vel = wheel->getVelocity(); curr_velS = wheelS->getVelocity(); @@ -63,26 +75,28 @@ } } +/************************************************************************* +* ------------------------------------------------------------- * +**************************************************************************/ void Wheelchair::ToFSafe_thread() { int ToFV[12]; - for(int i = 0; i < 6; i++) // Reads from the ToF Sensors - { + for(int i = 0; i < 6; i++) { // Reads from the ToF Sensors ToFV[i] = (*(ToF+i))->readFromOneSensor(); //out->printf("%d ", ToFV[i]); - } - - //out->printf("\r\n"); - + } + + //out->printf("\r\n"); + k++; - + if (k == 150) { k = 0; } - + ledgeArrayLF[k] = (*(ToF+1))->readFromOneSensor(); ledgeArrayRF[k] = (*(ToF+4))->readFromOneSensor(); - + /*for(int i = 0; i < 100; i++) { out->printf("%d, ",ledgeArrayRF[i]); @@ -90,73 +104,72 @@ out->printf("\r\n");*/ outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev(); - outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev(); - + outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev(); + for(int i = 0; i < 4; i++) { // Reads from the ToF Sensors runningAverage[i] = ((runningAverage[i]*(4) + ToFV[(i*3)+1]) / 5); } - + int sensor1 = ToFV[0]; int sensor4 = ToFV[3]; - if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor1 < curr_vel*curr_vel*1000*1000 || - 2 * maxDecelerationSlow*sensor4 < curr_vel*curr_vel*1000*1000) && - (sensor1 < 1500 || sensor4 < 1500)) || - 550 > sensor1 || 550 > sensor4) - { - if(x->read() > def) - { + if(curr_vel < 1 &&((2 * maxDecelerationSlow*sensor1 < curr_vel*curr_vel*1000*1000 || + 2 * maxDecelerationSlow*sensor4 < curr_vel*curr_vel*1000*1000) && + (sensor1 < 1500 || sensor4 < 1500)) || + 550 > sensor1 || 550 > sensor4) { + if(x->read() > def) { x->write(def); forwardSafety = 1; // You cannot move forward } } - - else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 || - 2 * maxDecelerationFast*sensor4 < curr_vel*curr_vel*1000*1000) && - (sensor1 < 1500 || sensor4 < 1500)) || - 550 > sensor1 || 550 > sensor4) - { - if(x->read() > def) - { + + else if(curr_vel > 1 &&((2 * maxDecelerationFast*sensor1 < curr_vel*curr_vel*1000*1000 || + 2 * maxDecelerationFast*sensor4 < curr_vel*curr_vel*1000*1000) && + (sensor1 < 1500 || sensor4 < 1500)) || + 550 > sensor1 || 550 > sensor4) { + if(x->read() > def) { x->write(def); forwardSafety = 1; // You cannot move forward } } - + else if ((runningAverage[0] > outlierToF[0]) || (runningAverage[1] > outlierToF[1])) { forwardSafety = 1; out->printf("I'M STOPPING BECAUSE OF A LEDGE\r\n"); - } - + } + else forwardSafety = 0; - + /*-------Side Tof begin----------*/ - + int sensor3 = ToFV[2]; //front left int sensor6 = ToFV[5]; //front right int sensor9 = ToFV[8]; //back int sensor12 = ToFV[11]; //back - + // float currAngularVelocity = IMU DATA; //Current angular velocity from IMU // float angle; //from IMU YAW, convert to cm // float arcLength = angle * WHEELCHAIR_RADIUS; //S = r*Ө - // Clear the front side first, else continue going straight or can't turn - // After clearing the front sideand movinf forward, check if can clear - // the back when turning - + /************************************************************************** + * Clear the front side first, else continue going straight or can't turn * + * After clearing the front sideand movinf forward, check if can clear * + * the back when turning * + **************************************************************************/ // Check if can clear side - + // When either sensors too close to the wall, can't turn if((sensor3 <= MIN_WALL_LENGTH) || (sensor6 <= MIN_WALL_LENGTH) || - (sensor12 <= MIN_WALL_LENGTH)) { + (sensor12 <= MIN_WALL_LENGTH)) { sideSafety = 1; } - // Check whether safe to keep turnin, user control <-- make sure - // currAngularVelocity is in correct units. Know the exact moment you can - // stop the chair going at a certain speed before its too late - // else if((currAngularVelocity * currAngularVelocity > 2 * + /************************************************************************* + * Check whether safe to keep turnin, user control <-- make sure * + * currAngularVelocity is in correct units. Know the exact moment you can * + * stop the chair going at a certain speed before its too late * + **************************************************************************/ + // else if((currAngularVelocity * currAngularVelocity > 2 * // MAX_ANGULAR_DECELERATION * angle) && (sensor3 <= angle || // sensor6 <= angle)) { // sideSafety = 1; //Not safe to turn @@ -165,22 +178,24 @@ else { sideSafety = 0; } - + /*-------Side Tof end -----------*/ } -/* Constructor for Wheelchair class */ -Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS, -VL53L1X** ToFT) +/************************************************************************* +* Constructor for Wheelchair class * +**************************************************************************/ +Wheelchair::Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* qei, QEI* qeiS, + VL53L1X** ToFT) { x_position = 0; y_position = 0; forwardSafety = 0; - + /* Initializes X and Y variables to Pins */ - x = new PwmOut(xPin); + x = new PwmOut(xPin); y = new PwmOut(yPin); - + /* Initializes IMU Library */ out = pc; // "out" is called for serial monitor out->printf("on\r\n"); @@ -188,76 +203,81 @@ Wheelchair::stop(); // Wheelchair is initially stationary imu->setup(); // turns on the IMU wheelS = qeiS; // "wheel" is called for encoder - wheel = qei; + wheel = qei; ToF = ToFT; // passes pointer with addresses of ToF sensors - - for(int i = 0; i < 12; i++) // initializes the ToF Sensors - { + + for(int i = 0; i < 12; i++) { // initializes the ToF Sensors (*(ToF+i))->initReading(0x31+((0x02)*i), 50000); } - + out->printf("wheelchair setup done \r\n"); // Make sure it initialized; prints in serial monitor ti = time; - for(int i = 0; i < 10; i++) - { - (*(ToF+1))->readFromOneSensor(); - (*(ToF+1))->readFromOneSensor(); + for(int i = 0; i < 10; i++) { + (*(ToF+1))->readFromOneSensor(); + (*(ToF+1))->readFromOneSensor(); } - for(int i = 0; i < 150; i++) - { + for(int i = 0; i < 150; i++) { ledgeArrayLF[i] = (*(ToF+1))->readFromOneSensor(); ledgeArrayRF[i] = (*(ToF+4))->readFromOneSensor(); } - + outlierToF[0] = LFTStats.mean() + 2*LFTStats.stdev(); outlierToF[1] = RFTStats.mean() + 2*RFTStats.stdev(); myPID.SetMode(AUTOMATIC); // PID mode: Automatic } -/* Move wheelchair with joystick on manual mode */ -void Wheelchair::move(float x_coor, float y_coor) +/************************************************************************* +* Move wheelchair with joystick on manual mode * +**************************************************************************/ +void Wheelchair::move(float x_coor, float y_coor) { - /* Scales one joystick measurement to the chair's joystick measurement */ + /* Scales one joystick measurement to the chair's joystick measurement */ float scaled_x = ((x_coor * 1.6f) + 1.7f)/3.3f; float scaled_y = (3.3f - (y_coor * 1.6f))/3.3f; - /* Sends the scaled joystic values to the chair */ - x->write(scaled_x); + /* Sends the scaled joystic values to the chair */ + x->write(scaled_x); y->write(scaled_y); } - -/* Automatic mode: move forward and update x,y coordinate sent to chair */ -void Wheelchair::forward() + + +/************************************************************************* +* Automatic mode: move forward and update x,y coordinate sent to chair * +**************************************************************************/ +void Wheelchair::forward() { //printf("current velosity; %f, curr vel S %f\r\n", curr_vel, curr_velS); - if(forwardSafety == 0) - { - x->write(high); - y->write(def+offset); + if(forwardSafety == 0) { + x->write(high); + y->write(def+offset); } out->printf("%f, %f\r\n", curr_pos, wheelS->getDistance(53.975)); } - -/* Automatic mode: move in reverse and update x,y coordinate sent to chair */ -void Wheelchair::backward() + +/************************************************************************* +* Automatic mode: move in reverse and update x,y coordinate sent to chair* +**************************************************************************/ +void Wheelchair::backward() { x->write(low); y->write(def); } - -/* Automatic mode: move right and update x,y coordinate sent to chair */ -void Wheelchair::right() +/************************************************************************* +* Automatic mode: move right and update x,y coordinate sent to chair * +**************************************************************************/ +void Wheelchair::right() { //if safe to move, from ToFSafety if(sideSafety == 0) { - x->write(def); - y->write(low); + x->write(def); + y->write(low); } } - - /* Automatic mode: move left and update x,y coordinate sent to chair */ -void Wheelchair::left() +/************************************************************************* +* Automatic mode: move left and update x,y coordinate sent to chair * +**************************************************************************/ +void Wheelchair::left() { //if safe to move, from ToFSafety if(sideSafety == 0) { @@ -265,239 +285,232 @@ y->write(high); } } - -/* Stop the wheelchair */ -void Wheelchair::stop() +/************************************************************************* +* Stop the wheelchair * +**************************************************************************/ +void Wheelchair::stop() { x->write(def); y->write(def); } - -/* Counter-clockwise is - - * Clockwise is + - * Range of deg: 0 to 360 - * This constructor takes in an angle from user and adjusts for turning right - */ +/************************************************************************* + * Counter-clockwise is ( - ) * + * Clockwise is ( + ) * + * Range of deg: 0 to 360 * + * This method takes in an angle from user and adjusts for turning right * +**************************************************************************/ void Wheelchair::pid_right(int deg) { bool overturn = false; // Boolean if angle over 360˚ - - out->printf("pid right\r\r\n"); + + out->printf("pid right\r\r\n"); x->write(def); // Update x sent to chair to be stationary Setpoint = curr_yaw + deg; // Relative angle we want to turn pid_yaw = curr_yaw; // Sets pid_yaw to angle input from user - + /* Turns on overturn boolean if setpoint over 360˚ */ - if(Setpoint > 360) - { + if(Setpoint > 360) { overturn = true; } - + myPID.SetTunings(5.5,0, 0.0035); // Sets the constants for P and D myPID.SetOutputLimits(0, def-low-.15); // Limit is set to the differnce between def and low myPID.SetControllerDirection(DIRECT); // PID mode: Direct - - /* PID stops when approaching a litte less than desired angle */ - while(pid_yaw < Setpoint - 3) - { + + /* PID stops when approaching a litte less than desired angle */ + while(pid_yaw < Setpoint - 3) { /* PID is set to correct angle range if angle greater than 360˚*/ - if(overturn && curr_yaw < Setpoint-deg-1) - { - pid_yaw = curr_yaw + 360; - } - else - { + if(overturn && curr_yaw < Setpoint-deg-1) { + pid_yaw = curr_yaw + 360; + } else { pid_yaw = curr_yaw; } - + myPID.Compute(); // Does PID calculations double tempor = -Output+def; // Temporary value with the voltage output - y->write(tempor); // Update y sent to chair - + y->write(tempor); // Update y sent to chair + /* Prints to serial monitor the current angle and setpoint */ - out->printf("curr_yaw %f\r\r\n", curr_yaw); + out->printf("curr_yaw %f\r\r\n", curr_yaw); out->printf("Setpoint = %f \r\n", Setpoint); - + wait(.05); // Small delay (milliseconds) } - + /* Saftey stop for wheelchair */ - Wheelchair::stop(); + Wheelchair::stop(); out->printf("done \r\n"); } - -/* Counter-clockwise is - - * Clockwise is + - * Range of deg: 0 to 360 - * This constructor takes in an angle from user and adjusts for turning left - */ -void Wheelchair::pid_left(int deg) +/************************************************************************* +* Counter-clockwise is ( - ) * +* Clockwise is ( + ) * +* Range of deg: 0 to 360 * +* This method takes in an angle from user and adjusts for turning left * +**************************************************************************/ +void Wheelchair::pid_left(int deg) { bool overturn = false; //Boolean if angle under 0˚ - - out->printf("pid Left\r\r\n"); + + out->printf("pid Left\r\r\n"); x->write(def); // Update x sent to chair to be stationary Setpoint = curr_yaw - deg; // Relative angle we want to turn pid_yaw = curr_yaw; // Sets pid_yaw to angle input from user - + /* Turns on overturn boolean if setpoint less than 0˚ */ - if(Setpoint < 0) - { + if(Setpoint < 0) { overturn = true; } - + myPID.SetTunings(5,0, 0.004); // Sets the constants for P and D myPID.SetOutputLimits(0,high-def-.12); // Limit is set to the differnce between def and low myPID.SetControllerDirection(REVERSE); // PID mode: Reverse - + /* PID stops when approaching a litte more than desired angle */ - while(pid_yaw > Setpoint+3) - { - /* PID is set to correct angle range if angle less than 0˚ */ - if(overturn && curr_yaw > Setpoint+deg+1) - { - pid_yaw = curr_yaw - 360; - } - else - { - pid_yaw = curr_yaw; - } - + while(pid_yaw > Setpoint+3) { + /* PID is set to correct angle range if angle less than 0˚ */ + if(overturn && curr_yaw > Setpoint+deg+1) { + pid_yaw = curr_yaw - 360; + } else { + pid_yaw = curr_yaw; + } + myPID.Compute(); // Does PID calculations double tempor = Output+def; // Temporary value with the voltage output y->write(tempor); // Update y sent to chair - + /* Prints to serial monitor the current angle and setpoint */ out->printf("curr_yaw %f\r\n", curr_yaw); out->printf("Setpoint = %f \r\n", Setpoint); - + wait(.05); // Small delay (milliseconds) } - - /* Saftey stop for wheelchair */ - Wheelchair::stop(); + + /* Saftey stop for wheelchair */ + Wheelchair::stop(); out->printf("done \r\n"); } - -/* This constructor determines whether to turn left or right */ -void Wheelchair::pid_turn(int deg) -{ - - /* Sets angle to coterminal angle for left turn if deg > 180 - * Sets angle to coterminal angle for right turn if deg < -180 - */ - if(deg > 180) - { + +/************************************************************************* +* This method determines whether to turn left or right * +**************************************************************************/ +void Wheelchair::pid_turn(int deg) +{ + + /***************************************************************** + * Sets angle to coterminal angle for left turn if deg > 180 * + * Sets angle to coterminal angle for right turn if deg < -180 * + *****************************************************************/ + if(deg > 180) { deg -= 360; + } else if(deg < -180) { + deg +=360; } - else if(deg < -180) - { - deg +=360; - } - + /* Makes sure angle inputted to function is positive */ int turnAmt = abs(deg); - + /* Calls PID_right if deg > 0, else calls PID_left if deg < 0 */ - if(deg >= 0) - { - Wheelchair::pid_right(turnAmt); - } - else - { - Wheelchair::pid_left(turnAmt); + if(deg >= 0) { + Wheelchair::pid_right(turnAmt); + } else { + Wheelchair::pid_left(turnAmt); } } -/* This constructor takes in distance to travel and adjust to move forward */ +/************************************************************************* +* This method takes in distance to travel and adjust to move forward * +**************************************************************************/ void Wheelchair::pid_forward(double mm) { mm -= 20; // Makes sure distance does not overshoot Input = 0; // Initializes input to zero: Test latter w/o wheel->reset(); // Resets encoders so that they start at 0 - + out->printf("pid foward\r\n"); - + double tempor; // Initializes Temporary variable for x input Setpoint = mm; // Initializes the setpoint to desired value - + myPIDDistance.SetTunings(5.5,0, 0.0015); // Sets constants for P and D - myPIDDistance.SetOutputLimits(0,high-def-.15); // Limit set to difference between high and def + myPIDDistance.SetOutputLimits(0,high-def-.15); // Limit set to difference between high and def myPIDDistance.SetControllerDirection(DIRECT); // PID mode: Direct - + y->write(def+offset); // Update y to make chair stationary - + /* Chair stops moving when Setpoint is reached */ - while(Input < Setpoint){ - - if(out->readable()) // Emergency Break - { + while(Input < Setpoint) { + + if(out->readable()) { // Emergency Break break; } Input = wheel->getDistance(53.975); // Gets distance from Encoder into PID wait(.05); // Slight Delay: *****Test without myPIDDistance.Compute(); // Compute distance traveled by chair - - tempor = Output + def; // Temporary output variable + + tempor = Output + def; // Temporary output variable x->write(tempor); // Update x sent to chair /* Prints to serial monitor the distance traveled by chair */ out->printf("distance %f\r\n", Input); - } - -} + } + +} -/* This constructor returns the relative angular position of chair */ +/************************************************************************* +* This method returns the relative angular position of chair * +**************************************************************************/ double Wheelchair::getTwistZ() { return imu->gyro_z(); -} +} -/* This constructor computes the relative angle for Twist message in ROS */ +/************************************************************************* +* This method computes the relative angle for Twist message in ROS * +**************************************************************************/ void Wheelchair::pid_twistA() { /* Initialize variables for angle and update x,y sent to chair */ char c; double temporA = def; - y->write(def); - x->write(def); - + y->write(def); + x->write(def); + PIDAngularV.SetTunings(.00015,0, 0.00); // Sets the constants for P and D PIDAngularV.SetOutputLimits(-.1, .1); // Limit set to be in range specified PIDAngularV.SetControllerDirection(DIRECT); // PID mode: Direct - + /* Computes angular position of wheelchair while turning */ - while(1) - { + while(1) { yDesired = angularV; - + /* Update and set all variable so that the chair is stationary * if the desired angle is zero */ - if(yDesired == 0) - { + if(yDesired == 0) { x->write(def); y->write(def); yDesired = 0; return; } - + /* Continuously updates with current angle measured by IMU */ - yIn = imu->gyro_z(); + yIn = imu->gyro_z(); PIDAngularV.Compute(); temporA += yOut; // Temporary value with the voltage output y->write(temporA); // Update y sent to chair - + //out->printf("temporA: %f, yDesired %f, angle: %f\r\n", temporA, yDesired, imu->gyro_z()); wait(.05); // Small delay (milliseconds) } - -} + +} -/* This constructor computes the relative velocity for Twist message in ROS */ +/************************************************************************* +* This method computes the relative velocity for Twist message in ROS * +**************************************************************************/ void Wheelchair::pid_twistV() { /* Initializes variables as default */ @@ -508,30 +521,28 @@ y->write(def); wheel->reset(); // Resets the encoders /* Sets the constants for P and D */ - PIDVelosity.SetTunings(.0005,0, 0.00); - PIDSlaveV.SetTunings(.005,0.000001, 0.000001); - + PIDVelosity.SetTunings(.0005,0, 0.00); + PIDSlaveV.SetTunings(.005,0.000001, 0.000001); + /* Limits to the range specified */ - PIDVelosity.SetOutputLimits(-.005, .005); - PIDSlaveV.SetOutputLimits(-.002, .002); - + PIDVelosity.SetOutputLimits(-.005, .005); + PIDSlaveV.SetOutputLimits(-.002, .002); + /* PID mode: Direct */ - PIDVelosity.SetControllerDirection(DIRECT); - PIDSlaveV.SetControllerDirection(DIRECT); - - while(1) - { + PIDVelosity.SetControllerDirection(DIRECT); + PIDSlaveV.SetControllerDirection(DIRECT); + + while(1) { linearV = .7; test1 = linearV*100; vel = curr_vel; vDesired = linearV*100; if(out->readable()) return; - /* Update and set all variable so that the chair is stationary - * if the velocity is zero - */ - if(linearV == 0) - { + /* Update and set all variable so that the chair is stationary + * if the velocity is zero + */ + if(linearV == 0) { x->write(def); y->write(def); @@ -540,21 +551,17 @@ dist_old = 0; return; } - - if(vDesired >= 0) - { + + if(vDesired >= 0) { PIDVelosity.SetTunings(.000004,0, 0.00); // Sets the constants for P and D PIDVelosity.SetOutputLimits(-.002, .002); // Limits to the range specified - } - else - { + } else { PIDVelosity.SetTunings(.000015,0, 0.00); // Sets the constants for P and D PIDVelosity.SetOutputLimits(-.0005, .0005); // Limits to range specified - } - + } + /* Sets maximum value of variable to 1 */ - if(temporV >= 1.5) - { + if(temporV >= 1.5) { temporV = 1.5; } /* Scales and makes some adjustments to velocity */ @@ -562,18 +569,15 @@ vInS = curr_vel-curr_velS; PIDVelosity.Compute(); PIDSlaveV.Compute(); - if(forwardSafety == 0) - { - temporV += vOut; - temporS += vOutS; - - /* Updates x,y sent to Wheelchair and for Odometry message in ROS */ - x->write(temporV); - test2 = temporV; - y->write(temporS); - } - else - { + if(forwardSafety == 0) { + temporV += vOut; + temporS += vOutS; + + /* Updates x,y sent to Wheelchair and for Odometry message in ROS */ + x->write(temporV); + test2 = temporV; + y->write(temporS); + } else { x->write(def); y->write(def); } @@ -583,43 +587,56 @@ } } -/* This constructor calculates the relative position of the chair everytime the encoders reset - * by setting its old position as the origin to calculate the new position - */ +/************************************************************************* +* This method calculates the relative position of the chair everytime the* +* encoders reset by setting its old position as the origin to calculate * +* the new position * +**************************************************************************/ void Wheelchair::odomMsg() { double dist_new = curr_pos; double dist = dist_new-dist_old; double temp_x = dist*sin(z_angular*3.14159/180); double temp_y = dist*cos(z_angular*3.14159/180); - + x_position += temp_x; y_position += temp_y; - + dist_old = dist_new; - } +} -/* This constructor prints the Odometry message to the serial monitor */ - void Wheelchair::showOdom() - { - out->printf("x %f, y %f, angle %f", x_position, y_position, z_angular); - } +/************************************************************************** +* This method prints the Odometry message to the serial monitor * +***************************************************************************/ +void Wheelchair::showOdom() +{ + out->printf("x %f, y %f, angle %f", x_position, y_position, z_angular); +} -/* This constructor returns the approximate distance based on the wheel diameter */ +/************************************************************************** +* This method returns the approximate distance based on the wheel diameter* +***************************************************************************/ float Wheelchair::getDistance() { return wheel->getDistance(Diameter); } -/* This constructor resets the wheel encoders */ +/************************************************************************** +* This method resets the Encoder's * +***************************************************************************/ void Wheelchair::resetDistance() { wheel->reset(); } +/*---------------------------------------------------------------------------------------------------*/ +/*---------------------------------------------------------------------------------------------------*/ +/*---------------------------------------------------------------------------------------------------*/ +/*---------------------------------------------------------------------------------------------------*/ +/*---------------------------------------------------------------------------------------------------*/ -/*Predetermined paths For Demmo*/ -void Wheelchair::desk() +/*Predetermined paths For Demmo*/ +void Wheelchair::desk() { Wheelchair::pid_forward(5461); Wheelchair::pid_right(87); @@ -627,8 +644,8 @@ Wheelchair::pid_right(87); Wheelchair::pid_forward(3658); } - -void Wheelchair::kitchen() + +void Wheelchair::kitchen() { Wheelchair::pid_forward(5461); Wheelchair::pid_right(87); @@ -636,7 +653,7 @@ Wheelchair::pid_left(90); Wheelchair::pid_forward(305); } - + void Wheelchair::desk_to_kitchen() { Wheelchair::pid_right(180);