root / branches / encoders / code / lib / src / libdragonfly / move.c @ 1345
History | View | Annotate | Download (3.99 KB)
1 | 7 | bcoltin | #include "dragonfly_lib.h" |
---|---|---|---|
2 | |||
3 | #include "move.h" |
||
4 | #include "rangefinder.h" |
||
5 | |||
6 | |||
7 | // Internal prototypes
|
||
8 | void translateAngulartoLinear (int velocity, int omega, int* vl, int* vr); |
||
9 | |||
10 | // global varaibles for move_avoid
|
||
11 | int d1, d2, d3, d4, d5;
|
||
12 | |||
13 | /**
|
||
14 | * @defgroup move Movement
|
||
15 | * @brief Functions fo controlling robot motion
|
||
16 | * Higher level functions to control the movement of robots.
|
||
17 | *
|
||
18 | * @{
|
||
19 | **/
|
||
20 | |||
21 | |||
22 | /**
|
||
23 | * Causes the robot to move with the given translation and rotational velocities.
|
||
24 | * motors_init must be called before this function can be used.
|
||
25 | *
|
||
26 | * @param velocity the translational velocity of the robot, in the range -255 to 255.
|
||
27 | * A positive value indicates forward motion, while a negative value indicates
|
||
28 | * backwards motion.
|
||
29 | *
|
||
30 | * @param omega the rotational velocity of the robot, in the range -255 to 255.
|
||
31 | * A positive value indicates a counterclockwise velocity, while a negative
|
||
32 | * value indicates a clockwise velocity.
|
||
33 | *
|
||
34 | * @see motors_init, motor1_set, motor2_set
|
||
35 | **/
|
||
36 | void move (int velocity, int omega) { |
||
37 | int vl = 0; |
||
38 | int vr = 0; |
||
39 | translateAngulartoLinear(velocity, omega, &vl , &vr ); |
||
40 | //
|
||
41 | |||
42 | if (vl < 0) { |
||
43 | motor1_set(BACKWARD, -vl); |
||
44 | } else {
|
||
45 | motor1_set(FORWARD, vl); |
||
46 | } |
||
47 | if (vr < 0) { |
||
48 | motor2_set(BACKWARD, -vr); |
||
49 | } else {
|
||
50 | motor2_set(FORWARD, vr); |
||
51 | } |
||
52 | } |
||
53 | |||
54 | |||
55 | /**
|
||
56 | * Moves the robot with the given translational and angular velocities
|
||
57 | * while avoiding obstacles. To be effective, this function must be
|
||
58 | * called repeatedly throughout the motion. It relies on the IR
|
||
59 | * rangefinders to detect obstacles. Before calling this function,
|
||
60 | * motors_init and range_init must be called.
|
||
61 | *
|
||
62 | * @param velocity the translational velocity of the robot, in the
|
||
63 | * range -255 to 255. A positive value indicates forward motion.
|
||
64 | *
|
||
65 | * @param omega the rotational velocity of the robot, in the range
|
||
66 | * -255 to 255. A positive value indicates a counterclockwise velocity.
|
||
67 | *
|
||
68 | * @param strength the strength of the avoid behavior, in the range
|
||
69 | * 0 to 100.
|
||
70 | *
|
||
71 | * @see motors_init, range_init, move
|
||
72 | **/
|
||
73 | void move_avoid(int velocity, int omega, int strength){ |
||
74 | int pControl;
|
||
75 | int vl = 0; |
||
76 | int vr = 0; |
||
77 | int temp;
|
||
78 | |||
79 | temp=range_read_distance(IR1); |
||
80 | d1 = (temp == -1) ? d1 : temp;
|
||
81 | |||
82 | temp=range_read_distance(IR2); |
||
83 | d2=(temp == -1) ? d2 : temp;
|
||
84 | |||
85 | temp=range_read_distance(IR3); |
||
86 | d3=(temp == -1) ? d3 : temp;
|
||
87 | |||
88 | temp=range_read_distance(IR4); |
||
89 | d4=(temp == -1) ? d4 : temp;
|
||
90 | |||
91 | temp=range_read_distance(IR5); |
||
92 | d5=(temp == -1) ? d5 : temp;
|
||
93 | |||
94 | /* Avoid obstacles ahead
|
||
95 | if(d2>170)
|
||
96 | v*=-1;
|
||
97 | |||
98 | Naturally slow down if there is something in the way.
|
||
99 | if(d2>150 || d1>180 || d3>180){
|
||
100 | v>>=1;
|
||
101 | */
|
||
102 | |||
103 | //pControl= (((d3-d1) + (d4-d5))*strength)/100;
|
||
104 | pControl= (((d5-d4))*strength)/100;
|
||
105 | omega = (omega*(100-strength))/100 + pControl; |
||
106 | translateAngulartoLinear(velocity, omega, &vl , &vr ); |
||
107 | |||
108 | if (vl < 0) { |
||
109 | motor1_set(BACKWARD, -vl); |
||
110 | } else {
|
||
111 | motor1_set(FORWARD, vl); |
||
112 | } |
||
113 | if (vr < 0) { |
||
114 | motor2_set(BACKWARD, -vr); |
||
115 | } else {
|
||
116 | motor2_set(FORWARD, vr); |
||
117 | } |
||
118 | } |
||
119 | |||
120 | /**@}**///end the motion group |
||
121 | |||
122 | void translateAngulartoLinear (int velocity, int omega, int* vl, int* vr) { |
||
123 | //omega: angle measure, positive couter-clockwise from front.
|
||
124 | // -180 <= omega <= 180
|
||
125 | //velocity: -255 <= velocity <= 255
|
||
126 | |||
127 | long int vltemp, vrtemp; |
||
128 | |||
129 | //make sure values are in bounds
|
||
130 | if (velocity < -255 || velocity >255 || omega < -255 || omega > 255) return; |
||
131 | |||
132 | //compute
|
||
133 | vrtemp = velocity + omega * 3;
|
||
134 | vltemp = velocity - omega * 3;
|
||
135 | |||
136 | //check to see if max linear velocities have been exceeded.
|
||
137 | if (vrtemp > 255) { |
||
138 | vltemp = 255 * vltemp / vrtemp;
|
||
139 | vrtemp = 255;
|
||
140 | } |
||
141 | if (vltemp > 255) { |
||
142 | vrtemp = 255 * vrtemp / vltemp;
|
||
143 | vltemp = 255;
|
||
144 | } |
||
145 | if (vrtemp < -255) { |
||
146 | vltemp = -255 * vltemp / vrtemp;
|
||
147 | vrtemp = -255;
|
||
148 | } |
||
149 | if (vltemp < -255) { |
||
150 | vrtemp = -255 * vrtemp / vltemp;
|
||
151 | vltemp = -255;
|
||
152 | } |
||
153 | |||
154 | *vr = (int)vrtemp;
|
||
155 | *vl = (int)vltemp;
|
||
156 | |||
157 | |||
158 | } |
||
159 |