Deze moet je hebben Menner Dennis

motorDriver.h

Committer:
Twan123
Date:
2018-12-19
Revision:
0:7866cf54b606

File content as of revision 0:7866cf54b606:

#include "mbed.h"
#include <ros.h>
#include <std_msgs/Float64.h>
#include <string>
#include "rtos.h"


class motorDriver{

public:
    motorDriver(PinName Left1, PinName Left2, PinName Right1, PinName Right2, PinName PWML, PinName PWMR/*, PinName spdReadL, PinName spdReadR, PinName Bat*/);
    
    //Direction functions: Subscribe and Set
//    void subDirection();
    void setDirection(bool  keyForward,  bool  keyBackward, bool  keyLeft, bool  keyRight);  // fwd bck, lft,rght

    
    // Get and publish speed information to the Raspberry Pi
//    void getSpeedL(); 
//    void getSpeedR();

//     void getBattery();
    
    //get and publish current information to the Raspberry Pi
//   void getCurrent();
//   void pubCurrent();
   

    
protected:
    
    DigitalOut speedLeft1;
    DigitalOut speedLeft2;        
    DigitalOut speedRight1;
    DigitalOut speedRight2; 
   
    PwmOut pwmL;
    PwmOut pwmR;
    
    //DigitalIn speedReadL;
    //DigitalIn speedReadR;
    
    //AnalogIn BatteryRead; 
          
    bool  keyForward;           
    bool  keyBackward;
    bool  keyLeft;
    bool  keyRight;
    
    
    //ros::NodeHandle nh;
    //std_msgs::Float64 value_msg;
    //float currentLeft; 
    //float currentRight;

    };