Li Weiyi
/
Cube
Microduino的cube小车。
Embed:
(wiki syntax)
Show/hide line numbers
Microduino_Motor.cpp
00001 #include "Microduino_Motor.h" 00002 00003 static motor_t motors[10]; // static array of key structures 00004 00005 uint8_t MotorCount = 0; // the total number of attached keys 00006 00007 Motor::Motor(PinName _motor_pinA, PinName _motor_pinB) 00008 { 00009 if ( MotorCount < 10) { 00010 this->motorIndex = MotorCount++; // assign a key index to this instance 00011 //if (_motor_pinA < NUM_DIGITAL_PINS && _motor_pinB < NUM_DIGITAL_PINS) { 00012 if (true) { 00013 #if 0 00014 pinMode( _motor_pinA, OUTPUT) ; 00015 #else 00016 _period_us = 255; // 500Hz 00017 pwmout_init(&_pwmA, _motor_pinA); 00018 pwmout_period_us(&_pwmA, _period_us); // 500Hz 00019 pwmout_pulsewidth_us(&_pwmA, 1); 00020 #endif 00021 motors[this->motorIndex].Pin.nbr_A = _motor_pinA; 00022 00023 #if 0 00024 pinMode( _motor_pinB, OUTPUT) ; 00025 #else 00026 pwmout_init(&_pwmB, _motor_pinB); 00027 pwmout_period_us(&_pwmB, _period_us); // 500Hz 00028 pwmout_pulsewidth_us(&_pwmB, 1); 00029 #endif 00030 motors[this->motorIndex].Pin.nbr_B = _motor_pinB; 00031 00032 this->fix=1; 00033 } 00034 } else { 00035 this->motorIndex = 255 ; // too many keys 00036 } 00037 } 00038 00039 void Motor::Fix(float _fix) 00040 { 00041 this->fix=_fix; 00042 } 00043 00044 int16_t Motor::GetData(int16_t _throttle, int16_t _steering, uint8_t _dir) 00045 { 00046 this->_motor_vol = _throttle; 00047 00048 if(_dir == 1) 00049 { 00050 this->_motor_vol -= _steering / 2; 00051 } 00052 else 00053 { 00054 this->_motor_vol += _steering / 2; 00055 } 00056 if (this->_motor_vol > 255) 00057 this->_motor_vol = 255; 00058 else if (this->_motor_vol < -255) 00059 this->_motor_vol = -255; 00060 00061 //this->_motor_vol *= fix; 00062 00063 return this->_motor_vol; 00064 } 00065 00066 #if 0 00067 void Motor::Driver(int16_t _motor_driver) 00068 { 00069 int8_t channel_A = motors[this->motorIndex].Pin.nbr_A; 00070 int8_t channel_B = motors[this->motorIndex].Pin.nbr_B; 00071 if (_motor_driver == 0) { 00072 digitalWrite(channel_A, LOW); 00073 digitalWrite(channel_B, LOW); 00074 } else if (_motor_driver > 0) { 00075 analogWrite(channel_A, _motor_driver); 00076 digitalWrite(channel_B, LOW); 00077 } else { 00078 analogWrite(channel_A, 255 + _motor_driver); 00079 digitalWrite(channel_B, HIGH); 00080 } 00081 } 00082 #else 00083 void Motor::Driver(int16_t _motor_driver) 00084 { 00085 //static bool flag = true; 00086 uint32_t pulseWidth = 0; 00087 //PinName channel_A = motors[this->motorIndex].Pin.nbr_A; 00088 //PinName channel_B = motors[this->motorIndex].Pin.nbr_B; 00089 #if 0 00090 pwmout_pulsewidth_us(&_pwmA, _period_us/2); 00091 pwmout_pulsewidth_us(&_pwmB, 0); 00092 return; 00093 #endif 00094 #if 0 00095 pwmout_pulsewidth_us(&_pwmA, 0); 00096 pwmout_pulsewidth_us(&_pwmB, _period_us/2); 00097 return; 00098 #endif 00099 if (_motor_driver == 0) { 00100 pwmout_pulsewidth_us(&_pwmA, 0); 00101 pwmout_pulsewidth_us(&_pwmB, 0); 00102 } else if (_motor_driver > 0) { 00103 #if 1 00104 pulseWidth = _period_us / 255 * _motor_driver; 00105 pwmout_pulsewidth_us(&_pwmA, pulseWidth); 00106 pwmout_pulsewidth_us(&_pwmB, 2); 00107 #else 00108 pwmout_pulsewidth_us(&_pwmA, _period_us/2); 00109 pwmout_pulsewidth_us(&_pwmB, 0); 00110 #endif 00111 } else { 00112 #if 0 00113 _motor_driver = 255 + _motor_driver; 00114 pulseWidth = _period_us / 255 * _motor_driver; 00115 pwmout_pulsewidth_us(&_pwmA, 0); 00116 pwmout_pulsewidth_us(&_pwmB, pulseWidth); 00117 #elif 1 00118 _motor_driver = abs(_motor_driver); 00119 pulseWidth = _period_us / 255 * _motor_driver; 00120 pwmout_pulsewidth_us(&_pwmA, 2); 00121 pwmout_pulsewidth_us(&_pwmB, pulseWidth); 00122 #elif 0 00123 _motor_driver = 255 + _motor_driver; 00124 pulseWidth = _period_us / 255 * _motor_driver; 00125 pwmout_pulsewidth_us(&_pwmA, _period_us); 00126 pwmout_pulsewidth_us(&_pwmB, pulseWidth); 00127 #else 00128 pwmout_pulsewidth_us(&_pwmA, 0); 00129 pwmout_pulsewidth_us(&_pwmB, 241); 00130 #endif 00131 } 00132 } 00133 #endif 00134 00135 void Motor::Free() 00136 { 00137 int8_t channel_A = motors[this->motorIndex].Pin.nbr_A; 00138 int8_t channel_B = motors[this->motorIndex].Pin.nbr_B; 00139 #if 0 00140 digitalWrite(channel_A, LOW); 00141 digitalWrite(channel_B, LOW); 00142 #else 00143 pwmout_pulsewidth_us(&_pwmA, 0); 00144 pwmout_pulsewidth_us(&_pwmB, 0); 00145 #endif 00146 } 00147 00148 void Motor::Brake() 00149 { 00150 int8_t channel_A = motors[this->motorIndex].Pin.nbr_A; 00151 int8_t channel_B = motors[this->motorIndex].Pin.nbr_B; 00152 #if 0 00153 digitalWrite(channel_A, HIGH); 00154 digitalWrite(channel_B, HIGH); 00155 #else 00156 pwmout_pulsewidth_us(&_pwmA, _period_us); 00157 pwmout_pulsewidth_us(&_pwmB, _period_us); 00158 #endif 00159 }
Generated on Tue Jul 12 2022 20:44:32 by 1.7.2