BuddyBotNucleoCode/Motor.cpp at master · mbparks/BuddyBotNucleoCode · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
#include "Motor.h"
#include "mbed.h"

//-----------------------------------------
//Public Function:  Constructor
//Inputs: 4 integers corresponding to the
//4 pins that are driving the L293D
//   Motor 1 + terminal
//   Motor 2 + terminal
//   Motor 1 - terminal
//   Motor 2 - terminal
//
//Returns: None
//-----------------------------------------
Motor::Motor(PinName hMotor1A, PinName hMotor2A, PinName hMotor1B, PinName hMotor2B)
    : _hMotor1A(hMotor1A)
    , _hMotor2A(hMotor2A)
    , _hMotor1B(hMotor1B)
    , _hMotor2B(hMotor2B)
    {
        _motorOn = 1;
        _motorOff = 0;

        _motorOnDuration = 0.02;

        _numMotorPulseCycleStandard = 5;
        _numMotorPulseCycleMax = 10;
}


//-----------------------------------------
//Public Function:  moveLeft
//Desctiption: Calls hMoveCW to turn motor
//to the left
//Inputs: none
//Returns: none
//-----------------------------------------
void Motor::moveLeft() {
    hMoveCW(_numMotorPulseCycleStandard);
}


//-----------------------------------------
//Public Function:  moveRight
//Desctiption: Calls hMoveCCW to turn motor
//to the left
//Inputs: none
//Returns: none
//-----------------------------------------
void Motor::moveRight() {
    hMoveCCW(_numMotorPulseCycleStandard);
}


//-----------------------------------------
//Private Function:  hMoveCW
//Desctiption: Pulse motor in sequence to
//cause clockwise motion, repeats as many
//times as input numPulseCycles
//Inputs: int numPulseCycles
//Returns: none
//-----------------------------------------
void Motor::hMoveCW(int numPulseCycles) {
    for (int x=0; x<numPulseCycles; x++) {
    _hMotor1A=_motorOn;
    _hMotor2A=_motorOn;
    _hMotor1B=_motorOff;
    _hMotor2B=_motorOff;
    wait(_motorOnDuration);

    _hMotor1A=_motorOff;
    _hMotor2A=_motorOn;
    _hMotor1B=_motorOn;
    _hMotor2B=_motorOff;
    wait(_motorOnDuration);

    _hMotor1A=_motorOff;
    _hMotor2A=_motorOff;
    _hMotor1B=_motorOn;
    _hMotor2B=_motorOn;
    wait(_motorOnDuration);

    _hMotor1A=_motorOn;
    _hMotor2A=_motorOff;
    _hMotor1B=_motorOff;
    _hMotor2B=_motorOn;
    wait(_motorOnDuration); }
}


//-----------------------------------------
//Private Function:  hMoveCCW
//Desctiption: Pulse motor in sequence to
//cause counter-clockwise motion, repeats as many
//times as input numPulseCycles
//Inputs: int numPulseCycles
//Returns: none
//-----------------------------------------
void Motor::hMoveCCW(int numPulseCycles) {
    for (int x=0; x<numPulseCycles; x++) {
    _hMotor1A=_motorOn;
    _hMotor2A=_motorOff;
    _hMotor1B=_motorOff;
    _hMotor2B=_motorOn;
    wait(_motorOnDuration);

    _hMotor1A=_motorOff;
    _hMotor2A=_motorOff;
    _hMotor1B=_motorOn;
    _hMotor2B=_motorOn;
    wait(_motorOnDuration);

    _hMotor1A=_motorOff;
    _hMotor2A=_motorOn;
    _hMotor1B=_motorOn;
    _hMotor2B=_motorOff;
    wait(_motorOnDuration);

    _hMotor1A=_motorOn;
    _hMotor2A=_motorOn;
    _hMotor1B=_motorOff;
    _hMotor2B=_motorOff;
    wait(_motorOnDuration); }
}