j k / ZumoControl

ZumoにFRDM KL-25Zを搭載した時のモーターコントロールライブラリです。 PWM用の端子が互換していないので、割り込みでパルスを作っています。 デフォルトは100Hzの10段階になっています。

ZumoControl.cpp@0:da217aaa04ef, 2013-07-09 (annotated)

Committer:
jksoft_mbed
Date:
Tue Jul 09 12:43:08 2013 +0000
Revision:
0:da217aaa04ef
??

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jksoft_mbed 0:da217aaa04ef 1 /* Zumo motor control Library
jksoft_mbed 0:da217aaa04ef 2 *
jksoft_mbed 0:da217aaa04ef 3 * Copyright (c) 2010-2013 jksoft
jksoft_mbed 0:da217aaa04ef 4 *
jksoft_mbed 0:da217aaa04ef 5 * Permission is hereby granted, free of charge, to any person obtaining a copy
jksoft_mbed 0:da217aaa04ef 6 * of this software and associated documentation files (the "Software"), to deal
jksoft_mbed 0:da217aaa04ef 7 * in the Software without restriction, including without limitation the rights
jksoft_mbed 0:da217aaa04ef 8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
jksoft_mbed 0:da217aaa04ef 9 * copies of the Software, and to permit persons to whom the Software is
jksoft_mbed 0:da217aaa04ef 10 * furnished to do so, subject to the following conditions:
jksoft_mbed 0:da217aaa04ef 11 *
jksoft_mbed 0:da217aaa04ef 12 * The above copyright notice and this permission notice shall be included in
jksoft_mbed 0:da217aaa04ef 13 * all copies or substantial portions of the Software.
jksoft_mbed 0:da217aaa04ef 14 *
jksoft_mbed 0:da217aaa04ef 15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
jksoft_mbed 0:da217aaa04ef 16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
jksoft_mbed 0:da217aaa04ef 17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
jksoft_mbed 0:da217aaa04ef 18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
jksoft_mbed 0:da217aaa04ef 19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
jksoft_mbed 0:da217aaa04ef 20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
jksoft_mbed 0:da217aaa04ef 21 * THE SOFTWARE.
jksoft_mbed 0:da217aaa04ef 22 */
jksoft_mbed 0:da217aaa04ef 23
jksoft_mbed 0:da217aaa04ef 24 #include "mbed.h"
jksoft_mbed 0:da217aaa04ef 25 #include "ZumoControl.h"
jksoft_mbed 0:da217aaa04ef 26
jksoft_mbed 0:da217aaa04ef 27 ZumoControl::ZumoControl(PinName rc,PinName rp,PinName lc,PinName lp) : _rc(rc),_rp(rp),_lc(lc),_lp(lp) {
jksoft_mbed 0:da217aaa04ef 28 _pwm.attach_us( this , &ZumoControl::cycle , MINPULSWIDTH );
jksoft_mbed 0:da217aaa04ef 29 pwm_count = 0;
jksoft_mbed 0:da217aaa04ef 30 pwm_count_max = (int)(1000000.0f/(PERIOD*MINPULSWIDTH));
jksoft_mbed 0:da217aaa04ef 31
jksoft_mbed 0:da217aaa04ef 32 pwm_set_r = 0;
jksoft_mbed 0:da217aaa04ef 33 pwm_set_l = 0;
jksoft_mbed 0:da217aaa04ef 34
jksoft_mbed 0:da217aaa04ef 35 value_r = 0;
jksoft_mbed 0:da217aaa04ef 36 value_l = 0;
jksoft_mbed 0:da217aaa04ef 37 }
jksoft_mbed 0:da217aaa04ef 38
jksoft_mbed 0:da217aaa04ef 39 ZumoControl::ZumoControl() : _rc(PTC9),_rp(PTD5),_lc(PTA13),_lp(PTD0) {
jksoft_mbed 0:da217aaa04ef 40 _pwm.attach_us( this , &ZumoControl::cycle , MINPULSWIDTH );
jksoft_mbed 0:da217aaa04ef 41 pwm_count = 0;
jksoft_mbed 0:da217aaa04ef 42 pwm_count_max = (int)(1000000.0f/(PERIOD*MINPULSWIDTH));
jksoft_mbed 0:da217aaa04ef 43
jksoft_mbed 0:da217aaa04ef 44 pwm_set_r = 0;
jksoft_mbed 0:da217aaa04ef 45 pwm_set_l = 0;
jksoft_mbed 0:da217aaa04ef 46
jksoft_mbed 0:da217aaa04ef 47 value_r = 0;
jksoft_mbed 0:da217aaa04ef 48 value_l = 0;
jksoft_mbed 0:da217aaa04ef 49 }
jksoft_mbed 0:da217aaa04ef 50
jksoft_mbed 0:da217aaa04ef 51 void ZumoControl::cycle(void)
jksoft_mbed 0:da217aaa04ef 52 {
jksoft_mbed 0:da217aaa04ef 53 pwm_count++;
jksoft_mbed 0:da217aaa04ef 54
jksoft_mbed 0:da217aaa04ef 55 if( pwm_count >= pwm_count_max )
jksoft_mbed 0:da217aaa04ef 56 {
jksoft_mbed 0:da217aaa04ef 57 pwm_count = 0;
jksoft_mbed 0:da217aaa04ef 58 value_r = 1;
jksoft_mbed 0:da217aaa04ef 59 value_l = 1;
jksoft_mbed 0:da217aaa04ef 60 }
jksoft_mbed 0:da217aaa04ef 61
jksoft_mbed 0:da217aaa04ef 62 if(pwm_count >= pwm_set_r )
jksoft_mbed 0:da217aaa04ef 63 {
jksoft_mbed 0:da217aaa04ef 64 value_r = 0;
jksoft_mbed 0:da217aaa04ef 65 }
jksoft_mbed 0:da217aaa04ef 66 if(pwm_count >= pwm_set_l )
jksoft_mbed 0:da217aaa04ef 67 {
jksoft_mbed 0:da217aaa04ef 68 value_l = 0;
jksoft_mbed 0:da217aaa04ef 69 }
jksoft_mbed 0:da217aaa04ef 70
jksoft_mbed 0:da217aaa04ef 71 _rp = value_r;
jksoft_mbed 0:da217aaa04ef 72 _lp = value_l;
jksoft_mbed 0:da217aaa04ef 73 }
jksoft_mbed 0:da217aaa04ef 74
jksoft_mbed 0:da217aaa04ef 75 void ZumoControl::left_motor (float speed) {
jksoft_mbed 0:da217aaa04ef 76 if( speed > 0 )
jksoft_mbed 0:da217aaa04ef 77 {
jksoft_mbed 0:da217aaa04ef 78 _lc = 0;
jksoft_mbed 0:da217aaa04ef 79 }
jksoft_mbed 0:da217aaa04ef 80 else
jksoft_mbed 0:da217aaa04ef 81 {
jksoft_mbed 0:da217aaa04ef 82 _lc = 1;
jksoft_mbed 0:da217aaa04ef 83 }
jksoft_mbed 0:da217aaa04ef 84 pwm_set_l = (int)((abs(speed) * 10.0)+0.5);
jksoft_mbed 0:da217aaa04ef 85 }
jksoft_mbed 0:da217aaa04ef 86
jksoft_mbed 0:da217aaa04ef 87 void ZumoControl::right_motor (float speed) {
jksoft_mbed 0:da217aaa04ef 88 if( speed > 0 )
jksoft_mbed 0:da217aaa04ef 89 {
jksoft_mbed 0:da217aaa04ef 90 _rc = 1;
jksoft_mbed 0:da217aaa04ef 91 }
jksoft_mbed 0:da217aaa04ef 92 else
jksoft_mbed 0:da217aaa04ef 93 {
jksoft_mbed 0:da217aaa04ef 94 _rc = 0;
jksoft_mbed 0:da217aaa04ef 95 }
jksoft_mbed 0:da217aaa04ef 96 pwm_set_r = (int)((abs(speed) * 10.0)+0.5);
jksoft_mbed 0:da217aaa04ef 97 }
jksoft_mbed 0:da217aaa04ef 98
jksoft_mbed 0:da217aaa04ef 99 void ZumoControl::forward (float speed) {
jksoft_mbed 0:da217aaa04ef 100 _lc = 0;
jksoft_mbed 0:da217aaa04ef 101 _rc = 1;
jksoft_mbed 0:da217aaa04ef 102 pwm_set_l = (int)((speed * 10.0)+0.5);
jksoft_mbed 0:da217aaa04ef 103 pwm_set_r = (int)((speed * 10.0)+0.5);
jksoft_mbed 0:da217aaa04ef 104 }
jksoft_mbed 0:da217aaa04ef 105
jksoft_mbed 0:da217aaa04ef 106 void ZumoControl::backward (float speed) {
jksoft_mbed 0:da217aaa04ef 107 _lc = 1;
jksoft_mbed 0:da217aaa04ef 108 _rc = 0;
jksoft_mbed 0:da217aaa04ef 109 pwm_set_l = (int)((speed * 10.0)+0.5);
jksoft_mbed 0:da217aaa04ef 110 pwm_set_r = (int)((speed * 10.0)+0.5);
jksoft_mbed 0:da217aaa04ef 111 }
jksoft_mbed 0:da217aaa04ef 112
jksoft_mbed 0:da217aaa04ef 113 void ZumoControl::left (float speed) {
jksoft_mbed 0:da217aaa04ef 114 _lc = 1;
jksoft_mbed 0:da217aaa04ef 115 _rc = 1;
jksoft_mbed 0:da217aaa04ef 116 pwm_set_l = (int)((speed * 10.0)+0.5);
jksoft_mbed 0:da217aaa04ef 117 pwm_set_r = (int)((speed * 10.0)+0.5);
jksoft_mbed 0:da217aaa04ef 118 }
jksoft_mbed 0:da217aaa04ef 119
jksoft_mbed 0:da217aaa04ef 120 void ZumoControl::right (float speed) {
jksoft_mbed 0:da217aaa04ef 121 _lc = 0;
jksoft_mbed 0:da217aaa04ef 122 _rc = 0;
jksoft_mbed 0:da217aaa04ef 123 pwm_set_l = (int)((speed * 10.0)+0.5);
jksoft_mbed 0:da217aaa04ef 124 pwm_set_r = (int)((speed * 10.0)+0.5);
jksoft_mbed 0:da217aaa04ef 125 }
jksoft_mbed 0:da217aaa04ef 126
jksoft_mbed 0:da217aaa04ef 127 void ZumoControl::stop (void) {
jksoft_mbed 0:da217aaa04ef 128 pwm_set_l = 0;
jksoft_mbed 0:da217aaa04ef 129 pwm_set_r = 0;
jksoft_mbed 0:da217aaa04ef 130 }