Jesi Miranda
Working file for communication with ROS
Dependencies: QEI2 PID VL53L1X_Filter
Diff: wheelchair.cpp
- Revision:
- 31:9283c8cd5362
- Parent:
- 29:5714715ab1f5
--- a/wheelchair.cpp Fri Jul 05 19:41:35 2019 +0000
+++ b/wheelchair.cpp Tue Aug 20 22:24:17 2019 +0000
@@ -1,34 +1,47 @@
-
+/*************************************************************************
+* 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
+volatile double vIn, vOut, vDesired; // Variables for PID Velocity
volatile double vInS, vOutS, vDesiredS; // Variables for PID Slave Wheel
volatile double yIn, yOut, yDesired; // Variables for PID turn velosity
-double dist_old, curr_pos, curr_posS; // Variables for odometry position
+double dist_old, curr_pos, curr_posS; // Variables for odometry position
-
+/*************************************************************************
+* 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
+
+
//e_button thread missing
-/* Thread measures current angular position */
+/*************************************************************************
+* Thread measures current angular position *
+**************************************************************************/
void Wheelchair::compass_thread()
{
//curr_yaw = imu->yaw(); //uncomment when imu is implemented
z_angular = curr_yaw;
}
-/* Thread measures velocity of wheels and distance traveled */
+/*************************************************************************
+* Thread measures velocity of wheels and distance traveled *
+**************************************************************************/
void Wheelchair::velocity_thread()
{
curr_vel = wheel->getVelocity();
@@ -36,7 +49,9 @@
curr_pos = wheel->getDistance(53.975);
curr_posS = wheelS->getDistance(53.975); //Sending slave encoder to ROS
}
-
+/*************************************************************************
+* ------------------------------------------------------------- *
+**************************************************************************/
void Wheelchair::assistSafe_thread()
{
//int ToFV[12];
@@ -79,9 +94,11 @@
}
-/* 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;
@@ -98,7 +115,7 @@
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);
}
@@ -109,7 +126,9 @@
myPID.SetMode(AUTOMATIC); // PID mode: Automatic
}
-/* Move wheelchair with joystick on manual mode */
+/*************************************************************************
+* 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 */
@@ -121,7 +140,9 @@
y->write(scaled_y);
}
-/* Automatic mode: move forward and update x,y coordinate sent to chair */
+/*************************************************************************
+* Automatic mode: move forward and update x,y coordinate sent to chair *
+**************************************************************************/
void Wheelchair::forward()
{
if(forwardSafety == 0) {
@@ -130,39 +151,50 @@
}
}
-/* Automatic mode: move in reverse and update x,y coordinate sent to chair */
+/*************************************************************************
+* 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 */
+/*************************************************************************
+* Automatic mode: move right and update x,y coordinate sent to chair *
+**************************************************************************/
void Wheelchair::right()
{
x->write(def);
y->write(low);
}
-/* Automatic mode: move left and update x,y coordinate sent to chair */
+/*************************************************************************
+* Automatic mode: move left and update x,y coordinate sent to chair *
+**************************************************************************/
void Wheelchair::left()
{
x->write(def);
y->write(high);
}
-/* Stop the wheelchair */
+/*************************************************************************
+* 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˚
@@ -206,11 +238,12 @@
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
- */
+/*************************************************************************
+ * 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˚
@@ -255,13 +288,16 @@
}
-/* This constructor determines whether to turn left or right */
+/*************************************************************************
+* 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
- */
+ /*****************************************************************
+ * 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) {
@@ -280,7 +316,9 @@
}
-/* 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
@@ -318,13 +356,17 @@
}
-/* 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 */
@@ -363,7 +405,9 @@
}
-/* 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 */
@@ -440,9 +484,11 @@
}
}
-/* 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()
{
dist_new = curr_pos;
@@ -457,25 +503,32 @@
dist_old = dist_new;
}
-/* This constructor prints the Odometry message to the serial monitor */
+/**************************************************************************
+* 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 encoder's */
+/**************************************************************************
+* This method resets the Encoder's *
+***************************************************************************/
void Wheelchair::resetDistance()
{
wheel->reset();
}
+
/*Predetermined paths For Demmo*/
void Wheelchair::desk()
{
@@ -499,4 +552,7 @@
{
Wheelchair::pid_right(180);
Wheelchair::pid_forward(3700);
-}
\ No newline at end of file
+}
+
+
+