Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / colonet / utilities / dragonfly_wireless_relay / move.c @ 13

History | View | Annotate | Download (3.92 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 vl = 0;
31
  int vr = 0;
32
  int temp;
33
  
34
  temp=read_distance(IR1);
35
  d1 = (temp == -1) ? d1 : temp;
36
  
37
  temp=read_distance(IR2);
38
  d2=(temp == -1) ? d2 : temp;
39
  
40
  temp=read_distance(IR3);
41
  d3=(temp == -1) ? d3 : temp;
42
  
43
  temp=read_distance(IR4);
44
  d4=(temp == -1) ? d4 : temp;
45
  
46
  temp=read_distance(IR5);
47
  d5=(temp == -1) ? d5 : temp;
48
  
49
  /* Avoid obstacles ahead
50
  if(d2>170)
51
    v*=-1;
52
    
53
  Naturally slow down if there is something in the way.
54
  if(d2>150 || d1>180 || d3>180){
55
    v>>=1;
56
  */
57
  
58
  pControl= (((d3-d1) + (d4-d5))*strength)/100;
59
  omega = (omega*(100-strength))/100 + pControl;
60
  translateAngulartoLinear(velocity, omega, &vl , &vr );
61
  
62
  if (vl < 0) {
63
    motor1_set(BACKWARD, -vl);
64
  } else {
65
    motor1_set(FORWARD, vl);
66
  }
67
  if (vr < 0) {
68
    motor2_set(BACKWARD, -vr);
69
  } else {
70
    motor2_set(FORWARD, vr);
71
  }
72
}
73

    
74
void translateAngulartoLinear (int velocity, int omega, int* vl, int* vr) {
75
        //omega: angle measure, positive couter-clockwise from front.
76
        // -180 <= omega <= 180
77
        //velocity: -255 <= velocity <= 255
78
  
79
  long int vltemp, vrtemp;
80
  
81
  //make sure values are in bounds
82
  if (velocity < -255 || velocity >255 || omega < -255 || omega > 255) return;
83
        
84
  //compute
85
        vrtemp = velocity + omega * 3;
86
        vltemp = velocity - omega * 3;
87
        
88
        //check to see if max linear velocities have been exceeded.
89
  if (vrtemp > 255) {
90
    vltemp = 255 * vltemp / vrtemp;
91
    vrtemp = 255;
92
  }
93
  if (vltemp > 255) {           
94
        vrtemp = 255 * vrtemp / vltemp;    
95
        vltemp = 255;  
96
  }
97
  if (vrtemp < -255) {
98
    vltemp = -255 * vltemp / vrtemp;
99
    vrtemp = -255;
100
  }
101
  if (vltemp < -255) {
102
    vrtemp = -255 * vrtemp / vltemp;
103
    vltemp = -255;
104
  }
105
  
106
  *vr = (int)vrtemp;
107
  *vl = (int)vltemp;
108
        
109

    
110
}
111

    
112
/*
113
Move and turn functions are overloaded so they can be called with or without the speed or time parameters.
114
If called wihtout parameters, the defaults will be used.
115
*/
116

    
117
//Forward
118
void moveForward(void) {
119
        motor1_set(FWD, NRML_SPD);
120
        motor2_set(FWD, NRML_SPD);
121
        delay_ms(DELAY_F);
122
}
123

    
124
/*void moveForward(int speed) {
125
        motor1_set(FWD, speed);
126
        motor2_set(FWD, speed);
127
        delay_ms(DELAY_F);
128
}
129

130
void moveForward(int speed, int time) {
131
        motor1_set(FWD, speed);
132
        motor2_set(FWD, speed);
133
        delay_ms(time);
134
}
135
*/
136
//Back
137
void moveBack(void) {
138
        motor1_set(BCK, HALF_SPD);
139
        motor2_set(BCK, HALF_SPD);
140
        delay_ms(DELAY_B);
141
}
142
/*
143
void moveBack(int speed) {
144
        motor1_set(BCK, speed);
145
        motor2_set(BCK, speed);
146
        delay_ms(DELAY_B);
147
}
148

149
void moveBack(int speed, int time) {
150
        motor1_set(BCK, speed);
151
        motor2_set(BCK, speed);
152
        delay_ms(time);
153
}
154
*/
155
//Turn Left
156
void turnLeft(void) {
157
        motor1_set(FWD, NRML_TURN);
158
        motor2_set(BCK, NRML_TURN);
159
        delay_ms(DELAY_L);
160
}
161
/*
162
void turnLeft(int speed) {
163
        motor1_set(FWD, speed);
164
        motor2_set(BCK, speed);
165
        delay_ms(DELAY_L);
166
}
167

168
void turnLeft(int speed, int time) {
169
        motor1_set(FWD, speed);
170
        motor2_set(BCK, speed);
171
        delay_ms(time);
172
}
173
*/
174
//Turn Right
175
void turnRight(void) {
176
        motor1_set(BCK, NRML_TURN);
177
        motor2_set(FWD, NRML_TURN);
178
        delay_ms(DELAY_R);
179
}
180
/*
181
void turnRight(int speed) {
182
        motor1_set(BCK, speed);
183
        motor2_set(FWD, speed);
184
        delay_ms(DELAY_R);
185
}
186

187
void turnRight(int speed, int time) {
188
        motor1_set(BCK, speed);
189
        motor2_set(FWD, speed);
190
        delay_ms(time);
191
}*/