涂 桂旺 / MotorDriver

test

Fork of MotorDriver by Bryce Williams

MotorDriver.cpp@1:19476b49822b, 2017-11-25 (annotated)

Committer:
tgw
Date:
Sat Nov 25 02:02:40 2017 +0000
Revision:
1:19476b49822b
Parent:
0:871adb9cf798 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
electromotivated 0:871adb9cf798 1 #include "MotorDriver.h"
electromotivated 0:871adb9cf798 2 #include "mbed.h"
electromotivated 0:871adb9cf798 3 #include <cmath>
electromotivated 0:871adb9cf798 4
electromotivated 0:871adb9cf798 5 // See .h file for function descriptions and details
electromotivated 0:871adb9cf798 6
electromotivated 0:871adb9cf798 7 MotorDriver::MotorDriver(DigitalOut in1, DigitalOut in2, PwmOut pwm, float pwmFreq, bool isBrakeable)
electromotivated 0:871adb9cf798 8 : _in1(in1), _in2(in2), _pwm(pwm)
electromotivated 0:871adb9cf798 9 {
electromotivated 0:871adb9cf798 10 // Initialize motor and variables
electromotivated 0:871adb9cf798 11 _in1 = 1; _in2 = 0; _pwm = 0.0; this->isBrakeable = isBrakeable;
electromotivated 0:871adb9cf798 12 _pwm.period(1.0/pwmFreq);
electromotivated 0:871adb9cf798 13 determineState();
electromotivated 0:871adb9cf798 14 }
electromotivated 0:871adb9cf798 15
electromotivated 0:871adb9cf798 16 void MotorDriver::determineState(){
electromotivated 0:871adb9cf798 17 if(_in1==1 && _in2==1){
electromotivated 0:871adb9cf798 18 motor_state.code = BRAKING;
electromotivated 0:871adb9cf798 19 motor_state.value = _pwm;
electromotivated 0:871adb9cf798 20 }
electromotivated 0:871adb9cf798 21 else if(_in1==1 && _in2==0){
electromotivated 0:871adb9cf798 22 motor_state.code = DRIVING_CW;
electromotivated 0:871adb9cf798 23 motor_state.value = _pwm;
electromotivated 0:871adb9cf798 24 }
electromotivated 0:871adb9cf798 25 else if(_in1==0 && _in2==1){
electromotivated 0:871adb9cf798 26 motor_state.code = DRIVING_CCW;
electromotivated 0:871adb9cf798 27 motor_state.value = _pwm;
electromotivated 0:871adb9cf798 28 }
electromotivated 0:871adb9cf798 29 else if(_in1==0 && _in2==0){
electromotivated 0:871adb9cf798 30 motor_state.code = COASTING;
electromotivated 0:871adb9cf798 31 motor_state.value = _pwm;
electromotivated 0:871adb9cf798 32 }
electromotivated 0:871adb9cf798 33 else{
tgw 1:19476b49822b 34 motor_state.code = ERROR_M; // Undefined config found
electromotivated 0:871adb9cf798 35 motor_state.value = _pwm;
electromotivated 0:871adb9cf798 36 }
electromotivated 0:871adb9cf798 37 }
electromotivated 0:871adb9cf798 38
electromotivated 0:871adb9cf798 39 State_t MotorDriver::setSpeed(float speed){
electromotivated 0:871adb9cf798 40 // Prevent instantaneous reversal; put into brake if requested to do so
electromotivated 0:871adb9cf798 41 bool isTryingToInstantReverse =
electromotivated 0:871adb9cf798 42 (bool)((speed < 0.0) && (motor_state.code == DRIVING_CW) && (_pwm > 0.05)) ||
electromotivated 0:871adb9cf798 43 (bool)((speed > 0.0) && (motor_state.code == DRIVING_CCW)&& (_pwm > 0.05));
electromotivated 0:871adb9cf798 44 if(isTryingToInstantReverse){
electromotivated 0:871adb9cf798 45 // Set motor to brake, set state to error
electromotivated 0:871adb9cf798 46 coast();
tgw 1:19476b49822b 47 motor_state.code = ERROR_M; motor_state.value = _pwm;
electromotivated 0:871adb9cf798 48 return motor_state;
electromotivated 0:871adb9cf798 49 }
electromotivated 0:871adb9cf798 50 else{
electromotivated 0:871adb9cf798 51 if(speed == 0.0)
electromotivated 0:871adb9cf798 52 {
electromotivated 0:871adb9cf798 53 // No effect to _in pins
electromotivated 0:871adb9cf798 54 }
electromotivated 0:871adb9cf798 55 else{
electromotivated 0:871adb9cf798 56 _in1 = (speed>0.0);
electromotivated 0:871adb9cf798 57 _in2 = (speed<0.0);
electromotivated 0:871adb9cf798 58 }
electromotivated 0:871adb9cf798 59 _pwm = std::abs(speed);
electromotivated 0:871adb9cf798 60 determineState();
electromotivated 0:871adb9cf798 61 return motor_state;
electromotivated 0:871adb9cf798 62 }
electromotivated 0:871adb9cf798 63 }
electromotivated 0:871adb9cf798 64
electromotivated 0:871adb9cf798 65 State_t MotorDriver::forceSetSpeed(float speed){
electromotivated 0:871adb9cf798 66 if(speed == 0.0)
electromotivated 0:871adb9cf798 67 {
electromotivated 0:871adb9cf798 68 // No effect to _in pins
electromotivated 0:871adb9cf798 69 }
electromotivated 0:871adb9cf798 70 else{
electromotivated 0:871adb9cf798 71 _in1 = (speed>0.0);
electromotivated 0:871adb9cf798 72 _in2 = (speed<0.0);
electromotivated 0:871adb9cf798 73 }
electromotivated 0:871adb9cf798 74 _pwm = std::abs(speed);
electromotivated 0:871adb9cf798 75 determineState();
electromotivated 0:871adb9cf798 76 return motor_state;
electromotivated 0:871adb9cf798 77 }
electromotivated 0:871adb9cf798 78
electromotivated 0:871adb9cf798 79 State_t MotorDriver::brake(float intensity){
electromotivated 0:871adb9cf798 80 if(!isBrakeable) coast();
electromotivated 0:871adb9cf798 81 else{
electromotivated 0:871adb9cf798 82 _in1=1; _in2=1; _pwm = _pwm * (1.0-intensity);
electromotivated 0:871adb9cf798 83 }
electromotivated 0:871adb9cf798 84 determineState();
electromotivated 0:871adb9cf798 85 return motor_state;
electromotivated 0:871adb9cf798 86 }
electromotivated 0:871adb9cf798 87
electromotivated 0:871adb9cf798 88 State_t MotorDriver::coast(){
electromotivated 0:871adb9cf798 89 _in1=0; _in2=0; _pwm = 1.0;
electromotivated 0:871adb9cf798 90 determineState();
electromotivated 0:871adb9cf798 91 return motor_state;
electromotivated 0:871adb9cf798 92 }
electromotivated 0:871adb9cf798 93
electromotivated 0:871adb9cf798 94 State_t MotorDriver::getState(){
electromotivated 0:871adb9cf798 95 return motor_state;
electromotivated 0:871adb9cf798 96 }