Project

General

Profile

Revision 775

Added by David Schultz almost 16 years ago

implemented averaging with older data to smooth out object detection values

View differences:

trunk/code/projects/object_manipulation/obj_detect_swarm/driver.c
59 59 
#ifdef COLOR_DETECT
60 60 
  /* set up color detect */
61 61 
  int rgb[3] = {0,0,0};
62 
  int rgb_old1[3] = {-1,0,0};
63 
  int rgb_old2[3] = {-1,0,0};
64 
  int rgb_avg[3] = {0,0,0};
62 65
63 66 
	//long int distance_red;
64 67 
	//long int distance_green;
......
112 115
113 116 
		digital_output(GREEN_LED,0);
114 117
115 
    ratio_gb = rgb[1] - rgb[2];
118 
    // check with older values
119 
    if (rgb_old1[0] != -1 && rgb_old2[0] != -1) {
120 
      rgb_avg[0] = (rgb[0]+rgb_old1[0]+rgb_old2[0])/3;
121 
      rgb_avg[1] = (rgb[1]+rgb_old1[1]+rgb_old2[1])/3;
122 
      rgb_avg[2] = (rgb[2]+rgb_old1[2]+rgb_old2[2])/3;
123 
    } else {
124 
      rgb_avg[0] = rgb[0];
125 
      rgb_avg[1] = rgb[1];
126 
      rgb_avg[2] = rgb[2];
127 
    }
128
129 
    if (rgb_old1[0] == -1) {
130 
      rgb_old1[0] = rgb[0];
131 
      rgb_old1[1] = rgb[1];
132 
      rgb_old1[2] = rgb[2];
133 
    } else {
134 
      if (rgb_old1[0] < rgb[0]+20
135 
        && rgb_old1[0] > rgb[0]-20) {
136 
        rgb_old1[0] = rgb[0];
137 
        rgb_old2[0] = rgb_old1[0];
138 
      }
139 
      if (rgb_old1[1] < rgb[1]+20
140 
        && rgb_old1[1] > rgb[1]-20) {
141 
        rgb_old1[1] = rgb[1];
142 
        rgb_old2[1] = rgb_old1[1];
143 
      }
144 
      if (rgb_old1[2] < rgb[2]+20
145 
        && rgb_old1[2] > rgb[2]-20) {
146 
        rgb_old1[2] = rgb[2];
147 
        rgb_old2[2] = rgb_old1[2];
148 
      }
149 
    }
150
151 
    // check ratio
152 
    ratio_gb = rgb_avg[1] - rgb_avg[2];
116 153 
    if (ratio_gb < -20)
117 154 
      ratio_gb = 2;
118 155 
    else if (ratio_gb > 20)
......
120 157 
    else
121 158 
      ratio_gb = 1;
122 159
123 
    if (rgb[0] > 200 && rgb[1] > 200 && rgb[2] > 200) {
160 
    if (rgb_avg[0] > 200 && rgb_avg[1] > 200 && rgb_avg[2] > 200) {
124 161 
      // we think this is open area
125 162 
      move(170,0);
126 163 
    } else if (ratio_gb == COLOR_WALL) {
......
144 181 
		distance_green = distance_green * rgb[0]/255;*/
145 182
146 183 
    usb_puts("R:");
147 
		usb_puti(rgb[0]);
184 
		usb_puti(rgb_avg[0]);
148 185 
		usb_puts("\tG:");
149 
		usb_puti(rgb[1]);
186 
		usb_puti(rgb_avg[1]);
150 187 
		usb_puts("\tB:");
151 
		usb_puti(rgb[2]);
188 
		usb_puti(rgb_avg[2]);
152 189 
		usb_puts("\t\tratio_gb:");
153 190 
    usb_puti(ratio_gb);
154 191 
    usb_puts("\r\n");
......
177 214 
  }
178 215
179 216 
  // object manip goes here
217 
  orb_set_color(YELLOW);
180 218 
  while(1) {
181 219 
    wl_do();
182 220 
  }

Also available in: Unified diff