ZumoにFRDM KL-25Zを搭載した時のモーターコントロールライブラリです。 PWM用の端子が互換していないので、割り込みでパルスを作っています。 デフォルトは100Hzの10段階になっています。
Revision 0:da217aaa04ef, committed 2013-07-09
- Comitter:
- jksoft_mbed
- Date:
- Tue Jul 09 12:43:08 2013 +0000
- Commit message:
- ??
Changed in this revision
ZumoControl.cpp | Show annotated file Show diff for this revision Revisions of this file |
ZumoControl.h | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ZumoControl.cpp Tue Jul 09 12:43:08 2013 +0000 @@ -0,0 +1,130 @@ +/* Zumo motor control Library + * + * Copyright (c) 2010-2013 jksoft + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "mbed.h" +#include "ZumoControl.h" + +ZumoControl::ZumoControl(PinName rc,PinName rp,PinName lc,PinName lp) : _rc(rc),_rp(rp),_lc(lc),_lp(lp) { + _pwm.attach_us( this , &ZumoControl::cycle , MINPULSWIDTH ); + pwm_count = 0; + pwm_count_max = (int)(1000000.0f/(PERIOD*MINPULSWIDTH)); + + pwm_set_r = 0; + pwm_set_l = 0; + + value_r = 0; + value_l = 0; +} + +ZumoControl::ZumoControl() : _rc(PTC9),_rp(PTD5),_lc(PTA13),_lp(PTD0) { + _pwm.attach_us( this , &ZumoControl::cycle , MINPULSWIDTH ); + pwm_count = 0; + pwm_count_max = (int)(1000000.0f/(PERIOD*MINPULSWIDTH)); + + pwm_set_r = 0; + pwm_set_l = 0; + + value_r = 0; + value_l = 0; +} + +void ZumoControl::cycle(void) +{ + pwm_count++; + + if( pwm_count >= pwm_count_max ) + { + pwm_count = 0; + value_r = 1; + value_l = 1; + } + + if(pwm_count >= pwm_set_r ) + { + value_r = 0; + } + if(pwm_count >= pwm_set_l ) + { + value_l = 0; + } + + _rp = value_r; + _lp = value_l; +} + +void ZumoControl::left_motor (float speed) { + if( speed > 0 ) + { + _lc = 0; + } + else + { + _lc = 1; + } + pwm_set_l = (int)((abs(speed) * 10.0)+0.5); +} + +void ZumoControl::right_motor (float speed) { + if( speed > 0 ) + { + _rc = 1; + } + else + { + _rc = 0; + } + pwm_set_r = (int)((abs(speed) * 10.0)+0.5); +} + +void ZumoControl::forward (float speed) { + _lc = 0; + _rc = 1; + pwm_set_l = (int)((speed * 10.0)+0.5); + pwm_set_r = (int)((speed * 10.0)+0.5); +} + +void ZumoControl::backward (float speed) { + _lc = 1; + _rc = 0; + pwm_set_l = (int)((speed * 10.0)+0.5); + pwm_set_r = (int)((speed * 10.0)+0.5); +} + +void ZumoControl::left (float speed) { + _lc = 1; + _rc = 1; + pwm_set_l = (int)((speed * 10.0)+0.5); + pwm_set_r = (int)((speed * 10.0)+0.5); +} + +void ZumoControl::right (float speed) { + _lc = 0; + _rc = 0; + pwm_set_l = (int)((speed * 10.0)+0.5); + pwm_set_r = (int)((speed * 10.0)+0.5); +} + +void ZumoControl::stop (void) { + pwm_set_l = 0; + pwm_set_r = 0; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ZumoControl.h Tue Jul 09 12:43:08 2013 +0000 @@ -0,0 +1,137 @@ +/* mbed Zumo motor control Library + * Copyright (c) 2010-2013 jksoft + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifndef ZUMOCONTROL_H +#define ZUMOCONTROL_H + +#include "mbed.h" + +#define PERIOD 100.0f // Hz +#define MINPULSWIDTH 1000.0f // us + +/** Zumo motor control class + * + * Example: + * @code + * // Drive the Zumo forward, turn left, back, turn right, at half speed for half a second + + #include "mbed.h" + #include "ZumoControl.h" + + ZumoControl zumo_ctrl; + + int main() { + + wait(0.5); + + zumo_ctrl.forward(0.5); + wait (0.5); + zumo_ctrl.left(0.5); + wait (0.5); + zumo_ctrl.backward(0.5); + wait (0.5); + zumo_ctrl.right(0.5); + wait (0.5); + + zumo_ctrl.stop(); + + } + * @endcode + */ +class ZumoControl { + + // Public functions +public: + + /** Create the Zumo object connected to the default pins + * + * @param rc GPIO pin used for Right motor control. Default is PTC9 + * @param rp GPIO pin used for Right motor PWM. Default is PTD5 + * @param lc GPIO pin used for Left motor control. Default is PTA13 + * @param lp GPIO pin used for Left motor PWM. Default is PTD0 + */ + ZumoControl(); + + + /** Create the Zumo object connected to specific pins + * + */ + ZumoControl(PinName rc,PinName rp,PinName lc,PinName lp); + + /** Directly control the speed and direction of the left motor + * + * @param speed A normalised number -1.0 - 1.0 represents the full range. + */ + void left_motor (float speed); + + /** Directly control the speed and direction of the right motor + * + * @param speed A normalised number -1.0 - 1.0 represents the full range. + */ + void right_motor (float speed); + + /** Drive both motors forward as the same speed + * + * @param speed A normalised number 0 - 1.0 represents the full range. + */ + void forward (float speed); + + /** Drive both motors backward as the same speed + * + * @param speed A normalised number 0 - 1.0 represents the full range. + */ + void backward (float speed); + + /** Drive left motor backwards and right motor forwards at the same speed to turn on the spot + * + * @param speed A normalised number 0 - 1.0 represents the full range. + */ + void left (float speed); + + /** Drive left motor forward and right motor backwards at the same speed to turn on the spot + * @param speed A normalised number 0 - 1.0 represents the full range. + */ + void right (float speed); + + /** Stop both motors + * + */ + void stop (void); + + +private : + + void cycle(void); + + DigitalOut _rc; + DigitalOut _rp; + DigitalOut _lc; + DigitalOut _lp; + + int pwm_set_r,pwm_set_l; + int pwm_count,pwm_count_max; + int value_r,value_l; + + Ticker _pwm; +}; + +#endif \ No newline at end of file