Jesus Fausto / wheelchaircontrolRosCom

Odometry communication

Dependencies:   PID QEI chair_BNO055 ros_lib_kinetic3

Dependents:   wheelchaircontrolrealtime1

Fork of wheelchaircontrol by Jesus Fausto

wheelchair.h@29:a6a1ccf14c63, 2018-11-03 (annotated)

Committer:
jvfausto
Date:
Sat Nov 03 20:35:30 2018 +0000
Revision:
29:a6a1ccf14c63
Parent:
28:889fc0e5f8c4 
Now with twist and Odom messages

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:fc0c4a184482 1 #ifndef wheelchair
ryanlin97 0:fc0c4a184482 2 #define wheelchair
ryanlin97 0:fc0c4a184482 3
jvfausto 21:8d11d953ceeb 4 //Importing libraries into wheelchair.h
ryanlin97 11:d14a1f7f1297 5 #include "chair_BNO055.h"
jvfausto 17:7f3b69300bb6 6 #include "PID.h"
ryanlin97 12:921488918749 7 #include "QEI.h"
ryanlin97 16:b403082eeacd 8 #include <ros.h>
ryanlin97 16:b403082eeacd 9 #include <geometry_msgs/Twist.h>
jvfausto 24:6c5b4b82f874 10 #include <nav_msgs/Odometry.h>
ryanlin97 12:921488918749 11
ryanlin97 14:9caca9fde9b0 12
jvfausto 21:8d11d953ceeb 13 /*
jvfausto 21:8d11d953ceeb 14 * joystick has analog out of 200-700, scale values between 1.3 and 3.3
jvfausto 21:8d11d953ceeb 15 */
jvfausto 21:8d11d953ceeb 16 #define def (2.5f/3.3f) //Default axis on joystick to stay neutral; used on x and y axis
jvfausto 21:8d11d953ceeb 17 #define high 3.3f/3.3f //High power on joystick; used on x and y axis
jvfausto 21:8d11d953ceeb 18 #define low (1.7f/3.3f) //Low power on joystick; used on x and y axis
jvfausto 21:8d11d953ceeb 19 #define offset .035f //Joystick ajustment to be able to go strait. Chair dependent on manufactoring presision
jvfausto 21:8d11d953ceeb 20 #define process .1 //Defines default time delay in seconds
ryanlin97 14:9caca9fde9b0 21
jvfausto 21:8d11d953ceeb 22 //Pin plug in for Nucleo-L432KC
jvfausto 21:8d11d953ceeb 23 #define xDir D9 //PWM Pins
ryanlin97 14:9caca9fde9b0 24 #define yDir D10
jvfausto 21:8d11d953ceeb 25 #define Encoder1 D7 //Digital In Pull Up Pin
ryanlin97 14:9caca9fde9b0 26 #define Encoder2 D8
ryanlin97 14:9caca9fde9b0 27
jvfausto 21:8d11d953ceeb 28 #define Diameter 31.75 //Diameter of encoder wheel
ryanlin97 12:921488918749 29 /** Wheelchair class
ryanlin97 12:921488918749 30 * Used for controlling the smart wheelchair
ryanlin97 12:921488918749 31 */
jvfausto 17:7f3b69300bb6 32
ryanlin97 0:fc0c4a184482 33 class Wheelchair
ryanlin97 0:fc0c4a184482 34 {
ryanlin97 0:fc0c4a184482 35 public:
ryanlin97 12:921488918749 36 /** Create Wheelchair Object with x,y pin for analog dc output
ryanlin97 12:921488918749 37 * serial for printout, and timer
ryanlin97 12:921488918749 38 */
jvfausto 22:d2f234fbc20d 39 Wheelchair(PinName xPin, PinName yPin, Serial* pc, Timer* time, QEI* wheel, QEI* wheelS);
ryanlin97 12:921488918749 40
ryanlin97 12:921488918749 41 /** move using the joystick */
ryanlin97 3:a5e71bfdb492 42 void move(float x_coor, float y_coor);
ryanlin97 12:921488918749 43
ryanlin97 12:921488918749 44 /* turn right a certain amount of degrees using PID*/
ryanlin97 12:921488918749 45 void pid_right(int deg);
ryanlin97 12:921488918749 46
ryanlin97 12:921488918749 47 /* turn left a certain amount of degrees using PID*/
ryanlin97 12:921488918749 48 void pid_left(int deg);
ryanlin97 12:921488918749 49
ryanlin97 12:921488918749 50 /* drive the wheelchair forward */
ryanlin97 1:c0beadca1617 51 void forward();
ryanlin97 12:921488918749 52
ryanlin97 12:921488918749 53 /* drive the wheelchair backward*/
ryanlin97 1:c0beadca1617 54 void backward();
ryanlin97 12:921488918749 55
ryanlin97 12:921488918749 56 /* turn the wheelchair right*/
ryanlin97 1:c0beadca1617 57 void right();
ryanlin97 12:921488918749 58
ryanlin97 12:921488918749 59 /* turn the wheelchair left*/
ryanlin97 1:c0beadca1617 60 void left();
ryanlin97 12:921488918749 61
ryanlin97 12:921488918749 62 /* stop the wheelchair*/
ryanlin97 1:c0beadca1617 63 void stop();
ryanlin97 12:921488918749 64
ryanlin97 12:921488918749 65 /* function to get imu data*/
ryanlin97 11:d14a1f7f1297 66 void compass_thread();
jvfausto 22:d2f234fbc20d 67 void velosity_thread();
jvfausto 24:6c5b4b82f874 68 void rosCom_thread();
jvfausto 21:8d11d953ceeb 69
jvfausto 21:8d11d953ceeb 70 /* move x millimiters foward using PID*/
jvfausto 19:71a6621ee5c3 71 void pid_forward(double mm);
jvfausto 21:8d11d953ceeb 72
jvfausto 21:8d11d953ceeb 73 /* move x millimiters reverse using PID*/
jvfausto 17:7f3b69300bb6 74 void pid_reverse(double mm);
jvfausto 21:8d11d953ceeb 75
jvfausto 21:8d11d953ceeb 76 /* gets the encoder distance moved since encoder reset*/
ryanlin97 12:921488918749 77 float getDistance();
jvfausto 21:8d11d953ceeb 78
jvfausto 21:8d11d953ceeb 79 /* resets encoder*/
ryanlin97 12:921488918749 80 void resetDistance();
jvfausto 21:8d11d953ceeb 81
jvfausto 21:8d11d953ceeb 82 /* function to to determine whether we are turning left or right*/
ryanlin97 12:921488918749 83 void pid_turn(int deg);
jvfausto 21:8d11d953ceeb 84
jvfausto 22:d2f234fbc20d 85 void pid_twistA();
jvfausto 22:d2f234fbc20d 86 void pid_twistV();
jvfausto 23:58ec657a44f2 87
jvfausto 23:58ec657a44f2 88 void odomMsg();
jvfausto 23:58ec657a44f2 89 void showOdom();
jvfausto 23:58ec657a44f2 90
jvfausto 21:8d11d953ceeb 91 /* functions with a predetermined path demmo*/
jvfausto 19:71a6621ee5c3 92 void desk();
jvfausto 19:71a6621ee5c3 93 void kitchen();
jvfausto 19:71a6621ee5c3 94 void desk_to_kitchen();
jvfausto 23:58ec657a44f2 95
jvfausto 28:889fc0e5f8c4 96
jvfausto 28:889fc0e5f8c4 97 double getTwistZ();
jvfausto 28:889fc0e5f8c4 98
jvfausto 28:889fc0e5f8c4 99
jvfausto 24:6c5b4b82f874 100 double x_position;
jvfausto 24:6c5b4b82f874 101 double y_position;
jvfausto 24:6c5b4b82f874 102 double z_angular;
jvfausto 28:889fc0e5f8c4 103 double curr_vel;
jvfausto 28:889fc0e5f8c4 104 double z_twistA;
ryanlin97 1:c0beadca1617 105 private:
jvfausto 21:8d11d953ceeb 106 /* Pointers for the joystick speed*/
ryanlin97 3:a5e71bfdb492 107 PwmOut* x;
ryanlin97 3:a5e71bfdb492 108 PwmOut* y;
jvfausto 21:8d11d953ceeb 109
jvfausto 21:8d11d953ceeb 110 chair_BNO055* imu; // Pointer to IMU
jvfausto 21:8d11d953ceeb 111 Serial* out; // Pointer to Serial Monitor
jvfausto 21:8d11d953ceeb 112 Timer* ti; // Pointer to the timer
jvfausto 21:8d11d953ceeb 113 QEI* wheel; // Pointer to encoder
jvfausto 22:d2f234fbc20d 114 QEI* wheelS; // Pointer to encoder
ryanlin97 0:fc0c4a184482 115 };
ryanlin97 0:fc0c4a184482 116 #endif