p kj / Mbed 2 deprecated MKS22-CubeFine

LPC824

Dependencies:   mbed

Fork of CubeFine by wu le

Microduino_Motor.cpp@0:362c1482232c, 2016-06-02 (annotated)

Committer:
lixianyu
Date:
Thu Jun 02 04:03:31 2016 +0000
Revision:
0:362c1482232c
Child:
1:54a2d380f8c7
work fine.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lixianyu 0:362c1482232c 1 #include "Microduino_Motor.h"
lixianyu 0:362c1482232c 2
lixianyu 0:362c1482232c 3 static motor_t motors[10]; // static array of key structures
lixianyu 0:362c1482232c 4
lixianyu 0:362c1482232c 5 uint8_t MotorCount = 0; // the total number of attached keys
lixianyu 0:362c1482232c 6
lixianyu 0:362c1482232c 7 Motor::Motor(PinName _motor_pinA, PinName _motor_pinB)
lixianyu 0:362c1482232c 8 {
lixianyu 0:362c1482232c 9 if ( MotorCount < 10) {
lixianyu 0:362c1482232c 10 this->motorIndex = MotorCount++; // assign a key index to this instance
lixianyu 0:362c1482232c 11 //if (_motor_pinA < NUM_DIGITAL_PINS && _motor_pinB < NUM_DIGITAL_PINS) {
lixianyu 0:362c1482232c 12 if (true) {
lixianyu 0:362c1482232c 13 #if 0
lixianyu 0:362c1482232c 14 pinMode( _motor_pinA, OUTPUT) ;
lixianyu 0:362c1482232c 15 #else
lixianyu 0:362c1482232c 16 _period_us = 255; // 500Hz
lixianyu 0:362c1482232c 17 pwmout_init(&_pwmA, _motor_pinA);
lixianyu 0:362c1482232c 18 pwmout_period_us(&_pwmA, _period_us); // 500Hz
lixianyu 0:362c1482232c 19 pwmout_pulsewidth_us(&_pwmA, 1);
lixianyu 0:362c1482232c 20 #endif
lixianyu 0:362c1482232c 21 motors[this->motorIndex].Pin.nbr_A = _motor_pinA;
lixianyu 0:362c1482232c 22
lixianyu 0:362c1482232c 23 #if 0
lixianyu 0:362c1482232c 24 pinMode( _motor_pinB, OUTPUT) ;
lixianyu 0:362c1482232c 25 #else
lixianyu 0:362c1482232c 26 pwmout_init(&_pwmB, _motor_pinB);
lixianyu 0:362c1482232c 27 pwmout_period_us(&_pwmB, _period_us); // 500Hz
lixianyu 0:362c1482232c 28 pwmout_pulsewidth_us(&_pwmB, 1);
lixianyu 0:362c1482232c 29 #endif
lixianyu 0:362c1482232c 30 motors[this->motorIndex].Pin.nbr_B = _motor_pinB;
lixianyu 0:362c1482232c 31
lixianyu 0:362c1482232c 32 this->fix=1;
lixianyu 0:362c1482232c 33 }
lixianyu 0:362c1482232c 34 } else {
lixianyu 0:362c1482232c 35 this->motorIndex = 255 ; // too many keys
lixianyu 0:362c1482232c 36 }
lixianyu 0:362c1482232c 37 }
lixianyu 0:362c1482232c 38
lixianyu 0:362c1482232c 39 void Motor::Fix(float _fix)
lixianyu 0:362c1482232c 40 {
lixianyu 0:362c1482232c 41 this->fix=_fix;
lixianyu 0:362c1482232c 42 }
lixianyu 0:362c1482232c 43
lixianyu 0:362c1482232c 44 int16_t Motor::GetData(int16_t _throttle, int16_t _steering, uint8_t _dir)
lixianyu 0:362c1482232c 45 {
lixianyu 0:362c1482232c 46 this->_motor_vol = _throttle;
lixianyu 0:362c1482232c 47
lixianyu 0:362c1482232c 48 if(_dir == 1)
lixianyu 0:362c1482232c 49 {
lixianyu 0:362c1482232c 50 this->_motor_vol -= _steering / 2;
lixianyu 0:362c1482232c 51 }
lixianyu 0:362c1482232c 52 else
lixianyu 0:362c1482232c 53 {
lixianyu 0:362c1482232c 54 this->_motor_vol += _steering / 2;
lixianyu 0:362c1482232c 55 }
lixianyu 0:362c1482232c 56 if (this->_motor_vol > 255)
lixianyu 0:362c1482232c 57 this->_motor_vol = 255;
lixianyu 0:362c1482232c 58 else if (this->_motor_vol < -255)
lixianyu 0:362c1482232c 59 this->_motor_vol = -255;
lixianyu 0:362c1482232c 60
lixianyu 0:362c1482232c 61 //this->_motor_vol *= fix;
lixianyu 0:362c1482232c 62
lixianyu 0:362c1482232c 63 return this->_motor_vol;
lixianyu 0:362c1482232c 64 }
lixianyu 0:362c1482232c 65
lixianyu 0:362c1482232c 66 #if 0
lixianyu 0:362c1482232c 67 void Motor::Driver(int16_t _motor_driver)
lixianyu 0:362c1482232c 68 {
lixianyu 0:362c1482232c 69 int8_t channel_A = motors[this->motorIndex].Pin.nbr_A;
lixianyu 0:362c1482232c 70 int8_t channel_B = motors[this->motorIndex].Pin.nbr_B;
lixianyu 0:362c1482232c 71 if (_motor_driver == 0) {
lixianyu 0:362c1482232c 72 digitalWrite(channel_A, LOW);
lixianyu 0:362c1482232c 73 digitalWrite(channel_B, LOW);
lixianyu 0:362c1482232c 74 } else if (_motor_driver > 0) {
lixianyu 0:362c1482232c 75 analogWrite(channel_A, _motor_driver);
lixianyu 0:362c1482232c 76 digitalWrite(channel_B, LOW);
lixianyu 0:362c1482232c 77 } else {
lixianyu 0:362c1482232c 78 analogWrite(channel_A, 255 + _motor_driver);
lixianyu 0:362c1482232c 79 digitalWrite(channel_B, HIGH);
lixianyu 0:362c1482232c 80 }
lixianyu 0:362c1482232c 81 }
lixianyu 0:362c1482232c 82 #else
lixianyu 0:362c1482232c 83 void Motor::Driver(int16_t _motor_driver)
lixianyu 0:362c1482232c 84 {
lixianyu 0:362c1482232c 85 //static bool flag = true;
lixianyu 0:362c1482232c 86 uint32_t pulseWidth = 0;
lixianyu 0:362c1482232c 87 //PinName channel_A = motors[this->motorIndex].Pin.nbr_A;
lixianyu 0:362c1482232c 88 //PinName channel_B = motors[this->motorIndex].Pin.nbr_B;
lixianyu 0:362c1482232c 89 #if 0
lixianyu 0:362c1482232c 90 pwmout_pulsewidth_us(&_pwmA, _period_us/2);
lixianyu 0:362c1482232c 91 pwmout_pulsewidth_us(&_pwmB, 0);
lixianyu 0:362c1482232c 92 return;
lixianyu 0:362c1482232c 93 #endif
lixianyu 0:362c1482232c 94 #if 0
lixianyu 0:362c1482232c 95 pwmout_pulsewidth_us(&_pwmA, 0);
lixianyu 0:362c1482232c 96 pwmout_pulsewidth_us(&_pwmB, _period_us/2);
lixianyu 0:362c1482232c 97 return;
lixianyu 0:362c1482232c 98 #endif
lixianyu 0:362c1482232c 99 if (_motor_driver == 0) {
lixianyu 0:362c1482232c 100 pwmout_pulsewidth_us(&_pwmA, 0);
lixianyu 0:362c1482232c 101 pwmout_pulsewidth_us(&_pwmB, 0);
lixianyu 0:362c1482232c 102 } else if (_motor_driver > 0) {
lixianyu 0:362c1482232c 103 #if 1
lixianyu 0:362c1482232c 104 pulseWidth = _period_us / 255 * _motor_driver;
lixianyu 0:362c1482232c 105 pwmout_pulsewidth_us(&_pwmA, pulseWidth);
lixianyu 0:362c1482232c 106 pwmout_pulsewidth_us(&_pwmB, 2);
lixianyu 0:362c1482232c 107 #else
lixianyu 0:362c1482232c 108 pwmout_pulsewidth_us(&_pwmA, _period_us/2);
lixianyu 0:362c1482232c 109 pwmout_pulsewidth_us(&_pwmB, 0);
lixianyu 0:362c1482232c 110 #endif
lixianyu 0:362c1482232c 111 } else {
lixianyu 0:362c1482232c 112 #if 0
lixianyu 0:362c1482232c 113 _motor_driver = 255 + _motor_driver;
lixianyu 0:362c1482232c 114 pulseWidth = _period_us / 255 * _motor_driver;
lixianyu 0:362c1482232c 115 pwmout_pulsewidth_us(&_pwmA, 0);
lixianyu 0:362c1482232c 116 pwmout_pulsewidth_us(&_pwmB, pulseWidth);
lixianyu 0:362c1482232c 117 #elif 1
lixianyu 0:362c1482232c 118 _motor_driver = abs(_motor_driver);
lixianyu 0:362c1482232c 119 pulseWidth = _period_us / 255 * _motor_driver;
lixianyu 0:362c1482232c 120 pwmout_pulsewidth_us(&_pwmA, 2);
lixianyu 0:362c1482232c 121 pwmout_pulsewidth_us(&_pwmB, pulseWidth);
lixianyu 0:362c1482232c 122 #elif 0
lixianyu 0:362c1482232c 123 _motor_driver = 255 + _motor_driver;
lixianyu 0:362c1482232c 124 pulseWidth = _period_us / 255 * _motor_driver;
lixianyu 0:362c1482232c 125 pwmout_pulsewidth_us(&_pwmA, _period_us);
lixianyu 0:362c1482232c 126 pwmout_pulsewidth_us(&_pwmB, pulseWidth);
lixianyu 0:362c1482232c 127 #else
lixianyu 0:362c1482232c 128 pwmout_pulsewidth_us(&_pwmA, 0);
lixianyu 0:362c1482232c 129 pwmout_pulsewidth_us(&_pwmB, 241);
lixianyu 0:362c1482232c 130 #endif
lixianyu 0:362c1482232c 131 }
lixianyu 0:362c1482232c 132 }
lixianyu 0:362c1482232c 133 #endif
lixianyu 0:362c1482232c 134
lixianyu 0:362c1482232c 135 void Motor::Free()
lixianyu 0:362c1482232c 136 {
lixianyu 0:362c1482232c 137 int8_t channel_A = motors[this->motorIndex].Pin.nbr_A;
lixianyu 0:362c1482232c 138 int8_t channel_B = motors[this->motorIndex].Pin.nbr_B;
lixianyu 0:362c1482232c 139 #if 0
lixianyu 0:362c1482232c 140 digitalWrite(channel_A, LOW);
lixianyu 0:362c1482232c 141 digitalWrite(channel_B, LOW);
lixianyu 0:362c1482232c 142 #else
lixianyu 0:362c1482232c 143 pwmout_pulsewidth_us(&_pwmA, 0);
lixianyu 0:362c1482232c 144 pwmout_pulsewidth_us(&_pwmB, 0);
lixianyu 0:362c1482232c 145 #endif
lixianyu 0:362c1482232c 146 }
lixianyu 0:362c1482232c 147
lixianyu 0:362c1482232c 148 void Motor::Brake()
lixianyu 0:362c1482232c 149 {
lixianyu 0:362c1482232c 150 int8_t channel_A = motors[this->motorIndex].Pin.nbr_A;
lixianyu 0:362c1482232c 151 int8_t channel_B = motors[this->motorIndex].Pin.nbr_B;
lixianyu 0:362c1482232c 152 #if 0
lixianyu 0:362c1482232c 153 digitalWrite(channel_A, HIGH);
lixianyu 0:362c1482232c 154 digitalWrite(channel_B, HIGH);
lixianyu 0:362c1482232c 155 #else
lixianyu 0:362c1482232c 156 pwmout_pulsewidth_us(&_pwmA, _period_us);
lixianyu 0:362c1482232c 157 pwmout_pulsewidth_us(&_pwmB, _period_us);
lixianyu 0:362c1482232c 158 #endif
lixianyu 0:362c1482232c 159 }