File content as of revision 0:f8c318023708:

#include"MotorControll.h"

motorControll::motorControll(){
}

void motorControll::setSpeed(float xData,float yData,float omega){
    timer.detach();
    
    xData_=xData;
    yData_=yData;
    omega_=omega;
    
    if(omega_!=0){
        xData_=0;
        yData_=0;
        Dimension_=abs(omega_);
    }
    else{
        xData_=(xData_- 0.5)*2;
        yData_=(yData_- 0.5)*2;
        
        if((xData_ > -0.05 && xData_<0.05) && (yData_ > -0.05 && yData_<0.05)){
            xData_=0;
            yData_=0;
        }
        
        Angle_= atan2(yData_,xData_);
        Dimension_= (sqrt(yData_ * yData_ + xData_ * xData_));
        if(abs(Dimension_)>1.0)
            Dimension_=1.0;
    }
    dataVector_[0]=xData_;
    dataVector_[1]=yData_;
    dataVector_[2]=omega_;
    calcPwm();
    
    timer.attach(&motorControll::flip,interval_);
}

float motorControll::getSpeed(Wheel wheel){
    switch(wheel){
        case Front_Right:
            return current_speedVector_[0];
            
        case Front_Left:
            return current_speedVector_[1];
            
        case Back_Right:
            return current_speedVector_[3];
        
        case Back_Left:
            return current_speedVector_[2];
            
        default:
            break;
    }
}

float motorControll::getAngularVelocity(Wheel wheel){
    switch(wheel){
        case Front_Right:
            return angularVelocity_[0];
            
        case Front_Left:
            return angularVelocity_[1];
            
        case Back_Right:
            return angularVelocity_[3];
        
        case Back_Left:
            return angularVelocity_[2];
        
        default:
            break;
    }
}

void motorControll::setVoltParRevolution(float Volt_per_Revolution){
    Volt_per_Revolution_=Volt_per_Revolution;
}

void motorControll::setAccel(float accel){
    accel_=accel;
}

void motorControll::setTicker(float interval){
    interval_=interval;
    timer.attach(&motorControll::flip,interval_);
}

void motorControll::calcPwm(){
    float max=0;
    for(int i=0;i<4;i++){
        target_speedVector_[i]=0;
        for(int k=0;k<3;k++){
            target_speedVector_[i]+=fixedVector_[i][k]*dataVector_[k];
        }
        if(max<abs(target_speedVector_[i]))
            max=abs(target_speedVector_[i]);
    }
    target_speedVector_[0] = target_speedVector_[0] / max * Dimension_;
    target_speedVector_[1] = -target_speedVector_[1] / max * Dimension_;
    target_speedVector_[2] = -target_speedVector_[2] / max * Dimension_;
    target_speedVector_[3] = target_speedVector_[3] / max * Dimension_;
}

void motorControll::flip(){
    for(int i=0;i<4;i++){
        
        if(target_speedVector_[i] > current_speedVector_[i])
            current_speedVector_[i] += accel_*interval_;
        else if(target_speedVector_[i] < current_speedVector_[i])
            current_speedVector_[i] -= accel_*interval_;
        
        if((target_speedVector_[i]+accel_*interval_ > current_speedVector_[i]) && (target_speedVector_[i]-accel_*interval_ < current_speedVector_[i])){
            current_speedVector_[i] = target_speedVector_[i];
            timer.detach();
        }
          
        angularVelocity_[i]=current_speedVector_[i]*13.2*Volt_per_Revolution_;
    }
}

float motorControll::fixedVector_[4][3]={{-1.0,1.0,1.0},
                                           {1.0,1.0,-1.0},
                                           {-1.0,1.0,-1.0},
                                           {1.0,1.0,1.0}};
float motorControll::dataVector_[3]={0,0,0};
float motorControll::target_speedVector_[4]={0,0,0,0};
float motorControll::current_speedVector_[4]={0,0,0,0};
float motorControll::angularVelocity_[4]={0,0,0,0};
float motorControll::xData_=0.0f;
float motorControll::yData_=0.0f;
float motorControll::omega_=0.0f;
float motorControll::Angle_=0.0f;
float motorControll::Dimension_=0.0f;
float motorControll::accel_=1000;
float motorControll::Volt_per_Revolution_=0.05;
float motorControll::interval_=0.001;
Ticker motorControll::timer;