p kj / Mbed 2 deprecated MKS22-CubeFine

LPC824

Dependencies:   mbed

Fork of CubeFine by wu le

Microduino_Motor.cpp@1:54a2d380f8c7, 2016-07-01 (annotated)

Committer:
beian10
Date:
Fri Jul 01 09:41:15 2016 +0000
Revision:
1:54a2d380f8c7
Parent:
0:362c1482232c
Child:
2:7964622fb5a5
cube

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