-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmotor_control001.c
119 lines (107 loc) · 2.92 KB
/
motor_control001.c
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
//motor_control001.c
#include "sumovore.h"
#include "motor_control.h"
#include <delays.h>
void spin_left(void);
void turn_left(void);
void straight_fwd_fast(void);
void straight_fwd_medium(void);
void straight_bk_fast(void);
void straight_bk_medium(void);
void turn_right(void);
void spin_right(void);
void spin_around(void);
void spin_one_place(void);
void spin_large_circle(void);
void motor_control(void)
{
spin_around();
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
spin_one_place();
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
spin_large_circle();
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
Delay10KTCYx(200); // Wait 0.250 seconds
/*// very simple motor control
if ( SeeLine.b.Center ) straight_fwd_fast();
else if (SeeLine.b.Left) spin_left();
else if (SeeLine.b.CntLeft) turn_left();
else if (SeeLine.b.CntRight) turn_right();
else if (SeeLine.b.Right) spin_right();
if ( (SeeLine.B ) == 0b00000u) motors_brake_all();*/
}
void spin_left(void)
{
set_motor_speed(left, rev_fast, 0);
set_motor_speed(right, fast, 0);
}
void turn_left(void)
{
set_motor_speed(left, stop, 0);
set_motor_speed(right, fast, 0);
}
void straight_fwd_fast(void)
{
set_motor_speed(left, fast, 0);
set_motor_speed(right, fast, -40);
}
void spin_right(void)
{
set_motor_speed(left, fast, 0);
set_motor_speed(right, rev_fast, 0);
}
void turn_right(void)
{
set_motor_speed(left, fast, 0);
set_motor_speed(right, stop, 0);
}
void straight_fwd_medium(void)
{
set_motor_speed(left, medium, 0);
set_motor_speed(right, medium, 0);
}
void straight_bk_fast(void)
{
set_motor_speed(left, rev_fast, 0);
set_motor_speed(right, rev_fast, 0);
}
void straight_bk_medium(void)
{
set_motor_speed(left, rev_medium, 0);
set_motor_speed(right, rev_medium, 0);
}
void spin_around(void)
{
set_motor_speed(left, stop, 0);
set_motor_speed(right, fast, 0);
}
void spin_one_place(void)
{
set_motor_speed(left, rev_fast, 0);
set_motor_speed(right, fast, 0);
}
void spin_large_circle(void)
{
set_motor_speed(left, slow, 0);
set_motor_speed(right, fast, 0);
}