root / trunk / code / projects / colonet / utilities / robot_slave / move.c @ 13
History | View | Annotate | Download (3.67 KB)
1 |
//Created 4/30/06 by James Kong
|
---|---|
2 |
|
3 |
#include "dragonfly_lib.h" |
4 |
//#include "pindefs_ff.h"
|
5 |
#include "move.h" |
6 |
|
7 |
void move (int velocity, int omega) { |
8 |
int vl = 0; |
9 |
int vr = 0; |
10 |
translateAngulartoLinear(velocity, omega, &vl , &vr ); |
11 |
//
|
12 |
|
13 |
if (vl < 0) { |
14 |
motor1_set(BACKWARD, -vl); |
15 |
} else {
|
16 |
motor1_set(FORWARD, vl); |
17 |
} |
18 |
if (vr < 0) { |
19 |
motor2_set(BACKWARD, -vr); |
20 |
} else {
|
21 |
motor2_set(FORWARD, vr); |
22 |
} |
23 |
} |
24 |
|
25 |
/*@strength, the strength of the avoid behavior from 0 to 100,
|
26 |
255 gives full control to avoid, 0 none. defines here for now*/
|
27 |
|
28 |
void move_avoid(int velocity, int omega, int strength){ |
29 |
int pControl;
|
30 |
int temp;
|
31 |
|
32 |
temp=read_distance(IR1); |
33 |
d1 = (temp == -1) ? d1 : temp;
|
34 |
|
35 |
temp=read_distance(IR2); |
36 |
d2=(temp == -1) ? d2 : temp;
|
37 |
|
38 |
temp=read_distance(IR3); |
39 |
d3=(temp == -1) ? d3 : temp;
|
40 |
|
41 |
temp=read_distance(IR4); |
42 |
d4=(temp == -1) ? d4 : temp;
|
43 |
|
44 |
temp=read_distance(IR5); |
45 |
d5=(temp == -1) ? d5 : temp;
|
46 |
|
47 |
/* Avoid obstacles ahead
|
48 |
if(d2>170)
|
49 |
v*=-1;
|
50 |
|
51 |
Naturally slow down if there is something in the way.
|
52 |
if(d2>150 || d1>180 || d3>180){
|
53 |
v>>=1;
|
54 |
*/
|
55 |
|
56 |
pControl= (((d3-d1) + (d4-d5))*strength)/100;
|
57 |
omega = (omega*(100-strength))/100 + pControl; |
58 |
|
59 |
move(velocity, omega); |
60 |
} |
61 |
|
62 |
void translateAngulartoLinear (int velocity, int omega, int* vl, int* vr) { |
63 |
//omega: angle measure, positive couter-clockwise from front.
|
64 |
// -180 <= omega <= 180
|
65 |
//velocity: -255 <= velocity <= 255
|
66 |
|
67 |
long int vltemp, vrtemp; |
68 |
|
69 |
//make sure values are in bounds
|
70 |
if (velocity < -255 || velocity >255 || omega < -255 || omega > 255) return; |
71 |
|
72 |
//compute
|
73 |
vrtemp = velocity + omega * 3;
|
74 |
vltemp = velocity - omega * 3;
|
75 |
|
76 |
//check to see if max linear velocities have been exceeded.
|
77 |
if (vrtemp > 255) { |
78 |
vltemp = 255 * vltemp / vrtemp;
|
79 |
vrtemp = 255;
|
80 |
} |
81 |
if (vltemp > 255) { |
82 |
vrtemp = 255 * vrtemp / vltemp;
|
83 |
vltemp = 255;
|
84 |
} |
85 |
if (vrtemp < -255) { |
86 |
vltemp = -255 * vltemp / vrtemp;
|
87 |
vrtemp = -255;
|
88 |
} |
89 |
if (vltemp < -255) { |
90 |
vrtemp = -255 * vrtemp / vltemp;
|
91 |
vltemp = -255;
|
92 |
} |
93 |
|
94 |
*vr = (int)vrtemp;
|
95 |
*vl = (int)vltemp;
|
96 |
|
97 |
|
98 |
} |
99 |
|
100 |
/*
|
101 |
Move and turn functions are overloaded so they can be called with or without the speed or time parameters.
|
102 |
If called wihtout parameters, the defaults will be used.
|
103 |
*/
|
104 |
|
105 |
//Forward
|
106 |
void moveForward(void) { |
107 |
motor1_set(FWD, NRML_SPD); |
108 |
motor2_set(FWD, NRML_SPD); |
109 |
delay_ms(DELAY_F); |
110 |
} |
111 |
|
112 |
/*void moveForward(int speed) {
|
113 |
motor1_set(FWD, speed);
|
114 |
motor2_set(FWD, speed);
|
115 |
delay_ms(DELAY_F);
|
116 |
}
|
117 |
|
118 |
void moveForward(int speed, int time) {
|
119 |
motor1_set(FWD, speed);
|
120 |
motor2_set(FWD, speed);
|
121 |
delay_ms(time);
|
122 |
}
|
123 |
*/
|
124 |
//Back
|
125 |
void moveBack(void) { |
126 |
motor1_set(BCK, HALF_SPD); |
127 |
motor2_set(BCK, HALF_SPD); |
128 |
delay_ms(DELAY_B); |
129 |
} |
130 |
/*
|
131 |
void moveBack(int speed) {
|
132 |
motor1_set(BCK, speed);
|
133 |
motor2_set(BCK, speed);
|
134 |
delay_ms(DELAY_B);
|
135 |
}
|
136 |
|
137 |
void moveBack(int speed, int time) {
|
138 |
motor1_set(BCK, speed);
|
139 |
motor2_set(BCK, speed);
|
140 |
delay_ms(time);
|
141 |
}
|
142 |
*/
|
143 |
//Turn Left
|
144 |
void turnLeft(void) { |
145 |
motor1_set(FWD, NRML_TURN); |
146 |
motor2_set(BCK, NRML_TURN); |
147 |
delay_ms(DELAY_L); |
148 |
} |
149 |
/*
|
150 |
void turnLeft(int speed) {
|
151 |
motor1_set(FWD, speed);
|
152 |
motor2_set(BCK, speed);
|
153 |
delay_ms(DELAY_L);
|
154 |
}
|
155 |
|
156 |
void turnLeft(int speed, int time) {
|
157 |
motor1_set(FWD, speed);
|
158 |
motor2_set(BCK, speed);
|
159 |
delay_ms(time);
|
160 |
}
|
161 |
*/
|
162 |
//Turn Right
|
163 |
void turnRight(void) { |
164 |
motor1_set(BCK, NRML_TURN); |
165 |
motor2_set(FWD, NRML_TURN); |
166 |
delay_ms(DELAY_R); |
167 |
} |
168 |
/*
|
169 |
void turnRight(int speed) {
|
170 |
motor1_set(BCK, speed);
|
171 |
motor2_set(FWD, speed);
|
172 |
delay_ms(DELAY_R);
|
173 |
}
|
174 |
|
175 |
void turnRight(int speed, int time) {
|
176 |
motor1_set(BCK, speed);
|
177 |
motor2_set(FWD, speed);
|
178 |
delay_ms(time);
|
179 |
}*/
|