Diff: MotorControll.cpp

Revision:

0:f8c318023708

Child:

1:c9970769436f

diff -r 000000000000 -r f8c318023708 MotorControll.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorControll.cpp	Sun Jun 17 11:49:53 2018 +0000
@@ -0,0 +1,140 @@
+#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;
\ No newline at end of file