motor.cpp

00001 #include "motor.h"
00002 
00003 Motor::Motor(PinName p, PinName d, PinName e, char* n, float Tcs, int cw/*, InterruptIn *c, InterruptIn *cc*/):
00004     _pulse(p),
00005     _dir(d),
00006     _error(e),
00007     _TXcm_s(Tcs),
00008     _name(n),
00009     _cw(cw),
00010     _ccw(1-cw),
00011     _lock(false)/*,
00012     _clockStop(c),
00013     _counterClockStop(cc)*/
00014 {
00015     _pulse = 0;
00016     _dir = 0;
00017     _error = 1;
00018     _pos = 0;
00019     setSpeed(1);
00020 }
00021 
00022 void Motor::pwm_io(int p_us, float dc)
00023 {
00024     _pulseTimeout.detach();
00025     if ((p_us == 0) || (dc == 0)) {
00026         return;
00027     }
00028     if (dc >= 1) {
00029         return;
00030     }
00031     _pulse = 0;
00032     _dir = 0;
00033     _error = 1;
00034     _onDelay = (int)(p_us * dc);
00035     _offDelay = p_us - _onDelay;
00036     printf("on delay: %dus\r\n_offDelay: %dus\r\n", _onDelay, _offDelay);
00037     toggleOn();
00038 }
00039 
00040 void Motor::toggleOn(void)
00041 {
00042 //    core_util_critical_section_enter();
00043     _pulse = 1;
00044 //    if(*_clockStop == 0 || *_counterClockStop == 0) return;
00045     _pulseTimeout.attach_us(this, &Motor::toggleOff, _onDelay);
00046 //    core_util_critical_section_exit();
00047 }
00048 
00049 void Motor::toggleOff(void)
00050 {
00051 //    core_util_critical_section_enter();
00052     _pulse = 0;
00053     _pulseTimeout.attach_us(this, &Motor::toggleOn, _offDelay);
00054 //    core_util_critical_section_exit();
00055 }
00056 
00057 void Motor::stop(void)
00058 {
00059 //    core_util_critical_section_enter();
00060     _pulseTimeout.detach();
00061     _rotateTimeout.detach();
00062     _lock=false;
00063 //    core_util_critical_section_exit();
00064 }
00065 void Motor::go(int direction)
00066 {
00067 //    core_util_critical_section_enter();
00068     if(_lock) {
00069         return;
00070     }
00071     _lock=true;
00072     _pulseTimeout.detach();
00073     _pulse = 0;
00074     _dir = (direction?_ccw:_cw);
00075     _error = 1;
00076     wait(0.001);
00077 //    core_util_critical_section_exit();
00078     toggleOn();
00079     wait(0.1);
00080 //    _rotateTimeout.attach(callback(this,&Motor::rotateCallBack), 0.1);
00081 //    core_util_critical_section_enter();
00082     float d=speed()*0.1;
00083     if(direction == MOTOR_SIDE) _pos += d;
00084     else _pos -= d;
00085     _pulseTimeout.detach();
00086     _lock=false;
00087 //    core_util_critical_section_exit();
00088 }
00089 
00090 void Motor::to(float p)
00091 {
00092 //    core_util_critical_section_enter();
00093     if(_lock) {
00094         return;
00095     }
00096     if (p==_pos) return;
00097     _lock=true;
00098     _goal = p;
00099     float time=(p-_pos)/speed();
00100     int direction=(time>0)?MOTOR_SIDE:NON_MOTOR_SIDE;
00101     if(time<0) time=-time;
00102     _pulseTimeout.detach();
00103     _pulse = 0;
00104     _dir = (direction?_ccw:_cw);
00105     _error = 1;
00106     wait(0.001);
00107 //    core_util_critical_section_exit();
00108     toggleOn();
00109     
00110     //wait(time);
00111     _rotateTimeout.attach(callback(this, &Motor::rotateCallBack), time);
00112 //    core_util_critical_section_enter();
00113 //    _pulseTimeout.detach();
00114 //    _pos=p;
00115 //    _lock=false;
00116 //    core_util_critical_section_exit();
00117 //    _goal=p;
00118 //    _pulseTimeout.attach(this, &Motor::reach_goal, time);
00119 }
00120 
00121 void Motor::setSpeed(float cm_s)
00122 {
00123     if(_lock) {
00124         return;
00125     }
00126     _lock=true;
00127     float T=_TXcm_s/cm_s;
00128     _onDelay=(int)T/2;
00129     _offDelay=T-_onDelay;
00130     _lock=false;
00131 }
00132 
00133 void Motor::rotateCallBack(void)
00134 {
00135     _pulseTimeout.detach();
00136     _pos = _goal;
00137     _lock = false;
00138     _goal = 0;
00139 }
00140 
00141 CD_2D34M::CD_2D34M(char* n, PinName d, PinName p, PinName a, PinName e/*, InterruptIn *c, InterruptIn *cc*/):
00142     Motor(p,d,e,n,CD_2D34M__TxCM_S,CD_2D34M__CW/*, c, cc*/), alarm(a)
00143 {
00144 #ifdef ALARM
00145     alarm.fall(this, &CD_2D34M::alarm_event);
00146 #endif
00147     //alarm.fall(this, &CD_2D34M::alarm_reset);
00148 }
00149 void CD_2D34M::alarm_event()
00150 {
00151     _error = 0;
00152     wait(0.01);
00153     alarm_reset();
00154 }
00155 void CD_2D34M::alarm_reset()
00156 {
00157     _error = 1;
00158     wait(0.01);
00159 }
00160 
00161 CD_2D24MB::CD_2D24MB(char* n, PinName p, PinName d, PinName e/*, InterruptIn *c, InterruptIn *cc*/):
00162     Motor(p,d,e,n,CD_2D24MB__TxCM_S,CD_2D24MB__CW/*, c, cc*/)
00163 {}