Changeset 145

Show
Ignore:
Timestamp:
02/25/06 18:58:43 (3 years ago)
Author:
agr
Message:

CMUcam2 coming into form...

Files:

Legend:

Unmodified
Added
Removed
Modified
Copied
Moved
  • trunk/hal/lpc2106-cmucam3/serial.c

    r90 r145  
    154154 
    155155 
    156  
    157156void uart0_write (char *str) 
    158157{ 
  • trunk/lib/cc3_ilp/cc3_ilp.c

    r139 r145  
    22#include "cc3_ilp.h" 
    33#include <stdbool.h> 
     4#include <stdio.h> 
    45 
     6void cc3_send_image_direct(void) 
     7{ 
     8    uint32_t x, y; 
     9    uint32_t size_x, size_y; 
    510 
     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} 
    631 
    732uint8_t cc3_load_img_rows( cc3_image_t *img, uint16_t rows ) 
     
    5883 int y, x; 
    5984 
    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; 
    6388pkt->num_pixels=0; 
    6489pkt->x0=UINT16_MAX; 
     
    6893mm_x=0; 
    6994mm_y=0; 
     95 
     96cc3_pixbuf_load(); 
    7097for(y=0; y<cc3_g_current_frame.height; y++ ) 
    7198for(x=0; x<cc3_g_current_frame.width; x++ ) 
     
    74101        cc3_pixbuf_read(); 
    75102        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;  
    82109        } else 
    83110        { 
    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;  
    86113        } 
    87114 
     
    98125} 
    99126 
    100 pkt->centroid_x= mm_x / cc3_g_current_frame.width; 
    101 pkt->centroid_y= mm_y / cc3_g_current_frame.height; 
    102127 
    103 // FIXME:  Density hack to keep it an integer 
    104128if(pkt->num_pixels>0 ) 
     129{ 
     130        // FIXME:  Density hack to keep it an integer 
    105131        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
     136else  
     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
    107147return 1; 
    108148} 
  • trunk/lib/cc3_ilp/cc3_ilp.h

    r138 r145  
    1717    uint32_t int_density; 
    1818    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; 
    2121} cc3_track_pkt_t; 
    2222 
     23void cc3_send_image_direct(void); 
    2324void cc3_get_pixel(cc3_image_t *img, uint16_t x, uint16_t y, cc3_pixel_t *out_pix); 
    2425void cc3_set_pixel(cc3_image_t *img, uint16_t x, uint16_t y, cc3_pixel_t *in_pix); 
  • trunk/projects/cmucam2/Makefile

    r105 r145  
    22INCLUDES= 
    33PROJECT=cmucam2 
     4LIBS=cc3_ilp 
    45 
    56include ../../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> 
    44#include <math.h> 
    55#include <stdbool.h> 
    66#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 
     15typedef 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 
     31char *cmucam2_cmds[CMUCAM2_CMD_END]; 
     32 
     33//int32_t cmucam2_get_command(cmucam2_command_t *cmd, int32_t *arg_list); 
     34int32_t cmucam2_get_command(int32_t *cmd, int32_t *arg_list); 
     35void set_cmucam2_commands(void); 
     36void print_ACK(void); 
     37void print_NCK(void); 
     38void cmucam2_write_t_packet(cc3_track_pkt_t *pkt); 
     39 
     40int main (void) 
     41
     42int32_t command; 
     43int32_t val,n; 
     44uint32_t arg_list[MAX_ARGS]; 
     45uint8_t error,poll_mode; 
     46cc3_track_pkt_t t_pkt; 
     47     
     48    set_cmucam2_commands(); 
     49 
     50    cmucam2_start:  
     51    poll_mode=0; 
    1652    cc3_system_setup (); 
    1753 
    18  
    19     //disable_ext_interrupt (); 
    20     //uart0_write("CMUcam3 v2 Starting up...\r"); 
    21     //system_setup (); 
    2254    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; 
    47133    } 
     134     
     135    } 
     136    else error=1; 
     137     
     138    if(error==1) print_NCK();  
     139 
     140    } 
    48141 
    49142 
     
    51144} 
    52145 
    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]); 
     146void cmucam2_write_t_packet(cc3_track_pkt_t *pkt) 
     147
     148if(pkt->centroid_x>255) pkt->centroid_x=255; 
     149if(pkt->centroid_y>255) pkt->centroid_y=255; 
     150if(pkt->x0>255) pkt->x0=255; 
     151if(pkt->x1>255) pkt->x1=255; 
     152if(pkt->y1>255) pkt->y1=255; 
     153if(pkt->y0>255) pkt->y0=255; 
     154if(pkt->num_pixels>255) pkt->num_pixels=255; 
     155if(pkt->int_density>255) pkt->int_density=255; 
     156 
     157if( pkt->num_pixels==0 ) printf( "T 0 0 0 0 0 0 0 0\r" ); 
     158else 
     159printf( "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 
     165void print_ACK() 
     166
     167printf( "ACK\r" ); 
     168
     169 
     170void print_NCK() 
     171
     172printf( "NCK\r" ); 
     173
     174 
     175 
     176void set_cmucam2_commands(void) 
     177
     178cmucam2_cmds[RETURN]="**";  
     179cmucam2_cmds[RESET]="RS";  
     180cmucam2_cmds[TRACK_COLOR]="TC";  
     181cmucam2_cmds[SEND_FRAME]="SF";   
     182cmucam2_cmds[FRAME_DIFF]="FD";   
     183cmucam2_cmds[GET_VERSION]="GV"; 
     184cmucam2_cmds[CAMERA_REG]="CR";   
     185cmucam2_cmds[POLL_MODE]="PM";  
     186cmucam2_cmds[GET_MEAN]="GM";  
     187cmucam2_cmds[SET_SERVO]="SV";  
     188cmucam2_cmds[VIRTUAL_WINDOW]="VW";  
     189cmucam2_cmds[DOWN_SAMPLE]="DS";  
     190
     191 
     192//int32_t cmucam2_get_command(cmucam2_command_t *cmd, int32_t *arg_list) 
     193int32_t cmucam2_get_command(int32_t *cmd, int32_t *arg_list) 
     194
     195char line_buf[MAX_LINE]; 
     196char c; 
     197char *token; 
     198int32_t fail,length,argc; 
     199uint32_t i; 
     200 
     201fail=0; 
     202length=0; 
     203*cmd=0; 
     204c=0; 
     205while(c!='\r' &&  c!='\n' ) 
     206
     207c=getchar(); 
     208if(length<(MAX_LINE-1))  
     209        { 
     210        line_buf[length]=c; 
     211        length++; 
     212        } 
     213else fail=1; 
     214
     215// wait until a return and then fail 
     216if(fail==1) return -1; 
     217line_buf[length]='\0'; 
     218 
     219if(line_buf[0]=='\r' || line_buf[0]=='\n' ) 
     220        { 
     221        *cmd=RETURN; 
     222        return 0; 
     223        } 
     224 
     225token = strtok( line_buf, " \r\n" ); 
     226 
     227if(token==NULL ) return -1; 
     228for(i=0; i<strlen(token); i++ ) 
     229        token[i]=toupper(token[i]); 
     230fail=1; 
     231for(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        } 
     241if(fail==1) return -1; 
     242argc=0; 
     243while (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++;  
    70259        } 
    71     } 
    72     putchar (3); 
    73     fflush (stdout); 
    74 
     260 
     261return -1; 
     262 
     263 
     264
     265 
     266