| User | Revision | Line number | New contents of line |
|---|
| gsulc |
0:178a07cd3e39
|
1
|
#include "move.h"
|
| gsulc |
0:178a07cd3e39
|
2
|
|
| gsulc |
0:178a07cd3e39
|
3
|
//void accelerate(Motor m, float distance) {
|
| gsulc |
0:178a07cd3e39
|
4
|
|
| gsulc |
0:178a07cd3e39
|
5
|
//}
|
| gsulc |
0:178a07cd3e39
|
6
|
|
| gsulc |
0:178a07cd3e39
|
7
|
//void move(int x, int y) {
|
| gsulc |
0:178a07cd3e39
|
8
|
|
| gsulc |
0:178a07cd3e39
|
9
|
//}
|
| gsulc |
0:178a07cd3e39
|
10
|
|
| gsulc |
0:178a07cd3e39
|
11
|
//void move_line(float distance) {
|
| gsulc |
0:178a07cd3e39
|
12
|
/*while (distance > 0) {
|
| gsulc |
0:178a07cd3e39
|
13
|
//distance -= distance_traveled();
|
| gsulc |
0:178a07cd3e39
|
14
|
left_motor.speed(LEFT_C*FULL_FOWARD);
|
| gsulc |
0:178a07cd3e39
|
15
|
right_motor.speed(RIGHT_C*FULL_FOWARD);
|
| gsulc |
0:178a07cd3e39
|
16
|
}
|
| gsulc |
0:178a07cd3e39
|
17
|
left_motor.stop();
|
| gsulc |
0:178a07cd3e39
|
18
|
right_motor.stop();*/
|
| gsulc |
0:178a07cd3e39
|
19
|
//}
|
| gsulc |
0:178a07cd3e39
|
20
|
/*
|
| gsulc |
0:178a07cd3e39
|
21
|
void rotate(int degrees) {
|
| gsulc |
0:178a07cd3e39
|
22
|
}
|
| gsulc |
0:178a07cd3e39
|
23
|
|
| gsulc |
0:178a07cd3e39
|
24
|
void rotate(float time, int direction) {
|
| gsulc |
0:178a07cd3e39
|
25
|
//left_motor.speed(0.5);
|
| gsulc |
0:178a07cd3e39
|
26
|
//right_motor.speed(0.5);
|
| gsulc |
0:178a07cd3e39
|
27
|
}
|
| gsulc |
0:178a07cd3e39
|
28
|
*/
|
| gsulc |
0:178a07cd3e39
|
29
|
//void move_arc(int radius, int length, int x, int y) {
|
| gsulc |
0:178a07cd3e39
|
30
|
//}
|
| gsulc |
0:178a07cd3e39
|
31
|
|
| gsulc |
0:178a07cd3e39
|
32
|
// Shahan
|
| gsulc |
0:178a07cd3e39
|
33
|
|
| gsulc |
0:178a07cd3e39
|
34
|
void moveforward(Motor wheel,float speed){
|
| gsulc |
0:178a07cd3e39
|
35
|
wheel.speed(speed);
|
| gsulc |
0:178a07cd3e39
|
36
|
}
|
| gsulc |
0:178a07cd3e39
|
37
|
|
| gsulc |
0:178a07cd3e39
|
38
|
void movebackward(Motor wheel,float speed){
|
| gsulc |
0:178a07cd3e39
|
39
|
wheel.speed(-speed);
|
| gsulc |
0:178a07cd3e39
|
40
|
}
|
| gsulc |
0:178a07cd3e39
|
41
|
|
| gsulc |
0:178a07cd3e39
|
42
|
void move(float speed, int tics){
|
| gsulc |
0:178a07cd3e39
|
43
|
/*r_goal = abs(distance)/TICS_PER_MM;
|
| gsulc |
0:178a07cd3e39
|
44
|
l_goal = abs(distance)/TICS_PER_MM;
|
| gsulc |
0:178a07cd3e39
|
45
|
if(distance > 0){
|
| gsulc |
0:178a07cd3e39
|
46
|
r_motor.speed(r_C);
|
| gsulc |
0:178a07cd3e39
|
47
|
l_motor.speed(l_C);
|
| gsulc |
0:178a07cd3e39
|
48
|
}
|
| gsulc |
0:178a07cd3e39
|
49
|
else if(distance < 0){
|
| gsulc |
0:178a07cd3e39
|
50
|
r_motor.speed(-r_C);
|
| gsulc |
0:178a07cd3e39
|
51
|
l_motor.speed(-l_C);
|
| gsulc |
0:178a07cd3e39
|
52
|
}*/
|
| gsulc |
0:178a07cd3e39
|
53
|
float distance = (248) * srf;
|
| gsulc |
0:178a07cd3e39
|
54
|
if(distance <= 14)
|
| gsulc |
0:178a07cd3e39
|
55
|
rotate(1);
|
| gsulc |
0:178a07cd3e39
|
56
|
|
| gsulc |
0:178a07cd3e39
|
57
|
l_goal = l_tics + tics;
|
| gsulc |
0:178a07cd3e39
|
58
|
r_goal = r_tics + tics;
|
| gsulc |
0:178a07cd3e39
|
59
|
|
| gsulc |
0:178a07cd3e39
|
60
|
l_motor.speed(speed * l_C);
|
| gsulc |
0:178a07cd3e39
|
61
|
r_motor.speed(speed * r_C);
|
| gsulc |
0:178a07cd3e39
|
62
|
move_state = Moving;
|
| gsulc |
0:178a07cd3e39
|
63
|
}
|
| gsulc |
0:178a07cd3e39
|
64
|
|
| gsulc |
0:178a07cd3e39
|
65
|
|
| gsulc |
0:178a07cd3e39
|
66
|
void QTIsensor_init(){
|
| gsulc |
0:178a07cd3e39
|
67
|
r_charge = 0;
|
| gsulc |
0:178a07cd3e39
|
68
|
l_charge = 0;
|
| gsulc |
0:178a07cd3e39
|
69
|
}
|
| gsulc |
0:178a07cd3e39
|
70
|
|
| gsulc |
0:178a07cd3e39
|
71
|
void QTIsensor_charge(){
|
| gsulc |
0:178a07cd3e39
|
72
|
r_charge = 1;
|
| gsulc |
0:178a07cd3e39
|
73
|
l_charge = 1;
|
| gsulc |
0:178a07cd3e39
|
74
|
}
|
| gsulc |
0:178a07cd3e39
|
75
|
|
| gsulc |
0:178a07cd3e39
|
76
|
void QTIsensor_discharge(){
|
| gsulc |
0:178a07cd3e39
|
77
|
int r_enc_bin = 1, l_enc_bin = 1;
|
| gsulc |
0:178a07cd3e39
|
78
|
if(r_encoder < QTI_THRESH) {
|
| gsulc |
0:178a07cd3e39
|
79
|
r_enc_bin = 0;
|
| gsulc |
0:178a07cd3e39
|
80
|
}
|
| gsulc |
0:178a07cd3e39
|
81
|
r_charge = 0;
|
| gsulc |
0:178a07cd3e39
|
82
|
if(l_encoder < QTI_THRESH) {
|
| gsulc |
0:178a07cd3e39
|
83
|
l_enc_bin = 0;
|
| gsulc |
0:178a07cd3e39
|
84
|
}
|
| gsulc |
0:178a07cd3e39
|
85
|
l_charge = 0;
|
| gsulc |
0:178a07cd3e39
|
86
|
|
| gsulc |
0:178a07cd3e39
|
87
|
if(prev_r_enc != r_enc_bin)
|
| gsulc |
0:178a07cd3e39
|
88
|
r_tics++;
|
| gsulc |
0:178a07cd3e39
|
89
|
if(prev_l_enc != l_enc_bin)
|
| gsulc |
0:178a07cd3e39
|
90
|
l_tics++;
|
| gsulc |
0:178a07cd3e39
|
91
|
|
| gsulc |
0:178a07cd3e39
|
92
|
prev_r_enc = r_enc_bin;
|
| gsulc |
0:178a07cd3e39
|
93
|
prev_l_enc = l_enc_bin;
|
| gsulc |
0:178a07cd3e39
|
94
|
}
|
| gsulc |
0:178a07cd3e39
|
95
|
|
| gsulc |
0:178a07cd3e39
|
96
|
void motors_stop(){
|
| gsulc |
0:178a07cd3e39
|
97
|
r_motor.speed(0);
|
| gsulc |
0:178a07cd3e39
|
98
|
l_motor.speed(0);
|
| gsulc |
0:178a07cd3e39
|
99
|
}
|
| gsulc |
0:178a07cd3e39
|
100
|
|
| gsulc |
0:178a07cd3e39
|
101
|
|
| gsulc |
0:178a07cd3e39
|
102
|
|
| gsulc |
0:178a07cd3e39
|
103
|
void calibrate(float speed, int rots) {
|
| gsulc |
0:178a07cd3e39
|
104
|
float l_corr = -1, r_corr = -1;
|
| gsulc |
0:178a07cd3e39
|
105
|
|
| gsulc |
0:178a07cd3e39
|
106
|
do {
|
| gsulc |
0:178a07cd3e39
|
107
|
l_goal = l_tics + rots;
|
| gsulc |
0:178a07cd3e39
|
108
|
r_goal = r_tics + rots;
|
| gsulc |
0:178a07cd3e39
|
109
|
|
| gsulc |
0:178a07cd3e39
|
110
|
wait(4);
|
| gsulc |
0:178a07cd3e39
|
111
|
|
| gsulc |
0:178a07cd3e39
|
112
|
l_motor.speed(speed * l_C);
|
| gsulc |
0:178a07cd3e39
|
113
|
r_motor.speed(speed * r_C);
|
| gsulc |
0:178a07cd3e39
|
114
|
|
| gsulc |
0:178a07cd3e39
|
115
|
while (r_tics < r_goal && l_tics < l_goal) {
|
| gsulc |
0:178a07cd3e39
|
116
|
pc.printf("Left : %d Goal: %d\n\r", l_tics, l_goal);
|
| gsulc |
0:178a07cd3e39
|
117
|
pc.printf("Right: %d Goal: %d\n\r", r_tics, r_goal);
|
| gsulc |
0:178a07cd3e39
|
118
|
//wait();
|
| gsulc |
0:178a07cd3e39
|
119
|
}
|
| gsulc |
0:178a07cd3e39
|
120
|
|
| gsulc |
0:178a07cd3e39
|
121
|
r_motor.speed(NEUTRAL);
|
| gsulc |
0:178a07cd3e39
|
122
|
l_motor.speed(NEUTRAL);
|
| gsulc |
0:178a07cd3e39
|
123
|
|
| gsulc |
0:178a07cd3e39
|
124
|
if(r_tics < r_goal) {
|
| gsulc |
0:178a07cd3e39
|
125
|
//slow l down
|
| gsulc |
0:178a07cd3e39
|
126
|
l_corr += 0.02;
|
| gsulc |
0:178a07cd3e39
|
127
|
}
|
| gsulc |
0:178a07cd3e39
|
128
|
else if(l_tics < l_goal) {
|
| gsulc |
0:178a07cd3e39
|
129
|
//slow r down
|
| gsulc |
0:178a07cd3e39
|
130
|
r_corr += 0.02;
|
| gsulc |
0:178a07cd3e39
|
131
|
}
|
| gsulc |
0:178a07cd3e39
|
132
|
else {
|
| gsulc |
0:178a07cd3e39
|
133
|
break;
|
| gsulc |
0:178a07cd3e39
|
134
|
}
|
| gsulc |
0:178a07cd3e39
|
135
|
|
| gsulc |
0:178a07cd3e39
|
136
|
wait(2);
|
| gsulc |
0:178a07cd3e39
|
137
|
|
| gsulc |
0:178a07cd3e39
|
138
|
l_goal = l_tics + rots;
|
| gsulc |
0:178a07cd3e39
|
139
|
r_goal = r_tics + rots;
|
| gsulc |
0:178a07cd3e39
|
140
|
|
| gsulc |
0:178a07cd3e39
|
141
|
l_motor.speed(-speed * l_C);
|
| gsulc |
0:178a07cd3e39
|
142
|
r_motor.speed(-speed * r_C);
|
| gsulc |
0:178a07cd3e39
|
143
|
|
| gsulc |
0:178a07cd3e39
|
144
|
while (r_tics < r_goal && l_tics < l_goal) {
|
| gsulc |
0:178a07cd3e39
|
145
|
pc.printf("Left : %d Goal: %d\n\r", l_tics, l_goal);
|
| gsulc |
0:178a07cd3e39
|
146
|
pc.printf("Right: %d Goal: %d\n\r", r_tics, r_goal);
|
| gsulc |
0:178a07cd3e39
|
147
|
//wait(1);
|
| gsulc |
0:178a07cd3e39
|
148
|
}
|
| gsulc |
0:178a07cd3e39
|
149
|
|
| gsulc |
0:178a07cd3e39
|
150
|
r_motor.speed(NEUTRAL);
|
| gsulc |
0:178a07cd3e39
|
151
|
l_motor.speed(NEUTRAL);
|
| gsulc |
0:178a07cd3e39
|
152
|
|
| gsulc |
0:178a07cd3e39
|
153
|
l_C = l_corr;
|
| gsulc |
0:178a07cd3e39
|
154
|
r_C = r_corr;
|
| gsulc |
0:178a07cd3e39
|
155
|
} while( abs(r_goal - r_tics) > 2 || abs(l_goal - l_tics) > 2);
|
| gsulc |
0:178a07cd3e39
|
156
|
}
|
| gsulc |
0:178a07cd3e39
|
157
|
|
| gsulc |
0:178a07cd3e39
|
158
|
void rotate(int angle){
|
| gsulc |
0:178a07cd3e39
|
159
|
float speed = 0.2;
|
| gsulc |
0:178a07cd3e39
|
160
|
if(angle != 0) {
|
| gsulc |
0:178a07cd3e39
|
161
|
r_goal = r_tics + 3;
|
| gsulc |
0:178a07cd3e39
|
162
|
l_goal = l_tics + 3;
|
| gsulc |
0:178a07cd3e39
|
163
|
pc.printf("now turning at angle %d\n\r",angle);
|
| gsulc |
0:178a07cd3e39
|
164
|
r_motor.speed(angle*speed*r_C);
|
| gsulc |
0:178a07cd3e39
|
165
|
l_motor.speed(-1*angle*speed*l_C);
|
| gsulc |
0:178a07cd3e39
|
166
|
pc.printf("************ Speed: %f", speed);
|
| gsulc |
0:178a07cd3e39
|
167
|
move_state = Moving;
|
| gsulc |
0:178a07cd3e39
|
168
|
}
|
| gsulc |
0:178a07cd3e39
|
169
|
else {
|
| gsulc |
0:178a07cd3e39
|
170
|
r_goal = r_tics;
|
| gsulc |
0:178a07cd3e39
|
171
|
l_goal = l_tics;
|
| gsulc |
0:178a07cd3e39
|
172
|
move_state = Moving;
|
| gsulc |
0:178a07cd3e39
|
173
|
}
|
| gsulc |
0:178a07cd3e39
|
174
|
|
| gsulc |
0:178a07cd3e39
|
175
|
|
| gsulc |
0:178a07cd3e39
|
176
|
}
|
| gsulc |
0:178a07cd3e39
|
177
|
|
| gsulc |
0:178a07cd3e39
|
178
|
void update_movement(float speed) {
|
| gsulc |
0:178a07cd3e39
|
179
|
int r_diff = r_goal - r_tics;
|
| gsulc |
0:178a07cd3e39
|
180
|
int l_diff = l_goal - l_tics;
|
| gsulc |
0:178a07cd3e39
|
181
|
|
| gsulc |
0:178a07cd3e39
|
182
|
if (r_diff > l_diff) {
|
| gsulc |
0:178a07cd3e39
|
183
|
l_motor.speed(speed / 1.2 *l_C);
|
| gsulc |
0:178a07cd3e39
|
184
|
wait_us(1 * (r_diff - l_diff));
|
| gsulc |
0:178a07cd3e39
|
185
|
l_motor.speed(speed*l_C);
|
| gsulc |
0:178a07cd3e39
|
186
|
}
|
| gsulc |
0:178a07cd3e39
|
187
|
|
| gsulc |
0:178a07cd3e39
|
188
|
else if (r_diff < l_diff) {
|
| gsulc |
0:178a07cd3e39
|
189
|
r_motor.speed(speed / 1.2 *r_C);
|
| gsulc |
0:178a07cd3e39
|
190
|
wait_us(1 * (l_diff - r_diff));
|
| gsulc |
0:178a07cd3e39
|
191
|
r_motor.speed(speed*r_C);
|
| gsulc |
0:178a07cd3e39
|
192
|
}
|
| gsulc |
0:178a07cd3e39
|
193
|
}
|
Gordon Sulc