RoboticsLearningPlatform/Labs/libraries/Motor/Motor.h at master · AF-ROBOTICS/RoboticsLearningPlatform · 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
/*
 *  Filename: drive.h
 *  Author: Capt Steven Beyer
 *  Created: 6 Mar 2019
 *  Description: Header file that provides functions to drive the
 *    DFECBot.
 *		L_DIR  = 2;  // direction b
 *		L_PWM  = 3;  // PWM at 490 Hz
 *		R_DIR  = 4;  // direction a
 *		R_PWM  = 5;  // PWM at 980 Hz

 *
 *  Required Files:
 *    Libraries : none
 *    Packages  : none
 *    Files     : none
 */


const int L_DIR  = 2;  // direction b
const int L_PWM  = 3;  // PWM at 490 Hz
const int R_DIR  = 4;  // direction a
const int R_PWM  = 5;  // PWM at 980 Hz

// ------------Motor_Init------------
// Initialize GPIO pins
// Used to control the direction of the motors and
// PWM signal applied to the motors.
// The motors are initially stopped
// Input: none
// Output: none
void Motor_Init(){
  // Initialize pins
  pinMode(R_PWM, OUTPUT);
  pinMode(R_DIR, OUTPUT);
  pinMode(L_DIR, OUTPUT);
  pinMode(L_PWM, OUTPUT);

  // stop the motors
  analogWrite(L_PWM, 0);
  analogWrite(R_PWM, 0);
}

// ------------Motor_Stop------------
// Stop the motors, power down the drivers, and
// set the PWM speed control to 0% duty cycle.
// Input: none
// Output: none
void Motor_Stop(){
  analogWrite(L_PWM, 0);
  analogWrite(R_PWM, 0);
}

// ------------Motor_Forward------------
// Drive the robot forward by running left and
// right wheels forward with the given duty
// cycles.
// Input: leftDuty  duty cycle of left wheel (0 to 255)
//        rightDuty duty cycle of right wheel (0 to 255)
// Output: none
// Assumes: Motor_Init() has been called
void Motor_Forward(int leftDuty, int rightDuty){
  // Set both motors forward
  digitalWrite(L_DIR,HIGH);
  digitalWrite(R_DIR,HIGH);

  // set PWM for each motor
  analogWrite(L_PWM, leftDuty);
  analogWrite(R_PWM, rightDuty);
}

// ------------Motor_Right------------
// Turn the robot to the right by running the
// left wheel forward and the right wheel
// backward with the given duty cycles.
// Input: leftDuty  duty cycle of left wheel (0 to 255)
//        rightDuty duty cycle of right wheel (0 to 255)
// Output: none
// Assumes: Motor_Init() has been called
void Motor_Right(int leftDuty, int rightDuty){
  // Set left wheel forward and right wheel backward
  digitalWrite(L_DIR,HIGH);
  digitalWrite(R_DIR,LOW);

  // set PWM for each motor
  analogWrite(L_PWM, leftDuty);
  analogWrite(R_PWM, rightDuty);
}

// ------------Motor_Left------------
// Turn the robot to the left by running the
// left wheel backward and the right wheel
// forward with the given duty cycles.
// Input: leftDuty  duty cycle of left wheel (0 to 255)
//        rightDuty duty cycle of right wheel (0 to 255)
// Output: none
// Assumes: Motor_Init() has been called
void Motor_Left(int leftDuty, int rightDuty){
  // Set left wheel backward and right wheel forward
  digitalWrite(L_DIR,LOW);
  digitalWrite(R_DIR,HIGH);

  // set PWM for each motor
  analogWrite(L_PWM, leftDuty);
  analogWrite(R_PWM, rightDuty);
}

// ------------Motor_Backward------------
// Drive the robot backward by running left and
// right wheels backward with the given duty
// cycles.
// Input: leftDuty  duty cycle of left wheel (0 to 255)
//        rightDuty duty cycle of right wheel (0 to 255)
// Output: none
// Assumes: Motor_Init() has been called
void Motor_Backward(int leftDuty, int rightDuty){
  // Set both wheels backward
  digitalWrite(L_DIR,LOW);
  digitalWrite(R_DIR,LOW);

  // set PWM for each motor
  analogWrite(L_PWM, leftDuty);
  analogWrite(R_PWM, rightDuty);
}