Project

General

Profile

Revision 521

broken robot code

View differences:

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