motordriver.cpp

00001 #include "motordriver.h"
00002 
00003 #include "mbed.h"
00004 
00005 Motor::Motor(PinName pwm, PinName fwd, PinName rev, int brakeable):
00006         _pwm(pwm), _fwd(fwd), _rev(rev) {
00007 
00008     // Set initial condition of PWM
00009     _pwm.period(0.001);
00010     _pwm = 0;
00011 
00012     // Initial condition of output enables
00013     _fwd = 0;
00014     _rev = 0;
00015 
00016     //set if the motor dirver is capable of braking. (addition)
00017     Brakeable= brakeable;
00018     sign = 0;//i.e nothing.
00019 }
00020 
00021 float Motor::speed(float speed) {
00022     float temp = 0;
00023     if (sign == 0) {
00024         _fwd = (speed > 0.0);
00025         _rev = (speed < 0.0);
00026         temp = abs(speed);
00027         _pwm = temp;
00028     } else if (sign == 1) {
00029         if (speed < 0) {
00030             _fwd = (speed > 0.0);
00031             _rev = (speed < 0.0);
00032             _pwm = 0;
00033             temp = 0;
00034        } else {
00035             _fwd = (speed > 0.0);
00036             _rev = (speed < 0.0);
00037             temp = abs(speed);
00038             _pwm = temp;
00039         }
00040     } else if (sign == -1) {
00041         if (speed > 0) {
00042             _fwd = (speed > 0.0);
00043             _rev = (speed < 0.0);
00044             _pwm = 0;
00045             temp = 0;
00046         } else {
00047             _fwd = (speed > 0.0);
00048             _rev = (speed < 0.0);
00049             temp = abs(speed);
00050             _pwm = temp;
00051         }
00052     }
00053     if (speed > 0)
00054         sign = 1;
00055     else if (speed < 0) {
00056         sign = -1;
00057     } else if (speed == 0) {
00058         sign = 0;
00059     }
00060     return temp;
00061 }
00062 //  (additions)
00063 void Motor::coast(void) {
00064     _fwd = 0;
00065     _rev = 0;
00066     _pwm = 0;
00067     sign = 0;
00068 }
00069 
00070 float Motor::stop(float duty) {
00071     if (Brakeable == 1) {
00072         _fwd = 1;
00073         _rev = 1;
00074         _pwm = duty;
00075         sign = 0;
00076         return duty;
00077     } else
00078         Motor::coast();
00079         return -1;
00080 }
00081 
00082 float Motor::state(void) {
00083     if ((_fwd == _rev) && (_pwm > 0)) {
00084         return -2;//braking
00085     } else if (_pwm == 0) {
00086         return 2;//coasting
00087     } else if ((_fwd == 0) && (_rev == 1)) {
00088         return -(_pwm);//reversing
00089     }  else if ((_fwd == 1) && (_rev == 0)) {
00090         return _pwm;//fowards
00091     } else
00092         return -3;//error
00093 }