涂 桂旺 / Mbed OS car_scr

test

Dependencies:   ESP8266 HCSR04 PID

Fork of car_test_v1 by 涂 桂旺

MotorDriver/MotorDriver.cpp@3:9e51de1050a1, 2017-11-25 (annotated)

Committer:
tgw
Date:
Sat Nov 25 03:36:58 2017 +0000
Revision:
3:9e51de1050a1
test

Who changed what in which revision?

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