Changeset 145
- Timestamp:
- 02/25/06 18:58:43 (3 years ago)
- Files:
-
- trunk/hal/lpc2106-cmucam3/serial.c (modified) (1 diff)
- trunk/lib/cc3_ilp/cc3_ilp.c (modified) (5 diffs)
- trunk/lib/cc3_ilp/cc3_ilp.h (modified) (1 diff)
- trunk/projects/cmucam2/Makefile (modified) (1 diff)
- trunk/projects/cmucam2/main.c (modified) (2 diffs)
Legend:
- Unmodified
- Added
- Removed
- Modified
- Copied
- Moved
trunk/hal/lpc2106-cmucam3/serial.c
r90 r145 154 154 155 155 156 157 156 void uart0_write (char *str) 158 157 { trunk/lib/cc3_ilp/cc3_ilp.c
r139 r145 2 2 #include "cc3_ilp.h" 3 3 #include <stdbool.h> 4 #include <stdio.h> 4 5 6 void cc3_send_image_direct(void) 7 { 8 uint32_t x, y; 9 uint32_t size_x, size_y; 5 10 11 cc3_set_led(1); 12 size_x=cc3_g_current_frame.width; 13 size_y=cc3_g_current_frame.height; 14 cc3_pixbuf_load (); 15 putchar (1); 16 putchar (size_x); 17 if (size_y > 255) 18 size_y = 255; 19 putchar (size_y); 20 for (y = 0; y < size_y; y++) { 21 putchar (2); 22 for (x = 0; x < size_x; x++) { 23 cc3_pixbuf_read (); 24 putchar (cc3_g_current_pixel.channel[CC3_RED]); 25 putchar (cc3_g_current_pixel.channel[CC3_GREEN]); 26 putchar (cc3_g_current_pixel.channel[CC3_BLUE]); 27 } 28 } 29 putchar (3); 30 } 6 31 7 32 uint8_t cc3_load_img_rows( cc3_image_t *img, uint16_t rows ) … … 58 83 int y, x; 59 84 60 if( (pkt->lower_bound ->channel[0]>pkt->upper_bound->channel[0]) ||61 (pkt->lower_bound ->channel[1]>pkt->upper_bound->channel[1]) ||62 (pkt->lower_bound ->channel[2]>pkt->upper_bound->channel[2]) ) return 0;85 if( (pkt->lower_bound.channel[0]>pkt->upper_bound.channel[0]) || 86 (pkt->lower_bound.channel[1]>pkt->upper_bound.channel[1]) || 87 (pkt->lower_bound.channel[2]>pkt->upper_bound.channel[2]) ) return 0; 63 88 pkt->num_pixels=0; 64 89 pkt->x0=UINT16_MAX; … … 68 93 mm_x=0; 69 94 mm_y=0; 95 96 cc3_pixbuf_load(); 70 97 for(y=0; y<cc3_g_current_frame.height; y++ ) 71 98 for(x=0; x<cc3_g_current_frame.width; x++ ) … … 74 101 cc3_pixbuf_read(); 75 102 if(cc3_g_current_frame.coi==CC3_ALL ) { 76 if(cc3_g_current_pixel.channel[0]>=pkt->lower_bound ->channel[0] &&77 cc3_g_current_pixel.channel[0]<=pkt->upper_bound ->channel[0] &&78 cc3_g_current_pixel.channel[1]>=pkt->lower_bound ->channel[1] &&79 cc3_g_current_pixel.channel[1]<=pkt->upper_bound ->channel[1] &&80 cc3_g_current_pixel.channel[2]>=pkt->lower_bound ->channel[2] &&81 cc3_g_current_pixel.channel[2]<=pkt->upper_bound ->channel[2] ) pixel_good=1;103 if(cc3_g_current_pixel.channel[0]>=pkt->lower_bound.channel[0] && 104 cc3_g_current_pixel.channel[0]<=pkt->upper_bound.channel[0] && 105 cc3_g_current_pixel.channel[1]>=pkt->lower_bound.channel[1] && 106 cc3_g_current_pixel.channel[1]<=pkt->upper_bound.channel[1] && 107 cc3_g_current_pixel.channel[2]>=pkt->lower_bound.channel[2] && 108 cc3_g_current_pixel.channel[2]<=pkt->upper_bound.channel[2] ) pixel_good=1; 82 109 } else 83 110 { 84 if(cc3_g_current_pixel.channel[cc3_g_current_frame.coi]>=pkt->lower_bound ->channel[cc3_g_current_frame.coi] &&85 cc3_g_current_pixel.channel[cc3_g_current_frame.coi]<=pkt->upper_bound ->channel[cc3_g_current_frame.coi] ) pixel_good=1;111 if(cc3_g_current_pixel.channel[cc3_g_current_frame.coi]>=pkt->lower_bound.channel[cc3_g_current_frame.coi] && 112 cc3_g_current_pixel.channel[cc3_g_current_frame.coi]<=pkt->upper_bound.channel[cc3_g_current_frame.coi] ) pixel_good=1; 86 113 } 87 114 … … 98 125 } 99 126 100 pkt->centroid_x= mm_x / cc3_g_current_frame.width;101 pkt->centroid_y= mm_y / cc3_g_current_frame.height;102 127 103 // FIXME: Density hack to keep it an integer104 128 if(pkt->num_pixels>0 ) 129 { 130 // FIXME: Density hack to keep it an integer 105 131 pkt->int_density=(pkt->num_pixels*1000) / ((pkt->x1 - pkt->x0)*(pkt->y1 - pkt->y0)); 106 else pkt->int_density=0; 132 pkt->centroid_x= mm_x / pkt->num_pixels; 133 pkt->centroid_y= mm_y / pkt->num_pixels; 134 135 } 136 else 137 { 138 pkt->int_density=0; 139 pkt->x0=0; 140 pkt->y0=0; 141 pkt->x1=0; 142 pkt->y1=0; 143 pkt->centroid_x=0; 144 pkt->centroid_y=0; 145 146 } 107 147 return 1; 108 148 } trunk/lib/cc3_ilp/cc3_ilp.h
r138 r145 17 17 uint32_t int_density; 18 18 uint8_t noise_filter; 19 cc3_pixel_t *upper_bound;20 cc3_pixel_t *lower_bound;19 cc3_pixel_t upper_bound; 20 cc3_pixel_t lower_bound; 21 21 } cc3_track_pkt_t; 22 22 23 void cc3_send_image_direct(void); 23 24 void cc3_get_pixel(cc3_image_t *img, uint16_t x, uint16_t y, cc3_pixel_t *out_pix); 24 25 void cc3_set_pixel(cc3_image_t *img, uint16_t x, uint16_t y, cc3_pixel_t *in_pix); trunk/projects/cmucam2/Makefile
r105 r145 2 2 INCLUDES= 3 3 PROJECT=cmucam2 4 LIBS=cc3_ilp 4 5 5 6 include ../../include/common.mk trunk/projects/cmucam2/main.c
r137 r145 1 #include "LPC2100.h"2 #include "cc3.h"3 #include "interrupt.h"1 #include <cc3.h> 2 #include <cc3_ilp.h> 3 #include <servo.h> 4 4 #include <math.h> 5 5 #include <stdbool.h> 6 6 #include <stdio.h> 7 #include "serial.h" 8 9 void image_send_direct (int size_x, int size_y); 10 11 12 int main () 13 { 14 unsigned int i = 0; 15 int val; 7 #include <string.h> 8 #include <ctype.h> 9 #include <stdlib.h> 10 11 #define MAX_ARGS 10 12 #define MAX_LINE 128 13 #define VERSION_BANNER "CMUcam2 v1.00 c6" 14 15 typedef enum { 16 RETURN, 17 RESET, 18 TRACK_COLOR, 19 SEND_FRAME, 20 FRAME_DIFF, 21 GET_VERSION, 22 GET_MEAN, 23 SET_SERVO, 24 CAMERA_REG, 25 POLL_MODE, 26 VIRTUAL_WINDOW, 27 DOWN_SAMPLE, 28 CMUCAM2_CMD_END // Must be last entry so array sizes are correct 29 } cmucam2_command_t; 30 31 char *cmucam2_cmds[CMUCAM2_CMD_END]; 32 33 //int32_t cmucam2_get_command(cmucam2_command_t *cmd, int32_t *arg_list); 34 int32_t cmucam2_get_command(int32_t *cmd, int32_t *arg_list); 35 void set_cmucam2_commands(void); 36 void print_ACK(void); 37 void print_NCK(void); 38 void cmucam2_write_t_packet(cc3_track_pkt_t *pkt); 39 40 int main (void) 41 { 42 int32_t command; 43 int32_t val,n; 44 uint32_t arg_list[MAX_ARGS]; 45 uint8_t error,poll_mode; 46 cc3_track_pkt_t t_pkt; 47 48 set_cmucam2_commands(); 49 50 cmucam2_start: 51 poll_mode=0; 16 52 cc3_system_setup (); 17 53 18 19 //disable_ext_interrupt ();20 //uart0_write("CMUcam3 v2 Starting up...\r");21 //system_setup ();22 54 cc3_uart0_init (115200,UART_8N1,UART_STDOUT); 23 // cc3_uart1_init (115200,UART_8N1,UART_STDOUT|UART_STDERR|UART_STDIN); 24 // cc3_camera_init (); 25 // 26 val=setvbuf(stdout, NULL, _IONBF, 0 ); 27 //fprintf( stderr, "This is a test...\n" ); 28 printf ("CMUcam3 Starting up\r\n"); 29 cc3_set_led (true); 30 31 val=0; 32 while(val!=2106) 33 { 34 printf( "Type a number, or 2106 to break...\r\n"); 35 scanf( "%d",&val ); 36 printf( "You typed %d\r\n",val); 37 } 38 /* for (i = 0; i < 50; i++) { 39 printf ("Try load\r\n"); 40 cc3_pixbuf_load (); 41 printf ("loaded..\r\n"); 42 }*/ 43 printf ("Sending Image\r\n"); 44 while (1) { 45 image_send_direct (cc3_g_current_frame.width, 46 cc3_g_current_frame.height); 55 val=setvbuf(stdout, NULL, _IONBF, 0 ); 56 57 cc3_camera_init (); 58 59 printf ("%s\r",VERSION_BANNER); 60 //cc3_set_led (true); 61 62 cc3_uart0_cr_lf(CC3_UART_CR_OR_LF); 63 64 cc3_servo_init(); 65 cc3_pixbuf_set_subsample( CC3_NEAREST, 2, 1 ); 66 67 while (1) { 68 printf( ":" ); 69 error=0; 70 n=cmucam2_get_command(&command, arg_list ); 71 if(n!=-1) 72 { 73 switch(command) 74 { 75 76 case RESET: 77 if(n!=0) { error=1; break; } else print_ACK(); 78 printf( "\r" ); 79 goto cmucam2_start; 80 break; 81 case GET_VERSION: 82 if(n!=0) { error=1; break; } else print_ACK(); 83 printf( "%s\r",VERSION_BANNER ); 84 break; 85 case POLL_MODE: 86 if(n!=1) { error=1; break; } else print_ACK(); 87 if(arg_list[0]==1 ) poll_mode=1; 88 else poll_mode=0; 89 break; 90 case SEND_FRAME: 91 if(n==1 && arg_list[0]>4) { error=1; break; } 92 else if(n>1) { error=1; break; } else print_ACK(); 93 cc3_send_image_direct(); 94 break; 95 case CAMERA_REG: 96 if(n%2!=0 || n<2) { error=1; break; } else print_ACK(); 97 for(int i=0; i<n; i+=2) 98 cc3_set_raw_register(arg_list[i], arg_list[i+1] ); 99 break; 100 case VIRTUAL_WINDOW: 101 if(n!=4) { error=1; break; } else print_ACK(); 102 cc3_pixbuf_set_roi( arg_list[0], arg_list[1], arg_list[2], arg_list[3] ); 103 break; 104 case DOWN_SAMPLE: 105 if(n!=2) { error=1; break; } else print_ACK(); 106 cc3_pixbuf_set_subsample( CC3_NEAREST, arg_list[0]+1, arg_list[1]); 107 break; 108 case TRACK_COLOR: 109 if(n!=0 && n!=6) { error=1; break; } else print_ACK(); 110 if(n==6) 111 { 112 t_pkt.lower_bound.channel[0]=arg_list[0]; 113 t_pkt.upper_bound.channel[0]=arg_list[1]; 114 t_pkt.lower_bound.channel[1]=arg_list[2]; 115 t_pkt.upper_bound.channel[1]=arg_list[3]; 116 t_pkt.lower_bound.channel[2]=arg_list[4]; 117 t_pkt.upper_bound.channel[2]=arg_list[5]; 118 } 119 do 120 { 121 cc3_track_color(&t_pkt); 122 cmucam2_write_t_packet(&t_pkt); 123 if(uart0_getc_nb()!=-1 ) break; 124 } while(poll_mode!=1); 125 break; 126 case SET_SERVO: 127 if(n!=2) { error=1; break; } else print_ACK(); 128 cc3_servo_set(arg_list[0], arg_list[1] ); 129 break; 130 default: 131 print_ACK(); 132 break; 47 133 } 134 135 } 136 else error=1; 137 138 if(error==1) print_NCK(); 139 140 } 48 141 49 142 … … 51 144 } 52 145 53 54 void image_send_direct (int size_x, int size_y) 55 { 56 int x, y; 57 cc3_pixbuf_load (); 58 putchar (1); 59 putchar (size_x); 60 if (size_y > 255) 61 size_y = 255; 62 putchar (size_y); 63 for (y = 0; y < size_y; y++) { 64 putchar (2); 65 for (x = 0; x < size_x; x++) { 66 cc3_pixbuf_read (); 67 putchar (cc3_g_current_pixel.channel[CC3_RED]); 68 putchar (cc3_g_current_pixel.channel[CC3_GREEN]); 69 putchar (cc3_g_current_pixel.channel[CC3_BLUE]); 146 void cmucam2_write_t_packet(cc3_track_pkt_t *pkt) 147 { 148 if(pkt->centroid_x>255) pkt->centroid_x=255; 149 if(pkt->centroid_y>255) pkt->centroid_y=255; 150 if(pkt->x0>255) pkt->x0=255; 151 if(pkt->x1>255) pkt->x1=255; 152 if(pkt->y1>255) pkt->y1=255; 153 if(pkt->y0>255) pkt->y0=255; 154 if(pkt->num_pixels>255) pkt->num_pixels=255; 155 if(pkt->int_density>255) pkt->int_density=255; 156 157 if( pkt->num_pixels==0 ) printf( "T 0 0 0 0 0 0 0 0\r" ); 158 else 159 printf( "T %d %d %d %d %d %d %d %d\r", pkt->centroid_x, pkt->centroid_y, 160 pkt->x0, pkt->y0, pkt->x1, pkt->y1, pkt->num_pixels, pkt->int_density ); 161 162 } 163 164 165 void print_ACK() 166 { 167 printf( "ACK\r" ); 168 } 169 170 void print_NCK() 171 { 172 printf( "NCK\r" ); 173 } 174 175 176 void set_cmucam2_commands(void) 177 { 178 cmucam2_cmds[RETURN]="**"; 179 cmucam2_cmds[RESET]="RS"; 180 cmucam2_cmds[TRACK_COLOR]="TC"; 181 cmucam2_cmds[SEND_FRAME]="SF"; 182 cmucam2_cmds[FRAME_DIFF]="FD"; 183 cmucam2_cmds[GET_VERSION]="GV"; 184 cmucam2_cmds[CAMERA_REG]="CR"; 185 cmucam2_cmds[POLL_MODE]="PM"; 186 cmucam2_cmds[GET_MEAN]="GM"; 187 cmucam2_cmds[SET_SERVO]="SV"; 188 cmucam2_cmds[VIRTUAL_WINDOW]="VW"; 189 cmucam2_cmds[DOWN_SAMPLE]="DS"; 190 } 191 192 //int32_t cmucam2_get_command(cmucam2_command_t *cmd, int32_t *arg_list) 193 int32_t cmucam2_get_command(int32_t *cmd, int32_t *arg_list) 194 { 195 char line_buf[MAX_LINE]; 196 char c; 197 char *token; 198 int32_t fail,length,argc; 199 uint32_t i; 200 201 fail=0; 202 length=0; 203 *cmd=0; 204 c=0; 205 while(c!='\r' && c!='\n' ) 206 { 207 c=getchar(); 208 if(length<(MAX_LINE-1)) 209 { 210 line_buf[length]=c; 211 length++; 212 } 213 else fail=1; 214 } 215 // wait until a return and then fail 216 if(fail==1) return -1; 217 line_buf[length]='\0'; 218 219 if(line_buf[0]=='\r' || line_buf[0]=='\n' ) 220 { 221 *cmd=RETURN; 222 return 0; 223 } 224 225 token = strtok( line_buf, " \r\n" ); 226 227 if(token==NULL ) return -1; 228 for(i=0; i<strlen(token); i++ ) 229 token[i]=toupper(token[i]); 230 fail=1; 231 for(i=0; i<CMUCAM2_CMD_END; i++ ) 232 { 233 if(strcmp(token, cmucam2_cmds[i])==0 ) 234 { 235 fail=0; 236 *cmd=i; 237 break; 238 } 239 240 } 241 if(fail==1) return -1; 242 argc=0; 243 while (1) 244 { 245 /* extract string from string sequence */ 246 token = strtok(NULL, " \r\n"); 247 /* check if there is nothing else to extract */ 248 if (token==NULL ) 249 { 250 // printf("Tokenizing complete\n"); 251 return argc; 252 } 253 for(i=0; i<strlen(token); i++ ) 254 { 255 if(!isdigit(token[i])) return -1; 256 } 257 arg_list[argc]=atoi(token); 258 argc++; 70 259 } 71 } 72 putchar (3); 73 fflush (stdout); 74 } 260 261 return -1; 262 263 264 } 265 266
