Where we will test the side ToF sensors
Dependencies: QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic
wheelchairControlSideTof/wheelchair.h@18:a10277a63b53, 2019-07-02 (annotated)
- Committer:
- isagmz
- Date:
- Tue Jul 02 17:27:51 2019 +0000
- Revision:
- 18:a10277a63b53
- Child:
- 19:5376c858e574
for debugging, added comments
Who changed what in which revision?
User | Revision | Line number | New 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 | 18:a10277a63b53 | 45 | #define MIN_WALL_LENGTH 10 // minimum distance from wall to ToF (cm) |
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 |