Motorfader.cpp

00001 /* mbed Motorfader Library
00002  *  
00003  * Copyright (c) 2007-2010 sford, cstyles
00004  *
00005  * Permission is hereby granted, free of charge, to any person obtaining a copy
00006  * of this software and associated documentation files (the "Software"), to deal
00007  * in the Software without restriction, including without limitation the rights
00008  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00009  * copies of the Software, and to permit persons to whom the Software is
00010  * furnished to do so, subject to the following conditions:
00011  *
00012  * The above copyright notice and this permission notice shall be included in
00013  * all copies or substantial portions of the Software.
00014  *
00015  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00016  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00017  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00018  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00019  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00020  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00021  * THE SOFTWARE.
00022  */
00023  
00024 #include "Motorfader.h"
00025 #include "mbed.h"
00026 
00027 Motorfader::Motorfader(PinName anarogpin,PinName cwpin,PinName ccwpin) : _faderPos(anarogpin) ,_cwpwm(cwpin),_ccwpwm(ccwpin) {
00028     _cwpwm.period(0.020);
00029     _ccwpwm.period(0.020);
00030     _cwpwm.write(1.0);
00031     _ccwpwm.write(1.0);
00032     _setPos = 0.0;
00033     _setVel = 1.0;
00034     wait_ms(10);
00035 }
00036 
00037 void Motorfader::set(float pos,float vel ) {
00038     _setPos = pos;
00039     _setVel = vel;
00040     wait_ms(10);
00041 }
00042 
00043 float Motorfader::get() {
00044     return _faderPos.read();
00045 }
00046 
00047 void Motorfader::update() {
00048 float P,D = 0;
00049     _nowPos = _faderPos.read();
00050     _cwpwm.write(0.0);
00051     _ccwpwm.write(0.0);
00052 
00053     P = abs(_setPos - _nowPos)*4.0f;
00054     if (P >1.0f )
00055     { 
00056         P =1.0f;
00057 //        _oldPos = _nowPos;
00058     }
00059 //    D = -abs(_oldPos - _nowPos)*1.0f;
00060 //    _oldPos = _nowPos;
00061     if((_nowPos+0.03f) < _setPos)
00062     {
00063         _cwpwm.write(_setVel*(P+D));
00064         _ccwpwm.write(0.0f);
00065 //        printf("inc ");
00066     }
00067     if((_nowPos-0.03f) > _setPos)
00068     {
00069         _cwpwm.write(0.0f);
00070         _ccwpwm.write(_setVel*(P+D));
00071 //        printf("dec ");
00072     }
00073 //        printf("nowpos=%1.3f setpos==%1.3f\r\n",_nowPos,_setPos);
00074 
00075 }