RobChair.h

Go to the documentation of this file.

/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2012, ISR University of Coimbra.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the ISR University of Coimbra nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Gonçalo Cabrita on 28/05/2012
*********************************************************************/

#include <ros/ros.h>
#include <can_msgs/CANFrame.h>

// Masks
#define ROBCHAIR_FUNCTION_MASK      0x007
#define ROBCHAIR_SOURCE_MASK        0x078
#define ROBCHAIR_DESTINATION_MASK   0x780

// Robot constants
#define ROBCHAIR_AXLE_LENGTH        0.61492
#define ROBCHAIR_WHEEL_RADIUS       0.175

#define ROCHAIR_ENCODER_PULSES      2000
#define ROBCHAIR_GEARBOX            10

#define ROBCHAIR_CONTROL_PERIOD     0.005

#define ROBCHAIR_K ((ROBCHAIR_CONTROL_PERIOD*ROCHAIR_ENCODER_PULSES*ROBCHAIR_GEARBOX)/(2* M_PI))  

class RobChair
{
public:
    
    typedef enum _RobChairDS {
        
        PC = 0,
        RightPDrive = 1,
        LeftPDrive = 2,
        BothPDrives = 3,
        RightEncoder = 4,
        LeftEncoder = 5,
        BothEncoders = 6,
        Joystick = 10,
        SyncMCU = 15
        
    } RobChairDS;
    
    typedef enum _CommonFunction {
        
        TurnOff = 0,
        TurnOn = 1
        
    } CommonFunction;
    
    typedef enum _PDriveFunction {
        
        SetDACCommand = 2,
        SetPDriveControlMode = 3,
        DataFromMotor = 4
        
    } PDriveFunction;
    
    typedef enum _EncoderFunction {
        
        SetVelocityValue = 2,
        SetEncoderControlMode = 3,
        SetPIControlValues = 4,
        ResetEncoderInformation = 5,
        DataFromEncoder = 6
        
    } EncoderFunction;
    
    typedef enum _TriggerFunction {
        
        SyncronizeAllNodes = 15
        
    } TriggerFunction;
    
    RobChair();

    ~RobChair();
    
    void initializeRobChair(ros::Publisher * can_pub, double kp=0.0, double ki=0.0);
    
    void setPIControlValues(RobChairDS destination, double kp, double ki);
    
    void setVelocities(double linear_velocity, double angular_velocity);
    
    void resetEncoders(){ sendCANFrame(RobChair::BothEncoders, RobChair::ResetEncoderInformation, NULL, 0); }
    
    void receivedCANFrame(const can_msgs::CANFrame::ConstPtr& msg);
    
    double getPositionX(){ return x_; }
    double getPositionY(){ return y_; }
    double getYaw(){ return yaw_; }
    double getLinearVelocity(){ return linear_velocity_; }
    double getAngularVelocity(){ return angular_velocity_; }
    
private:
    
    void sendCANFrame(RobChairDS destination, char function, char * data, int data_count, char n=0);
    
    void setVelocity(RobChairDS destination, double velocity);
    
    void decodeEncoderData(RobChairDS encoder, const can_msgs::CANFrame::ConstPtr& msg);
    
    
    void turnPDrivesOn(){ sendCANFrame(BothPDrives, TurnOn, NULL, 0); }
    void turnPDrivesOff(){ sendCANFrame(BothPDrives, TurnOff, NULL, 0); }
    
    void turnEncodersOn(){ sendCANFrame(BothEncoders, TurnOn, NULL, 0); }
    void turnEncodersOff(){ sendCANFrame(BothEncoders, TurnOff, NULL, 0); }
    
    void turnTriggerOn(){ sendCANFrame(SyncMCU, TurnOn, NULL, 0); }
    void turnTriggerOff(){ sendCANFrame(SyncMCU, TurnOff, NULL, 0); }
    
    
    ros::Publisher * can_pub_;
    
    char n_;
    
    // RobChair variables
    double x_;
    double y_;
    double yaw_;
    double linear_velocity_;
    double angular_velocity_;
    
    double left_wheel_position_;
    double left_wheel_velocity_;
    double right_wheel_position_;
    double right_wheel_velocity_;
    
    // Aux variables
    double last_left_wheel_position_;
    double last_right_wheel_position_;
    bool got_left_encoder_data_;
    bool got_right_encoder_data_;

    double limitAngle(double angle);
};

// EOF