Isabella Gomez Torres / Mbed OS TEST_SideToF

Where we will test the side ToF sensors

Dependencies:   QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic

wheelchairControlSideTof/wheelchair.h@19:5376c858e574, 2019-07-02 (annotated)

Committer:
isagmz
Date:
Tue Jul 02 17:45:37 2019 +0000
Revision:
19:5376c858e574
Parent:
18:a10277a63b53
Child:
20:3c1b58654e67
to share

Who changed what in which revision?

UserRevisionLine numberNew contents of line
isagmz 18:a10277a63b53 1 #ifndef wheelchair
isagmz 18:a10277a63b53 2 #define wheelchair
isagmz 18:a10277a63b53 3 /*************************************************************************
isagmz 18:a10277a63b53 4 * Importing libraries into wheelchair.h *
isagmz 18:a10277a63b53 5 **************************************************************************/
isagmz 18:a10277a63b53 6 #include "chair_BNO055.h"
isagmz 18:a10277a63b53 7 #include "PID.h"
isagmz 18:a10277a63b53 8 #include "QEI.h"
isagmz 18:a10277a63b53 9 #include "VL53L1X.h"
isagmz 18:a10277a63b53 10 #include "statistics.h"
isagmz 18:a10277a63b53 11 #include "Watchdog.h"
isagmz 18:a10277a63b53 12
isagmz 18:a10277a63b53 13 #include <ros.h>
isagmz 18:a10277a63b53 14 #include <geometry_msgs/Twist.h>
isagmz 18:a10277a63b53 15 #include <nav_msgs/Odometry.h>
isagmz 18:a10277a63b53 16
isagmz 18:a10277a63b53 17 /*************************************************************************
isagmz 18:a10277a63b53 18 * Joystick has analog out of 200-700, scale values between 1.3 and 3.3 *
isagmz 18:a10277a63b53 19 * Here are some global constants for joystick *
isagmz 18:a10277a63b53 20 **************************************************************************/
isagmz 18:a10277a63b53 21 #define def (2.5f/3.3f) //Default axis on joystick to stay neutral; used on x and y axis
isagmz 18:a10277a63b53 22 #define high 3.3f/3.3f //High power on joystick; used on x and y axis
isagmz 18:a10277a63b53 23 #define low (1.7f/3.3f) //Low power on joystick; used on x and y axis
isagmz 18:a10277a63b53 24 #define offset .03f //Joystick adjustment to be able to go straight. Chair dependent on manufactoring precision
isagmz 18:a10277a63b53 25 #define process .1 //Defines default time delay in seconds
isagmz 18:a10277a63b53 26 /*************************************************************************
isagmz 18:a10277a63b53 27 *Pin plug-in for Nucleo-L432KC/Compatible with Nucleo-400 series (F767ZI)*
isagmz 18:a10277a63b53 28 **************************************************************************/
isagmz 18:a10277a63b53 29 #define xDir PA_6 //* PWM Pins */
isagmz 18:a10277a63b53 30 #define yDir PA_5
isagmz 18:a10277a63b53 31 #define Encoder1 D7 //*Digital In Pull Up Pin */
isagmz 18:a10277a63b53 32 #define Encoder2 D8
isagmz 18:a10277a63b53 33 #define Diameter 31.75 //Diameter of encoder wheel
isagmz 18:a10277a63b53 34 #define maxDecelerationSlow 120
isagmz 18:a10277a63b53 35 #define maxDecelerationFast 30
isagmz 18:a10277a63b53 36 #define ToFSensorNum 12
isagmz 18:a10277a63b53 37
isagmz 18:a10277a63b53 38 /*************************************************************************
isagmz 18:a10277a63b53 39 *IMU definitions for turning wheelchair
isagmz 18:a10277a63b53 40 **************************************************************************/
isagmz 18:a10277a63b53 41 #define WHEELCHAIR_RADIUS 56 //distance from IMU to edge of wheelchair(cm)
isagmz 18:a10277a63b53 42 #define MAX_ANGULAR_DECELERATION 60 //found through testing, max
isagmz 18:a10277a63b53 43 //acceleration at which chair can
isagmz 18:a10277a63b53 44 //stop while turning. In degree per sec
isagmz 19:5376c858e574 45 #define MIN_WALL_LENGTH 100 // minimum distance from wall to ToF (mm)
isagmz 18:a10277a63b53 46 /*************************************************************************
isagmz 18:a10277a63b53 47 * *
isagmz 18:a10277a63b53 48 * Wheelchair class *
isagmz 18:a10277a63b53 49 * Used for controlling the Smart Wheelchair *
isagmz 18:a10277a63b53 50 * *
isagmz 18:a10277a63b53 51 **************************************************************************/
isagmz 18:a10277a63b53 52 class Wheelchair
isagmz 18:a10277a63b53 53 {
isagmz 18:a10277a63b53 54 public:
isagmz 18:a10277a63b53 55 /*************************************************************************
isagmz 18:a10277a63b53 56 * Create Wheelchair constructor with x,y pin for analog-to-dc output *
isagmz 18:a10277a63b53 57 * serial for printout, and time. This is also used to initialize some *
isagmz 18:a10277a63b53 58 * variables for the timer,encoders and time-of-flight sensors *
isagmz 18:a10277a63b53 59 **************************************************************************/
isagmz 18:a10277a63b53 60 Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* wheel, QEI* wheelS,
isagmz 18:a10277a63b53 61 VL53L1X** ToF);
isagmz 18:a10277a63b53 62
isagmz 18:a10277a63b53 63 /*************************************************************************
isagmz 18:a10277a63b53 64 * This method is to move using the joystick *
isagmz 18:a10277a63b53 65 **************************************************************************/
isagmz 18:a10277a63b53 66 void move(float x_coor, float y_coor);
isagmz 18:a10277a63b53 67
isagmz 18:a10277a63b53 68 /*************************************************************************
isagmz 18:a10277a63b53 69 * This method is to drive the wheelchair forward manually (NO PID used)*
isagmz 18:a10277a63b53 70 ************************************************************************ */
isagmz 18:a10277a63b53 71 void forward();
isagmz 18:a10277a63b53 72 /*************************************************************************
isagmz 18:a10277a63b53 73 * This method is to drive the wheelchair backwards (NO PID used) *
isagmz 18:a10277a63b53 74 ************************************************************************ */
isagmz 18:a10277a63b53 75 void backward();
isagmz 18:a10277a63b53 76
isagmz 18:a10277a63b53 77 /*************************************************************************
isagmz 18:a10277a63b53 78 * This method is to turn the wheelchair right manually (NO PID used) *
isagmz 18:a10277a63b53 79 ************************************************************************ */
isagmz 18:a10277a63b53 80 void right();
isagmz 18:a10277a63b53 81
isagmz 18:a10277a63b53 82 /*************************************************************************
isagmz 18:a10277a63b53 83 * This method is to turn the wheelchair left manually (NO PID used) *
isagmz 18:a10277a63b53 84 ************************************************************************ */
isagmz 18:a10277a63b53 85 void left();
isagmz 18:a10277a63b53 86
isagmz 18:a10277a63b53 87 /*************************************************************************
isagmz 18:a10277a63b53 88 * This method stops the wheelchair and reset the joystick position *
isagmz 18:a10277a63b53 89 ************************************************************************ */
isagmz 18:a10277a63b53 90 void stop();
isagmz 18:a10277a63b53 91
isagmz 18:a10277a63b53 92 /*************************************************************************
isagmz 18:a10277a63b53 93 * This method is a thread that will obtain the IMU information such *
isagmz 18:a10277a63b53 94 * as the gyroscope x,y axis. Z-axis is not used. *
isagmz 18:a10277a63b53 95 ************************************************************************ */
isagmz 18:a10277a63b53 96 void compass_thread();
isagmz 18:a10277a63b53 97
isagmz 18:a10277a63b53 98 /*************************************************************************
isagmz 18:a10277a63b53 99 * This method is a thread that will calculate the velocity of the *
isagmz 18:a10277a63b53 100 * wheechair using the encoder values this is being obatined. *
isagmz 18:a10277a63b53 101 ************************************************************************ */
isagmz 18:a10277a63b53 102 void velocity_thread();
isagmz 18:a10277a63b53 103
isagmz 18:a10277a63b53 104 //not being used
isagmz 18:a10277a63b53 105 void rosCom_thread();
isagmz 18:a10277a63b53 106
isagmz 18:a10277a63b53 107 /*************************************************************************
isagmz 18:a10277a63b53 108 * This method is a thread that iterates through all the sensor's *
isagmz 18:a10277a63b53 109 * values and determines whether or not the wheelchair is about to hit *
isagmz 18:a10277a63b53 110 * or crash into something. If the sensors detect something close to *
isagmz 18:a10277a63b53 111 * the chair, then the chair will safely halt and allow movement in the *
isagmz 18:a10277a63b53 112 * direction opposed to where an object is detected. *
isagmz 18:a10277a63b53 113 ************************************************************************ */
isagmz 18:a10277a63b53 114 void ToFSafe_thread();
isagmz 18:a10277a63b53 115
isagmz 18:a10277a63b53 116 /*************************************************************************
isagmz 18:a10277a63b53 117 * This method is a thread that will constantly be checking the value *
isagmz 18:a10277a63b53 118 * of the emergency button. If the button is pressed, then the chair *
isagmz 18:a10277a63b53 119 * will stop and the entire system will reset. *
isagmz 18:a10277a63b53 120 ************************************************************************ */
isagmz 18:a10277a63b53 121 void emergencyButton_thread();
isagmz 18:a10277a63b53 122
isagmz 18:a10277a63b53 123 /*************************************************************************
isagmz 18:a10277a63b53 124 * This method gets the encoder values and calculates the distance since*
isagmz 18:a10277a63b53 125 * the last encoder reset. *
isagmz 18:a10277a63b53 126 **************************************************************************/
isagmz 18:a10277a63b53 127 float getDistance();
isagmz 18:a10277a63b53 128
isagmz 18:a10277a63b53 129 /*************************************************************************
isagmz 18:a10277a63b53 130 * This method resets the encoder value to recalculate distance *
isagmz 18:a10277a63b53 131 **************************************************************************/
isagmz 18:a10277a63b53 132 void resetDistance();
isagmz 18:a10277a63b53 133
isagmz 18:a10277a63b53 134
isagmz 18:a10277a63b53 135 /*************************************************************************
isagmz 18:a10277a63b53 136 * *
isagmz 18:a10277a63b53 137 * PID Control Methods *
isagmz 18:a10277a63b53 138 * *
isagmz 18:a10277a63b53 139 *************************************************************************/
isagmz 18:a10277a63b53 140
isagmz 18:a10277a63b53 141
isagmz 18:a10277a63b53 142 /*************************************************************************
isagmz 18:a10277a63b53 143 * This method moves the wheelchair x-millimiters forward using PID *
isagmz 18:a10277a63b53 144 **************************************************************************/
isagmz 18:a10277a63b53 145 void pid_forward(double mm);
isagmz 18:a10277a63b53 146
isagmz 18:a10277a63b53 147 /*************************************************************************
isagmz 18:a10277a63b53 148 * This method turns the chair right a certain amount of degrees using PID*
isagmz 18:a10277a63b53 149 **************************************************************************/
isagmz 18:a10277a63b53 150 void pid_right(int deg);
isagmz 18:a10277a63b53 151
isagmz 18:a10277a63b53 152 /*************************************************************************
isagmz 18:a10277a63b53 153 * This method turns the chair left a certain amount of degrees using PID *
isagmz 18:a10277a63b53 154 **************************************************************************/
isagmz 18:a10277a63b53 155 void pid_left(int deg);
isagmz 18:a10277a63b53 156
isagmz 18:a10277a63b53 157 /*************************************************************************
isagmz 18:a10277a63b53 158 * This method determines whether to turn the wheelchair left or right *
isagmz 18:a10277a63b53 159 **************************************************************************/
isagmz 18:a10277a63b53 160 void pid_turn(int deg);
isagmz 18:a10277a63b53 161
isagmz 18:a10277a63b53 162
isagmz 18:a10277a63b53 163 /*************************************************************************
isagmz 18:a10277a63b53 164 * *
isagmz 18:a10277a63b53 165 * ROS-Embed Communication Methods *
isagmz 18:a10277a63b53 166 * *
isagmz 18:a10277a63b53 167 *************************************************************************/
isagmz 18:a10277a63b53 168
isagmz 18:a10277a63b53 169 /*************************************************************************
isagmz 18:a10277a63b53 170 * This method computes the relative angle for Twist message in ROS *
isagmz 18:a10277a63b53 171 *************************************************************************/
isagmz 18:a10277a63b53 172 void pid_twistA();
isagmz 18:a10277a63b53 173
isagmz 18:a10277a63b53 174 /*************************************************************************
isagmz 18:a10277a63b53 175 * This method computes the relative velocity for Twist message in ROS *
isagmz 18:a10277a63b53 176 *************************************************************************/
isagmz 18:a10277a63b53 177 void pid_twistV();
isagmz 18:a10277a63b53 178
isagmz 18:a10277a63b53 179 /*************************************************************************
isagmz 18:a10277a63b53 180 * This method computes the angular position for Twist message in ROS *
isagmz 18:a10277a63b53 181 *************************************************************************/
isagmz 18:a10277a63b53 182 double getTwistZ();
isagmz 18:a10277a63b53 183
isagmz 18:a10277a63b53 184 /*************************************************************************
isagmz 18:a10277a63b53 185 * This method calculates the relative position of the chair everytime *
isagmz 18:a10277a63b53 186 * the encoders reset by setting its old position as the origin to *
isagmz 18:a10277a63b53 187 * calculate the new position. Moreover, this function is important to *
isagmz 18:a10277a63b53 188 * be able to publish (send) the information to ROS *
isagmz 18:a10277a63b53 189 *************************************************************************/
isagmz 18:a10277a63b53 190 void odomMsg();
isagmz 18:a10277a63b53 191
isagmz 18:a10277a63b53 192 /*************************************************************************
isagmz 18:a10277a63b53 193 * This method prints the Odometry message to the serial monitor *
isagmz 18:a10277a63b53 194 *************************************************************************/
isagmz 18:a10277a63b53 195 void showOdom();
isagmz 18:a10277a63b53 196
isagmz 18:a10277a63b53 197 /*************************************************************************
isagmz 18:a10277a63b53 198 * (not being used) Functions with a predetermined path (demo) *
isagmz 18:a10277a63b53 199 *************************************************************************/
isagmz 18:a10277a63b53 200 void desk();
isagmz 18:a10277a63b53 201 void kitchen();
isagmz 18:a10277a63b53 202 void desk_to_kitchen();
isagmz 18:a10277a63b53 203
isagmz 18:a10277a63b53 204 /*************************************************************************
isagmz 18:a10277a63b53 205 * Public Variables *
isagmz 18:a10277a63b53 206 **************************************************************************/
isagmz 18:a10277a63b53 207 double x_position;
isagmz 18:a10277a63b53 208 double y_position;
isagmz 18:a10277a63b53 209 double z_angular;
isagmz 18:a10277a63b53 210 double curr_vel;
isagmz 18:a10277a63b53 211 double z_twistA;
isagmz 18:a10277a63b53 212 double linearV;
isagmz 18:a10277a63b53 213 double angularV;
isagmz 18:a10277a63b53 214 double vel;
isagmz 18:a10277a63b53 215 double test1, test2;
isagmz 18:a10277a63b53 216 bool forwardSafety;
isagmz 18:a10277a63b53 217 bool sideSafety; //to check if can turn
isagmz 18:a10277a63b53 218 double curr_yaw, curr_velS; // Variable that contains current relative angle
isagmz 18:a10277a63b53 219
isagmz 18:a10277a63b53 220 private:
isagmz 18:a10277a63b53 221 int runningAverage[4];
isagmz 18:a10277a63b53 222 // Expected data used to compare whether of not there is a ledge. This serves as a ground base
isagmz 18:a10277a63b53 223 // Array is used for calibrating the time of flight sensors
isagmz 18:a10277a63b53 224 // Used to calculate stdev and mean on
isagmz 18:a10277a63b53 225
isagmz 18:a10277a63b53 226
isagmz 18:a10277a63b53 227
isagmz 18:a10277a63b53 228 /* Pointers for the joystick speed */
isagmz 18:a10277a63b53 229 PwmOut* x;
isagmz 18:a10277a63b53 230 PwmOut* y;
isagmz 18:a10277a63b53 231
isagmz 18:a10277a63b53 232 //Pointers for PCB
isagmz 18:a10277a63b53 233 PwmOut* on;
isagmz 18:a10277a63b53 234 PwmOut* off;
isagmz 18:a10277a63b53 235
isagmz 18:a10277a63b53 236 DigitalIn* e_button; //Pointer to e_button
isagmz 18:a10277a63b53 237
isagmz 18:a10277a63b53 238 chair_BNO055* imu; // Pointer to IMU
isagmz 18:a10277a63b53 239 Serial* out; // Pointer to Serial Monitor
isagmz 18:a10277a63b53 240 Timer* ti; // Pointer to the timer
isagmz 18:a10277a63b53 241 QEI* wheel; // Pointer to encoder
isagmz 18:a10277a63b53 242 QEI* wheelS; // Pointer to encoder
isagmz 18:a10277a63b53 243 VL53L1X** ToF; // Arrays of pointers to ToF sensors
isagmz 18:a10277a63b53 244
isagmz 18:a10277a63b53 245
isagmz 18:a10277a63b53 246
isagmz 18:a10277a63b53 247
isagmz 18:a10277a63b53 248 };
isagmz 18:a10277a63b53 249 #endif