Revision 521
broken robot code
trunk/code/projects/colonet/robot/colonet_dragonfly/colonet_dragonfly.c | ||
---|---|---|
16 | 16 |
|
17 | 17 |
#include <colonet_dragonfly.h> |
18 | 18 |
|
19 |
#define TARGET_POSITION_STOP_DISTANCE_THRESHOLD 15 |
|
20 |
|
|
19 | 21 |
typedef struct { |
20 | 22 |
unsigned char msgId; //is this necessary? |
21 | 23 |
void (*handler)(void); |
... | ... | |
25 | 27 |
static UserHandler user_handlers[USER_DEFINED_MSG_TOTAL]; |
26 | 28 |
|
27 | 29 |
static int robot_x, robot_y; |
28 |
static int server_xbee_id; // ID of server xbee; set when a colonet packet is received.
|
|
30 |
static int updated_robot_pos_ready;
|
|
29 | 31 |
|
30 | 32 |
/* Internal function prototypes */ |
31 | 33 |
static void packet_string_to_struct(ColonetRobotServerPacket* dest_pkt, unsigned char* pkt_buf); |
... | ... | |
48 | 50 |
|
49 | 51 |
robot_x = 0; |
50 | 52 |
robot_y = 0; |
51 |
server_xbee_id = -1;
|
|
53 |
updated_robot_pos_ready = 1;
|
|
52 | 54 |
|
53 | 55 |
return 0; |
54 | 56 |
} |
... | ... | |
82 | 84 |
wl_send_global_packet(colonet_pgh.groupCode, 0, (char*)&pkt, sizeof(pkt), 0); |
83 | 85 |
} |
84 | 86 |
|
85 |
static void move_to_position_routine(int x, int y) { |
|
86 |
/* TODO emarinel - control algorithm */ |
|
87 |
motor1_set(1, 200); |
|
88 |
motor2_set(1, 200); |
|
87 |
static int distsquared(int x1, int y1,int x2, int y2) { |
|
88 |
return (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1); |
|
89 | 89 |
} |
90 | 90 |
|
91 |
/* cubic approximation of arctan. */ |
|
92 |
static void arctan(float t) { |
|
93 |
float t3 = t*t*t; |
|
94 |
return -0.039 * t3 + 0.74 * t + 0.00011; |
|
95 |
} |
|
96 |
|
|
97 |
static void move_to_position_routine(int target_x, int target_y) { |
|
98 |
int last_x, last_y; |
|
99 |
|
|
100 |
do { |
|
101 |
while (!updated_robot_pos_ready) { // ghetto condition variable |
|
102 |
wl_do(); |
|
103 |
} |
|
104 |
|
|
105 |
int x_error = target_x - robot_x |
|
106 |
int y_error = target_y - robot_y; |
|
107 |
|
|
108 |
int x_diff = robot_x - last_x; |
|
109 |
int y_diff = robot_y - last_y; |
|
110 |
|
|
111 |
//float dir = arctan(y/x); |
|
112 |
|
|
113 |
|
|
114 |
int sum_x = |
|
115 |
|
|
116 |
|
|
117 |
last_x = robot_x; |
|
118 |
last_y = robot_y; |
|
119 |
|
|
120 |
motor1_set(1, 200); |
|
121 |
motor2_set(1, 200); |
|
122 |
|
|
123 |
while (distsquared(robot_x, robot_y, target_x, target_y) > |
|
124 |
TARGET_POSITION_STOP_DISTANCE_THRESHOLD*TARGET_POSITION_STOP_DISTANCE_THRESHOLD); |
|
125 |
} |
|
126 |
|
|
91 | 127 |
/* Private functions */ |
92 | 128 |
|
93 | 129 |
/** @brief Handles colonet packets. Should be called by parse_buffer |
... | ... | |
103 | 139 |
unsigned char* args; //up to 7 char args |
104 | 140 |
unsigned int int_args[3]; //up to 3 int (2-byte) args |
105 | 141 |
|
106 |
/* Globally store this id. */ |
|
107 |
server_xbee_id = wl_source; |
|
108 |
|
|
109 | 142 |
usb_puts("Packet received.\n"); |
110 | 143 |
char buf[40]; |
111 | 144 |
sprintf(buf, "length=%d\n", length); |
... | ... | |
198 | 231 |
robot_x = int_args[0]; |
199 | 232 |
robot_y = int_args[1]; |
200 | 233 |
|
234 |
updated_robot_pos_ready; |
|
235 |
|
|
201 | 236 |
sprintf(buf, "pos is: %d %d\n", robot_x, robot_y); |
202 | 237 |
usb_puts(buf); |
203 | 238 |
break; |
Also available in: Unified diff