-
Notifications
You must be signed in to change notification settings - Fork 0
/
Motor.h
153 lines (137 loc) · 3.31 KB
/
Motor.h
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
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
char motor_duty_= 127; // Defalt PWM 50%
char motor_init_=0; // Status initial
// *** Motor A *****
// PD0 ====> 1A
// PD1 ====> 1B
// PC2 ====> 1E (PWM1)
// *** Motor B *****
// PB1 ====> 2A
// PB2 ====> 2B
// PC1 ====> 2E (PWM2)
//****************************************************
//********** Initial Motor Function ******************
//****************************************************
void Motor_Init()
{
if (motor_init_==0) // First time ?
{
motor_init_=1; // Status
ANSELH.F0=0; // RB1 ==> Digital IO
ANSELH.F2=0; // RB2 ==> Digital IO
TRISB.F1=0; // Motor B 2A
TRISB.F2=0; // Motor B 2B
TRISD.F0=0; // Motor A 1A
TRISD.F1=0; // MOtor A 1B
Pwm1_Init(5000); // Initail PWM 1E
Pwm2_Init(5000); // Initail PWM 2E
}
}
//****************************************************
//****************************************************
//********** Control Duty Cycle *********************
//****************************************************
void Change_Duty(char speed)
{
if (speed != motor_duty_) // Check Same old speed
{
motor_duty_=speed; // Save for old speed
Pwm1_Change_Duty(speed); // Motor A
Pwm2_Change_Duty(speed); // Motor B
}
}
//****************************************************
/********** Motor A Forward ********/
void Motor_A_FWD()
{
Pwm1_Start();
PORTD.F0 =0;
PORTD.F1 =1;
}
/************************************/
/********** Motor B Forward ********/
void Motor_B_FWD()
{
Pwm2_Start();
PORTB.F1 =0;
PORTB.F2 =1;
}
/************************************/
/********** Motor A Backward *******/
void Motor_A_BWD()
{
Pwm1_Start();
PORTD.F0 =1;
PORTD.F1 =0;
}
/************************************/
/********** Motor B Backward *******/
void Motor_B_BWD()
{
Pwm2_Start();
PORTB.F1 =1;
PORTB.F2 =0;
}
/************************************/
/********** Go Forward ************/
void Forward(char speed)
{
Motor_Init();
Change_Duty(speed);
Motor_A_FWD();
Motor_B_FWD();
}
/************************************/
/********** Go Backward ************/
void Backward(char speed)
{
Motor_Init();
Change_Duty(speed);
Motor_A_BWD();
Motor_B_BWD();
}
/************************************/
/********** Spin Left *************/
void S_Right(char speed)
{
Motor_Init();
Change_Duty(speed);
Motor_A_FWD();
Motor_B_BWD();
}
/************************************/
/********** Spin Right ************/
void S_Left(char speed)
{
Motor_Init();
Change_Duty(speed);
Motor_A_BWD();
Motor_B_FWD();
}
/************************************/
/********** Motor A Off ************/
void Motor_A_Off()
{
Pwm1_Stop();
PORTD.F0 =0;
PORTD.F1 =0;
}
/************************************/
/********** Motor B Off ************/
void Motor_B_Off()
{
Pwm2_Stop();
PORTB.F1 =0;
PORTB.F2 =0;
}
/************************************/
/********** Stop Motor ************/
void Motor_Stop()
{
Change_Duty(0);
// Pwm1_Change_Duty(0); // Motor A
// Pwm2_Change_Duty(0); // Motor B
Motor_A_Off();
Motor_B_Off();
motor_init_=0;
}
/************************************/