File content as of revision 1:d721e32cf79c:

#include "mbed.h"
#include "Camera_LS_Y201.h"
#include "SimpleJpegDecode.h"
#include "bmp24.h"
#include "Servo.h"
#include "motordriver.h"

LocalFileSystem fs("local");
 
#define DEBMSG      printf
#define NEWLINE()   printf("\r\n")
 
#define FILENAME    "/local/IMG_%04d.jpg"

#define INPUT_FILE  "/local/IMG_0000.jpg"
#define OUTPUT_FILE "/local/IMG_0000.bmp"

Camera_LS_Y201 cam1(p13, p14);


SimpleJpegDecode decode;

bmp24 bmp;
 
typedef struct work {
    FILE *fp;
} work_t;
 
work_t work;

Motor m(p21, p19, p20, 1);
Servo Panning(p22);
Servo Latching(p23);



void Latch()
{
    Latching = 0.4;
    wait(2);
}

void Fire()
{
    Latching = 0.001;
    wait(2);
}

void dcreload()
{
    for (float s= 0; s < 1.0 ; s += 0.1) {
        m.speed(s); // functions in libarary on https://mbed.org/cookbook/Motor
        wait(0.22);
    }
    for (float s= 1; s > 0 ; s -= 0.1) {
        m.speed(s); // functions in libarary on https://mbed.org/cookbook/Motor
        wait(0.22);
    }    
    m.stop(0.5);
}
void dcrelease()
{
    for (float s= 0; s > -1.0 ; s -= 0.1) {
        m.speed(s); // functions in libarary on https://mbed.org/cookbook/Motor
        wait(0.15);
    }
    for (float s= -1; s < 0 ; s += 0.1) {
        m.speed(s); // functions in libarary on https://mbed.org/cookbook/Motor
        wait(0.14);
    }    
    m.stop(0.5);
}


void callbackRGB(int x, int y, uint8_t* rgb)
{
    bmp.point(x, y, rgb);
}

void convert_to_bmp(){
    printf("%s\r\n", __FILE__);

    bmp.clear();
    decode.setOnResult(callbackRGB);
    decode.clear();
    //printf("input: %s\r\n", INPUT_FILE);
    FILE* fp = fopen(INPUT_FILE, "rb");
    if (fp == NULL) {
         error("open error\r\n");
    }
    while(!feof(fp)) {
        int c = fgetc(fp);
        decode.input(c);
    }
    fclose(fp);
    //printf("output: %s\r\n", OUTPUT_FILE);
    if (!bmp.writeFile(OUTPUT_FILE)) {
        error("write error\r\n");
    }
}    


 
/**
 * Callback function for readJpegFileContent.
 *
 * @param buf A pointer to a buffer.
 * @param siz A size of the buffer.
 */
void callback_func(int done, int total, uint8_t *buf, size_t siz) {
    fwrite(buf, siz, 1, work.fp);
 
    static int n = 0;
    int tmp = done * 100 / total;
    if (n != tmp) {
        n = tmp;
        //DEBMSG("Writing...: %3d%%", n);
        //NEWLINE();
    }
}
 
/**
 * Capture.
 *
 * @param cam A pointer to a camera object.
 * @param filename The file name.
 *
 * @return Return 0 if it succeed.
 */
int capture(Camera_LS_Y201 *cam, char *filename) {
    /*
     * Take a picture.
     */
    if (cam->takePicture() != 0) {
        return -1;
    }
    //DEBMSG("Captured.");
    //NEWLINE();
 
    /*
     * Open file.
     */
    work.fp = fopen(filename, "wb");
    if (work.fp == NULL) {
        return -2;
    }
 
    /*
     * Read the content.
     */
    //DEBMSG("%s", filename);
   // NEWLINE();
    if (cam->readJpegFileContent(callback_func) != 0) {
        fclose(work.fp);
        return -3;
    }
    fclose(work.fp);
 
    /*
     * Stop taking pictures.
     */
    cam->stopTakingPictures();
 
    return 0;
}
 
 
int read_picture()
{
	 // super-simplified BMP read algorithm to pull out RGB data
	 // read image for coloring scheme
	 
	 int width = 64;	 
	 int height = 48;

	 //char image[height][width][3]; // first number here is 1024 pixels in my image, 3 is for RGB values
	 FILE *streamIn;
	 streamIn = fopen("/local/IMG_0000.bmp", "r");
	 if (streamIn == (FILE *)0){
	   printf("File opening error ocurred. Exiting program.\n");
	   exit(0);
	 }
	 int i = 0;
	 int byte = 0;
	 int count = 0;

     int red = 0;
     int green = 0;
     int blue = 0;
	 
	 int left = 0;
	 int middle = 0;
	 int right = 0;
	 
	 int sum = 0;
	 
	 
	 for(i=0;i<54;i++) byte = getc(streamIn);  // strip out BMP header THIS IS THE KEY
	
	 for(int y=0;y<height;y++){
 		 for(int x=0;x<width;x++){
		    
		    blue = getc(streamIn);  // use BMP 24bit with no alpha channel
		    green = getc(streamIn);  // BMP uses BGR but we want RGB, grab byte-by-byte
		    red = getc(streamIn);  // reverse-order array indexing fixes RGB issue...
		    		    
			
			if ((red > 150) && (green < 150) && (blue < 150)) // IS the pixel red??
			{
		    	//printf("red ");
		    	if (x < 21) left++;
		    	else if ((x > 21) && (x < 42)) middle++;
		    	else right++;	 
	    	}  
	    	count++; 
		 }
	 }

	fclose(streamIn);
	
	sum = left + right + middle;

	printf("Total: %i\r\n", sum);

	return sum;
	
}

void camera_init(){
	   
  	printf("\r\n\n\n");
    cam1.setImageSize(ImageSize160x120);
    DEBMSG("Camera module");
    NEWLINE();
    DEBMSG("Resetting...");
    NEWLINE();
    wait(1);
 
    if (cam1.reset() == 0) {
        DEBMSG("Reset OK.");
        NEWLINE();
    } else {
        DEBMSG("Reset fail.");
        NEWLINE();
        error("Reset fail.");
    }
    //wait(1);
}

void take_picture(){
    char fname[64];
	int cnt = 0;    
    snprintf(fname, sizeof(fname) - 1, FILENAME, cnt);
    int r = capture(&cam1, fname);
    if (r == 0) {
        ;
        //DEBMSG("[%04d]:OK.", cnt);
        //NEWLINE();
    } else {
        DEBMSG("[%04d]:NG. (code=%d)", cnt, r);
        NEWLINE();
        error("Failure.");
	}	
}


int main()
{
	int sum = 0;
 	camera_init();

	int fired = 0;

	Latch();


	while(1){
		fired = 0;
		sum = 0;
		for(float i = 0.1; i< .9; i+= .10){ //Scans left to right
	        Panning = i;
	        wait(.01);
	        
	     	take_picture();
			convert_to_bmp();   
			sum = read_picture(); // Returns total number of red pixels
	     	printf("sum that matters: %i\r\n", sum);
	     	if (sum > 400){ // If the number of red pixels is above the threshold: Fire and reload
				i-=.1;
				Panning = i;
				Fire();
				dcreload();
				Latch();
				dcrelease();
				fired = 1;
				break;
			}   
   		}
		
		
		
		for(float i = 0.9; i> .1; i-= .10){ //Scans right to left
	        if (fired == 1)
	        	break;
	        	
	        Panning = i;
	        wait(.01);
	        
	     	take_picture();
			convert_to_bmp();   
			sum = read_picture();   // Returns total number of red pixels
	     	
	     	if (sum > 400){ // If the number of red pixels is above the threshold: Fire and reload
				i+=.1;
				Panning = i;
				Fire();
				dcreload();
				Latch();
				dcrelease();
				break;
			}   
   		}		
			   
	}

}