hnct-robocon / MotorControll

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

MotorControll.cpp@1:c9970769436f, 2018-11-20 (annotated)

Committer:
Takkun
Date:
Tue Nov 20 08:47:57 2018 +0000
Revision:
1:c9970769436f
Parent:
0:f8c318023708 
controll forth omni motor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Takkun 0:f8c318023708 1 #include"MotorControll.h"
Takkun 0:f8c318023708 2
Takkun 0:f8c318023708 3 motorControll::motorControll(){
Takkun 0:f8c318023708 4 }
Takkun 0:f8c318023708 5
Takkun 0:f8c318023708 6 void motorControll::setSpeed(float xData,float yData,float omega){
Takkun 0:f8c318023708 7 timer.detach();
Takkun 0:f8c318023708 8
Takkun 0:f8c318023708 9 xData_=xData;
Takkun 0:f8c318023708 10 yData_=yData;
Takkun 0:f8c318023708 11 omega_=omega;
Takkun 0:f8c318023708 12
Takkun 0:f8c318023708 13 if(omega_!=0){
Takkun 0:f8c318023708 14 xData_=0;
Takkun 0:f8c318023708 15 yData_=0;
Takkun 0:f8c318023708 16 Dimension_=abs(omega_);
Takkun 0:f8c318023708 17 }
Takkun 0:f8c318023708 18 else{
Takkun 0:f8c318023708 19 xData_=(xData_- 0.5)*2;
Takkun 0:f8c318023708 20 yData_=(yData_- 0.5)*2;
Takkun 0:f8c318023708 21
Takkun 0:f8c318023708 22 if((xData_ > -0.05 && xData_<0.05) && (yData_ > -0.05 && yData_<0.05)){
Takkun 0:f8c318023708 23 xData_=0;
Takkun 0:f8c318023708 24 yData_=0;
Takkun 0:f8c318023708 25 }
Takkun 0:f8c318023708 26
Takkun 0:f8c318023708 27 Angle_= atan2(yData_,xData_);
Takkun 0:f8c318023708 28 Dimension_= (sqrt(yData_ * yData_ + xData_ * xData_));
Takkun 0:f8c318023708 29 if(abs(Dimension_)>1.0)
Takkun 0:f8c318023708 30 Dimension_=1.0;
Takkun 0:f8c318023708 31 }
Takkun 0:f8c318023708 32 dataVector_[0]=xData_;
Takkun 0:f8c318023708 33 dataVector_[1]=yData_;
Takkun 0:f8c318023708 34 dataVector_[2]=omega_;
Takkun 0:f8c318023708 35 calcPwm();
Takkun 0:f8c318023708 36
Takkun 0:f8c318023708 37 timer.attach(&motorControll::flip,interval_);
Takkun 0:f8c318023708 38 }
Takkun 0:f8c318023708 39
Takkun 0:f8c318023708 40 float motorControll::getSpeed(Wheel wheel){
Takkun 0:f8c318023708 41 switch(wheel){
Takkun 0:f8c318023708 42 case Front_Right:
Takkun 0:f8c318023708 43 return current_speedVector_[0];
Takkun 0:f8c318023708 44
Takkun 0:f8c318023708 45 case Front_Left:
Takkun 0:f8c318023708 46 return current_speedVector_[1];
Takkun 0:f8c318023708 47
Takkun 0:f8c318023708 48 case Back_Right:
Takkun 0:f8c318023708 49 return current_speedVector_[3];
Takkun 0:f8c318023708 50
Takkun 0:f8c318023708 51 case Back_Left:
Takkun 0:f8c318023708 52 return current_speedVector_[2];
Takkun 0:f8c318023708 53
Takkun 0:f8c318023708 54 default:
Takkun 0:f8c318023708 55 break;
Takkun 0:f8c318023708 56 }
Takkun 0:f8c318023708 57 }
Takkun 0:f8c318023708 58
Takkun 0:f8c318023708 59 float motorControll::getAngularVelocity(Wheel wheel){
Takkun 0:f8c318023708 60 switch(wheel){
Takkun 0:f8c318023708 61 case Front_Right:
Takkun 0:f8c318023708 62 return angularVelocity_[0];
Takkun 0:f8c318023708 63
Takkun 0:f8c318023708 64 case Front_Left:
Takkun 0:f8c318023708 65 return angularVelocity_[1];
Takkun 0:f8c318023708 66
Takkun 0:f8c318023708 67 case Back_Right:
Takkun 0:f8c318023708 68 return angularVelocity_[3];
Takkun 0:f8c318023708 69
Takkun 0:f8c318023708 70 case Back_Left:
Takkun 0:f8c318023708 71 return angularVelocity_[2];
Takkun 0:f8c318023708 72
Takkun 0:f8c318023708 73 default:
Takkun 0:f8c318023708 74 break;
Takkun 0:f8c318023708 75 }
Takkun 0:f8c318023708 76 }
Takkun 0:f8c318023708 77
Takkun 0:f8c318023708 78 void motorControll::setVoltParRevolution(float Volt_per_Revolution){
Takkun 0:f8c318023708 79 Volt_per_Revolution_=Volt_per_Revolution;
Takkun 0:f8c318023708 80 }
Takkun 0:f8c318023708 81
Takkun 0:f8c318023708 82 void motorControll::setAccel(float accel){
Takkun 0:f8c318023708 83 accel_=accel;
Takkun 0:f8c318023708 84 }
Takkun 0:f8c318023708 85
Takkun 0:f8c318023708 86 void motorControll::setTicker(float interval){
Takkun 0:f8c318023708 87 interval_=interval;
Takkun 0:f8c318023708 88 timer.attach(&motorControll::flip,interval_);
Takkun 0:f8c318023708 89 }
Takkun 0:f8c318023708 90
Takkun 0:f8c318023708 91 void motorControll::calcPwm(){
Takkun 0:f8c318023708 92 float max=0;
Takkun 0:f8c318023708 93 for(int i=0;i<4;i++){
Takkun 0:f8c318023708 94 target_speedVector_[i]=0;
Takkun 0:f8c318023708 95 for(int k=0;k<3;k++){
Takkun 0:f8c318023708 96 target_speedVector_[i]+=fixedVector_[i][k]*dataVector_[k];
Takkun 0:f8c318023708 97 }
Takkun 0:f8c318023708 98 if(max<abs(target_speedVector_[i]))
Takkun 0:f8c318023708 99 max=abs(target_speedVector_[i]);
Takkun 0:f8c318023708 100 }
Takkun 0:f8c318023708 101 target_speedVector_[0] = target_speedVector_[0] / max * Dimension_;
Takkun 0:f8c318023708 102 target_speedVector_[1] = -target_speedVector_[1] / max * Dimension_;
Takkun 0:f8c318023708 103 target_speedVector_[2] = -target_speedVector_[2] / max * Dimension_;
Takkun 0:f8c318023708 104 target_speedVector_[3] = target_speedVector_[3] / max * Dimension_;
Takkun 0:f8c318023708 105 }
Takkun 0:f8c318023708 106
Takkun 0:f8c318023708 107 void motorControll::flip(){
Takkun 0:f8c318023708 108 for(int i=0;i<4;i++){
Takkun 0:f8c318023708 109
Takkun 0:f8c318023708 110 if(target_speedVector_[i] > current_speedVector_[i])
Takkun 0:f8c318023708 111 current_speedVector_[i] += accel_*interval_;
Takkun 0:f8c318023708 112 else if(target_speedVector_[i] < current_speedVector_[i])
Takkun 0:f8c318023708 113 current_speedVector_[i] -= accel_*interval_;
Takkun 0:f8c318023708 114
Takkun 0:f8c318023708 115 if((target_speedVector_[i]+accel_*interval_ > current_speedVector_[i]) && (target_speedVector_[i]-accel_*interval_ < current_speedVector_[i])){
Takkun 0:f8c318023708 116 current_speedVector_[i] = target_speedVector_[i];
Takkun 0:f8c318023708 117 timer.detach();
Takkun 0:f8c318023708 118 }
Takkun 0:f8c318023708 119
Takkun 0:f8c318023708 120 angularVelocity_[i]=current_speedVector_[i]*13.2*Volt_per_Revolution_;
Takkun 0:f8c318023708 121 }
Takkun 0:f8c318023708 122 }
Takkun 0:f8c318023708 123
Takkun 0:f8c318023708 124 float motorControll::fixedVector_[4][3]={{-1.0,1.0,1.0},
Takkun 0:f8c318023708 125 {1.0,1.0,-1.0},
Takkun 0:f8c318023708 126 {-1.0,1.0,-1.0},
Takkun 0:f8c318023708 127 {1.0,1.0,1.0}};
Takkun 0:f8c318023708 128 float motorControll::dataVector_[3]={0,0,0};
Takkun 0:f8c318023708 129 float motorControll::target_speedVector_[4]={0,0,0,0};
Takkun 0:f8c318023708 130 float motorControll::current_speedVector_[4]={0,0,0,0};
Takkun 0:f8c318023708 131 float motorControll::angularVelocity_[4]={0,0,0,0};
Takkun 0:f8c318023708 132 float motorControll::xData_=0.0f;
Takkun 0:f8c318023708 133 float motorControll::yData_=0.0f;
Takkun 0:f8c318023708 134 float motorControll::omega_=0.0f;
Takkun 0:f8c318023708 135 float motorControll::Angle_=0.0f;
Takkun 0:f8c318023708 136 float motorControll::Dimension_=0.0f;
Takkun 1:c9970769436f 137 float motorControll::accel_=100;
Takkun 0:f8c318023708 138 float motorControll::Volt_per_Revolution_=0.05;
Takkun 0:f8c318023708 139 float motorControll::interval_=0.001;
Takkun 0:f8c318023708 140 Ticker motorControll::timer;