YellowMotors.cpp

00001 #include <math.h>
00002 #include "YellowMotors.h"
00003 #include "mbed.h"
00004 
00005 
00006 float motorLinearizationL(float desired)
00007 {
00008     return (float)(((6.6691*(exp(0.0249053*(desired*100))+2.92508))/100.0)*(desired*10/9+0.1));
00009 };
00010 
00011 float motorLinearizationR(float desired)
00012 {
00013     return (float)(4.096509*(exp(0.0286952296*(desired*100))+5.073644964)/100.0*(desired*10/9+0.1)); // \/ history of trial and error
00014     //return 5.9693939*(exp(0.0251906*(desired*100))+3.162519)/100.0; // that wasn't that bad at all! to early start, the rest ok
00015     //return (3.45183*(exp(0.0292461*(desired*100))+5.51727))/100.0;
00016 };
00017 
00018 YellowMotors::YellowMotors(PinName clk, PinName  lat, PinName  dat, PinName ena, PinName Lpwm, PinName Rpwm) : _clk(clk), _lat(lat), _dat(dat), _ena(ena), L(D11), R(D3) //, _Lpwm(Lpwm), _Rpwm(Rpwm)
00019 {
00020     _init();
00021 }
00022 void YellowMotors::_init()
00023 {
00024     _directions = 0xff;
00025     //PwmOut _Lpwm(D11);
00026     //PwmOut _Rpwm(D3);
00027     //DigitalOut _dat(dat);
00028     //DigitalOut _clk(clk);
00029     //DigitalOut _lat(lat);
00030 
00031     //motorItself::motorItself L(Lpwm);
00032     //motorItself::motorItself R(Rpwm);
00033     // and create motor objects
00034     _ena.write(0);
00035     //pc.printf("konstruktor yellow. Lpwm: %d\n\r", Lpwm);
00036 }
00037 void YellowMotors::operator= (const float value)
00038 {
00039     this->set(0, value);
00040     this->set(1, value);
00041 };
00042 void YellowMotors::set(float value, int whichMotor )
00043 {
00044     //pc.printf("value: %.3f", value);
00045     char directionsNew = _directions;
00046     if(whichMotor == -1) { // case set both;
00047         L = motorLinearizationL(abs(value));
00048         R = motorLinearizationL(abs(value));
00049         if(value > 0) {
00050             directionsNew &= ~L_B;
00051             directionsNew |= L_F;
00052             directionsNew &= ~R_B;
00053             directionsNew |= R_F;
00054         } else if (value < 0) {
00055             directionsNew &= ~L_F;
00056             directionsNew |= L_B;
00057             directionsNew &= ~R_F;
00058             directionsNew |= R_B;
00059         } else { // this and other " else value == 0 " update directions register to not allow any movement during reset/programming. Basiclly if the speed is to be 0, then turn the motors off.
00060             L = 0;
00061             R = 0;
00062             directionsNew |= L_B;
00063             directionsNew |= L_F;
00064             directionsNew |= R_B;
00065             directionsNew |= R_F;
00066         }
00067     } else {
00068         if(whichMotor == 0) {
00069             L = motorLinearizationL(abs(value));
00070             if(value > 0) {
00071                 directionsNew &= ~L_B;
00072                 directionsNew |= L_F;
00073             } else if (value < 0) {
00074                 directionsNew &= ~L_F;
00075                 directionsNew |= L_B;
00076             } else {
00077                 L = 0;
00078                 directionsNew |= L_B;
00079                 directionsNew |= L_F;
00080             }
00081         } else if(whichMotor == 1) {
00082             R = motorLinearizationR(abs(value));
00083             if(value > 0) {
00084                 directionsNew &= ~R_B;
00085                 directionsNew |= R_F;
00086             } else if (value < 0) {
00087                 directionsNew &= ~R_F;
00088                 directionsNew |= R_B;
00089             } else {
00090                 R = 0;
00091                 directionsNew |= R_B;
00092                 directionsNew |= R_F;
00093             }
00094         }
00095     }
00096     if ( _directions != directionsNew ) { // only change register content when needed.
00097         _directions = directionsNew;
00098         setDirections(_directions);
00099     }
00100 };
00101 
00102 float YellowMotors::get(int whichMotor)
00103 {
00104     if(whichMotor == -1) { // case set both;
00105         return this->L._pwmSigned + this->R._pwmSigned;
00106     } else {
00107         if(whichMotor == 0) {
00108             return L._pwmSigned;
00109         } else if(whichMotor == 1) {
00110             return R._pwmSigned;
00111         }
00112     }
00113     return -1;
00114 };
00115 
00116 void YellowMotors::setDirections(char directions)
00117 {
00118     if (_led != 0) *_led = 1;
00119     //pc.printf("\n\r_led ma adres: %d\n\r", _led);
00120     //pc.printf("### set dirs: ");
00121     for(int i=0; i<8; ++i)
00122         //pc.printf("%c", ((directions >> (7-i)) & 0x1)?'x':'_');
00123     //pc.printf(" ###\n\r");
00124     _directions = directions;
00125     _lat = 0;
00126     for (signed char i = 7; i >= 0; i--) {
00127         _clk = 0;
00128         _dat = (directions >> i) & 0x1;
00129         wait(0.00001);
00130         _clk = 1;
00131     }
00132     _lat = 1;
00133     _clk = 0;
00134     if (_led != 0) *_led = 0;
00135 };
00136 
00137 void YellowMotors::attachled(DigitalOut & led)
00138 {
00139     _led = &led;
00140 }