00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <ros/ros.h>
00039 #include <can_msgs/CANFrame.h>
00040
00041
00042 #define ROBCHAIR_FUNCTION_MASK 0x007
00043 #define ROBCHAIR_SOURCE_MASK 0x078
00044 #define ROBCHAIR_DESTINATION_MASK 0x780
00045
00046
00047 #define ROBCHAIR_AXLE_LENGTH 0.61492
00048 #define ROBCHAIR_WHEEL_RADIUS 0.175
00049
00050 #define ROCHAIR_ENCODER_PULSES 2000
00051 #define ROBCHAIR_GEARBOX 10
00052
00053 #define ROBCHAIR_CONTROL_PERIOD 0.005
00054
00055 #define ROBCHAIR_K ((ROBCHAIR_CONTROL_PERIOD*ROCHAIR_ENCODER_PULSES*ROBCHAIR_GEARBOX)/(2* M_PI))
00056
00057 class RobChair
00058 {
00059 public:
00060
00062 typedef enum _RobChairDS {
00063
00064 PC = 0,
00065 RightPDrive = 1,
00066 LeftPDrive = 2,
00067 BothPDrives = 3,
00068 RightEncoder = 4,
00069 LeftEncoder = 5,
00070 BothEncoders = 6,
00071 Joystick = 10,
00072 SyncMCU = 15
00073
00074 } RobChairDS;
00075
00077 typedef enum _CommonFunction {
00078
00079 TurnOff = 0,
00080 TurnOn = 1
00081
00082 } CommonFunction;
00083
00085 typedef enum _PDriveFunction {
00086
00087 SetDACCommand = 2,
00088 SetPDriveControlMode = 3,
00089 DataFromMotor = 4
00090
00091 } PDriveFunction;
00092
00094 typedef enum _EncoderFunction {
00095
00096 SetVelocityValue = 2,
00097 SetEncoderControlMode = 3,
00098 SetPIControlValues = 4,
00099 ResetEncoderInformation = 5,
00100 DataFromEncoder = 6
00101
00102 } EncoderFunction;
00103
00105 typedef enum _TriggerFunction {
00106
00107 SyncronizeAllNodes = 15
00108
00109 } TriggerFunction;
00110
00118 RobChair();
00120
00123 ~RobChair();
00124
00134 void initializeRobChair(ros::Publisher * can_pub, double kp=0.0, double ki=0.0);
00135
00145 void setPIControlValues(RobChairDS destination, double kp, double ki);
00146
00155 void setVelocities(double linear_velocity, double angular_velocity);
00156
00163 void resetEncoders(){ sendCANFrame(RobChair::BothEncoders, RobChair::ResetEncoderInformation, NULL, 0); }
00164
00172 void receivedCANFrame(const can_msgs::CANFrame::ConstPtr& msg);
00173
00181 double getPositionX(){ return x_; }
00189 double getPositionY(){ return y_; }
00197 double getYaw(){ return yaw_; }
00205 double getLinearVelocity(){ return linear_velocity_; }
00213 double getAngularVelocity(){ return angular_velocity_; }
00214
00215 private:
00216
00228 void sendCANFrame(RobChairDS destination, char function, char * data, int data_count, char n=0);
00229
00238 void setVelocity(RobChairDS destination, double velocity);
00239
00248 void decodeEncoderData(RobChairDS encoder, const can_msgs::CANFrame::ConstPtr& msg);
00249
00250
00257 void turnPDrivesOn(){ sendCANFrame(BothPDrives, TurnOn, NULL, 0); }
00264 void turnPDrivesOff(){ sendCANFrame(BothPDrives, TurnOff, NULL, 0); }
00265
00272 void turnEncodersOn(){ sendCANFrame(BothEncoders, TurnOn, NULL, 0); }
00279 void turnEncodersOff(){ sendCANFrame(BothEncoders, TurnOff, NULL, 0); }
00280
00287 void turnTriggerOn(){ sendCANFrame(SyncMCU, TurnOn, NULL, 0); }
00294 void turnTriggerOff(){ sendCANFrame(SyncMCU, TurnOff, NULL, 0); }
00295
00296
00298 ros::Publisher * can_pub_;
00299
00301 char n_;
00302
00303
00305 double x_;
00307 double y_;
00309 double yaw_;
00311 double linear_velocity_;
00313 double angular_velocity_;
00314
00316 double left_wheel_position_;
00318 double left_wheel_velocity_;
00320 double right_wheel_position_;
00322 double right_wheel_velocity_;
00323
00324
00326 double last_left_wheel_position_;
00328 double last_right_wheel_position_;
00330 bool got_left_encoder_data_;
00332 bool got_right_encoder_data_;
00333
00341 double limitAngle(double angle);
00342 };
00343
00344