ZumoControl.cpp

00001 /* Zumo motor control Library
00002  *
00003  * Copyright (c) 2010-2013 jksoft
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 "mbed.h"
00025 #include "ZumoControl.h"
00026 
00027 ZumoControl::ZumoControl(PinName rc,PinName rp,PinName lc,PinName lp) : _rc(rc),_rp(rp),_lc(lc),_lp(lp)  {
00028     _pwm.attach_us( this  , &ZumoControl::cycle , MINPULSWIDTH );
00029     pwm_count = 0;
00030     pwm_count_max = (int)(1000000.0f/(PERIOD*MINPULSWIDTH));
00031     
00032     pwm_set_r = 0;
00033     pwm_set_l = 0;
00034     
00035     value_r = 0;
00036     value_l = 0;
00037 }
00038 
00039 ZumoControl::ZumoControl() : _rc(PTC9),_rp(PTD5),_lc(PTA13),_lp(PTD0)  {
00040     _pwm.attach_us( this  , &ZumoControl::cycle , MINPULSWIDTH );
00041     pwm_count = 0;
00042     pwm_count_max = (int)(1000000.0f/(PERIOD*MINPULSWIDTH));
00043     
00044     pwm_set_r = 0;
00045     pwm_set_l = 0;
00046     
00047     value_r = 0;
00048     value_l = 0;
00049 }
00050 
00051 void ZumoControl::cycle(void)
00052 {
00053     pwm_count++;
00054     
00055     if( pwm_count >= pwm_count_max )
00056     {
00057         pwm_count = 0;
00058         value_r = 1;
00059         value_l = 1;
00060     }
00061     
00062     if(pwm_count >= pwm_set_r )
00063     {
00064         value_r = 0;
00065     }
00066     if(pwm_count >= pwm_set_l )
00067     {
00068         value_l = 0;
00069     }
00070     
00071     _rp = value_r;
00072     _lp = value_l;
00073 }
00074 
00075 void ZumoControl::left_motor (float speed) {
00076     if( speed > 0 )
00077     {
00078         _lc = 0;
00079     }
00080     else
00081     {
00082         _lc = 1;
00083     }
00084     pwm_set_l = (int)((abs(speed) * 10.0)+0.5);
00085 }
00086 
00087 void ZumoControl::right_motor (float speed) {
00088     if( speed > 0 )
00089     {
00090         _rc = 1;
00091     }
00092     else
00093     {
00094         _rc = 0;
00095     }
00096     pwm_set_r = (int)((abs(speed) * 10.0)+0.5);
00097 }
00098 
00099 void ZumoControl::forward (float speed) {
00100     _lc = 0;
00101     _rc = 1;
00102     pwm_set_l = (int)((speed * 10.0)+0.5);
00103     pwm_set_r = (int)((speed * 10.0)+0.5);
00104 }
00105 
00106 void ZumoControl::backward (float speed) {
00107     _lc = 1;
00108     _rc = 0;
00109     pwm_set_l = (int)((speed * 10.0)+0.5);
00110     pwm_set_r = (int)((speed * 10.0)+0.5);
00111 }
00112 
00113 void ZumoControl::left (float speed) {
00114     _lc = 1;
00115     _rc = 1;
00116     pwm_set_l = (int)((speed * 10.0)+0.5);
00117     pwm_set_r = (int)((speed * 10.0)+0.5);
00118 }
00119 
00120 void ZumoControl::right (float speed) {
00121     _lc = 0;
00122     _rc = 0;
00123     pwm_set_l = (int)((speed * 10.0)+0.5);
00124     pwm_set_r = (int)((speed * 10.0)+0.5);
00125 }
00126 
00127 void ZumoControl::stop (void) {
00128     pwm_set_l = 0;
00129     pwm_set_r = 0;
00130 }