Kyle Post / Mbed 2 deprecated Color_Targeting_Catapult

Any changes are to allow conversion to BMP

Dependencies:   BaseJpegDecode Camera_LS_Y201 Motordriver Servo mbed

Fork of Bitmap_copy_copy by Eric Wieser

main.cpp@1:d721e32cf79c, 2014-04-30 (annotated)

Committer:
kylepost3
Date:
Wed Apr 30 14:19:33 2014 +0000
Revision:
1:d721e32cf79c
Parent:
0:ded454e83f81 
Final design project for ECE 4180.  The device takes scans left to right taking pictures (In JPEG), converts the picture to BMP, and scans for red pixels.  When the threshold of red pixels is reached, it fires the catapult and reloads.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
EricWieser 0:ded454e83f81 1 #include "mbed.h"
kylepost3 1:d721e32cf79c 2 #include "Camera_LS_Y201.h"
kylepost3 1:d721e32cf79c 3 #include "SimpleJpegDecode.h"
kylepost3 1:d721e32cf79c 4 #include "bmp24.h"
kylepost3 1:d721e32cf79c 5 #include "Servo.h"
kylepost3 1:d721e32cf79c 6 #include "motordriver.h"
kylepost3 1:d721e32cf79c 7
kylepost3 1:d721e32cf79c 8 LocalFileSystem fs("local");
kylepost3 1:d721e32cf79c 9
kylepost3 1:d721e32cf79c 10 #define DEBMSG printf
kylepost3 1:d721e32cf79c 11 #define NEWLINE() printf("\r\n")
kylepost3 1:d721e32cf79c 12
kylepost3 1:d721e32cf79c 13 #define FILENAME "/local/IMG_%04d.jpg"
kylepost3 1:d721e32cf79c 14
kylepost3 1:d721e32cf79c 15 #define INPUT_FILE "/local/IMG_0000.jpg"
kylepost3 1:d721e32cf79c 16 #define OUTPUT_FILE "/local/IMG_0000.bmp"
kylepost3 1:d721e32cf79c 17
kylepost3 1:d721e32cf79c 18 Camera_LS_Y201 cam1(p13, p14);
kylepost3 1:d721e32cf79c 19
kylepost3 1:d721e32cf79c 20
kylepost3 1:d721e32cf79c 21 SimpleJpegDecode decode;
kylepost3 1:d721e32cf79c 22
kylepost3 1:d721e32cf79c 23 bmp24 bmp;
kylepost3 1:d721e32cf79c 24
kylepost3 1:d721e32cf79c 25 typedef struct work {
kylepost3 1:d721e32cf79c 26 FILE *fp;
kylepost3 1:d721e32cf79c 27 } work_t;
kylepost3 1:d721e32cf79c 28
kylepost3 1:d721e32cf79c 29 work_t work;
kylepost3 1:d721e32cf79c 30
kylepost3 1:d721e32cf79c 31 Motor m(p21, p19, p20, 1);
kylepost3 1:d721e32cf79c 32 Servo Panning(p22);
kylepost3 1:d721e32cf79c 33 Servo Latching(p23);
kylepost3 1:d721e32cf79c 34
kylepost3 1:d721e32cf79c 35
kylepost3 1:d721e32cf79c 36
kylepost3 1:d721e32cf79c 37 void Latch()
kylepost3 1:d721e32cf79c 38 {
kylepost3 1:d721e32cf79c 39 Latching = 0.4;
kylepost3 1:d721e32cf79c 40 wait(2);
kylepost3 1:d721e32cf79c 41 }
kylepost3 1:d721e32cf79c 42
kylepost3 1:d721e32cf79c 43 void Fire()
kylepost3 1:d721e32cf79c 44 {
kylepost3 1:d721e32cf79c 45 Latching = 0.001;
kylepost3 1:d721e32cf79c 46 wait(2);
kylepost3 1:d721e32cf79c 47 }
kylepost3 1:d721e32cf79c 48
kylepost3 1:d721e32cf79c 49 void dcreload()
kylepost3 1:d721e32cf79c 50 {
kylepost3 1:d721e32cf79c 51 for (float s= 0; s < 1.0 ; s += 0.1) {
kylepost3 1:d721e32cf79c 52 m.speed(s); // functions in libarary on https://mbed.org/cookbook/Motor
kylepost3 1:d721e32cf79c 53 wait(0.22);
kylepost3 1:d721e32cf79c 54 }
kylepost3 1:d721e32cf79c 55 for (float s= 1; s > 0 ; s -= 0.1) {
kylepost3 1:d721e32cf79c 56 m.speed(s); // functions in libarary on https://mbed.org/cookbook/Motor
kylepost3 1:d721e32cf79c 57 wait(0.22);
kylepost3 1:d721e32cf79c 58 }
kylepost3 1:d721e32cf79c 59 m.stop(0.5);
kylepost3 1:d721e32cf79c 60 }
kylepost3 1:d721e32cf79c 61 void dcrelease()
kylepost3 1:d721e32cf79c 62 {
kylepost3 1:d721e32cf79c 63 for (float s= 0; s > -1.0 ; s -= 0.1) {
kylepost3 1:d721e32cf79c 64 m.speed(s); // functions in libarary on https://mbed.org/cookbook/Motor
kylepost3 1:d721e32cf79c 65 wait(0.15);
kylepost3 1:d721e32cf79c 66 }
kylepost3 1:d721e32cf79c 67 for (float s= -1; s < 0 ; s += 0.1) {
kylepost3 1:d721e32cf79c 68 m.speed(s); // functions in libarary on https://mbed.org/cookbook/Motor
kylepost3 1:d721e32cf79c 69 wait(0.14);
kylepost3 1:d721e32cf79c 70 }
kylepost3 1:d721e32cf79c 71 m.stop(0.5);
kylepost3 1:d721e32cf79c 72 }
kylepost3 1:d721e32cf79c 73
kylepost3 1:d721e32cf79c 74
kylepost3 1:d721e32cf79c 75 void callbackRGB(int x, int y, uint8_t* rgb)
kylepost3 1:d721e32cf79c 76 {
kylepost3 1:d721e32cf79c 77 bmp.point(x, y, rgb);
kylepost3 1:d721e32cf79c 78 }
kylepost3 1:d721e32cf79c 79
kylepost3 1:d721e32cf79c 80 void convert_to_bmp(){
kylepost3 1:d721e32cf79c 81 printf("%s\r\n", __FILE__);
kylepost3 1:d721e32cf79c 82
kylepost3 1:d721e32cf79c 83 bmp.clear();
kylepost3 1:d721e32cf79c 84 decode.setOnResult(callbackRGB);
kylepost3 1:d721e32cf79c 85 decode.clear();
kylepost3 1:d721e32cf79c 86 //printf("input: %s\r\n", INPUT_FILE);
kylepost3 1:d721e32cf79c 87 FILE* fp = fopen(INPUT_FILE, "rb");
kylepost3 1:d721e32cf79c 88 if (fp == NULL) {
kylepost3 1:d721e32cf79c 89 error("open error\r\n");
kylepost3 1:d721e32cf79c 90 }
kylepost3 1:d721e32cf79c 91 while(!feof(fp)) {
kylepost3 1:d721e32cf79c 92 int c = fgetc(fp);
kylepost3 1:d721e32cf79c 93 decode.input(c);
kylepost3 1:d721e32cf79c 94 }
kylepost3 1:d721e32cf79c 95 fclose(fp);
kylepost3 1:d721e32cf79c 96 //printf("output: %s\r\n", OUTPUT_FILE);
kylepost3 1:d721e32cf79c 97 if (!bmp.writeFile(OUTPUT_FILE)) {
kylepost3 1:d721e32cf79c 98 error("write error\r\n");
kylepost3 1:d721e32cf79c 99 }
kylepost3 1:d721e32cf79c 100 }
kylepost3 1:d721e32cf79c 101
EricWieser 0:ded454e83f81 102
kylepost3 1:d721e32cf79c 103
kylepost3 1:d721e32cf79c 104 /**
kylepost3 1:d721e32cf79c 105 * Callback function for readJpegFileContent.
kylepost3 1:d721e32cf79c 106 *
kylepost3 1:d721e32cf79c 107 * @param buf A pointer to a buffer.
kylepost3 1:d721e32cf79c 108 * @param siz A size of the buffer.
kylepost3 1:d721e32cf79c 109 */
kylepost3 1:d721e32cf79c 110 void callback_func(int done, int total, uint8_t *buf, size_t siz) {
kylepost3 1:d721e32cf79c 111 fwrite(buf, siz, 1, work.fp);
kylepost3 1:d721e32cf79c 112
kylepost3 1:d721e32cf79c 113 static int n = 0;
kylepost3 1:d721e32cf79c 114 int tmp = done * 100 / total;
kylepost3 1:d721e32cf79c 115 if (n != tmp) {
kylepost3 1:d721e32cf79c 116 n = tmp;
kylepost3 1:d721e32cf79c 117 //DEBMSG("Writing...: %3d%%", n);
kylepost3 1:d721e32cf79c 118 //NEWLINE();
kylepost3 1:d721e32cf79c 119 }
kylepost3 1:d721e32cf79c 120 }
kylepost3 1:d721e32cf79c 121
kylepost3 1:d721e32cf79c 122 /**
kylepost3 1:d721e32cf79c 123 * Capture.
kylepost3 1:d721e32cf79c 124 *
kylepost3 1:d721e32cf79c 125 * @param cam A pointer to a camera object.
kylepost3 1:d721e32cf79c 126 * @param filename The file name.
kylepost3 1:d721e32cf79c 127 *
kylepost3 1:d721e32cf79c 128 * @return Return 0 if it succeed.
kylepost3 1:d721e32cf79c 129 */
kylepost3 1:d721e32cf79c 130 int capture(Camera_LS_Y201 *cam, char *filename) {
kylepost3 1:d721e32cf79c 131 /*
kylepost3 1:d721e32cf79c 132 * Take a picture.
kylepost3 1:d721e32cf79c 133 */
kylepost3 1:d721e32cf79c 134 if (cam->takePicture() != 0) {
kylepost3 1:d721e32cf79c 135 return -1;
kylepost3 1:d721e32cf79c 136 }
kylepost3 1:d721e32cf79c 137 //DEBMSG("Captured.");
kylepost3 1:d721e32cf79c 138 //NEWLINE();
kylepost3 1:d721e32cf79c 139
kylepost3 1:d721e32cf79c 140 /*
kylepost3 1:d721e32cf79c 141 * Open file.
kylepost3 1:d721e32cf79c 142 */
kylepost3 1:d721e32cf79c 143 work.fp = fopen(filename, "wb");
kylepost3 1:d721e32cf79c 144 if (work.fp == NULL) {
kylepost3 1:d721e32cf79c 145 return -2;
kylepost3 1:d721e32cf79c 146 }
kylepost3 1:d721e32cf79c 147
kylepost3 1:d721e32cf79c 148 /*
kylepost3 1:d721e32cf79c 149 * Read the content.
kylepost3 1:d721e32cf79c 150 */
kylepost3 1:d721e32cf79c 151 //DEBMSG("%s", filename);
kylepost3 1:d721e32cf79c 152 // NEWLINE();
kylepost3 1:d721e32cf79c 153 if (cam->readJpegFileContent(callback_func) != 0) {
kylepost3 1:d721e32cf79c 154 fclose(work.fp);
kylepost3 1:d721e32cf79c 155 return -3;
kylepost3 1:d721e32cf79c 156 }
kylepost3 1:d721e32cf79c 157 fclose(work.fp);
kylepost3 1:d721e32cf79c 158
kylepost3 1:d721e32cf79c 159 /*
kylepost3 1:d721e32cf79c 160 * Stop taking pictures.
kylepost3 1:d721e32cf79c 161 */
kylepost3 1:d721e32cf79c 162 cam->stopTakingPictures();
kylepost3 1:d721e32cf79c 163
kylepost3 1:d721e32cf79c 164 return 0;
kylepost3 1:d721e32cf79c 165 }
kylepost3 1:d721e32cf79c 166
kylepost3 1:d721e32cf79c 167
kylepost3 1:d721e32cf79c 168 int read_picture()
kylepost3 1:d721e32cf79c 169 {
kylepost3 1:d721e32cf79c 170 // super-simplified BMP read algorithm to pull out RGB data
kylepost3 1:d721e32cf79c 171 // read image for coloring scheme
kylepost3 1:d721e32cf79c 172
kylepost3 1:d721e32cf79c 173 int width = 64;
kylepost3 1:d721e32cf79c 174 int height = 48;
kylepost3 1:d721e32cf79c 175
kylepost3 1:d721e32cf79c 176 //char image[height][width][3]; // first number here is 1024 pixels in my image, 3 is for RGB values
kylepost3 1:d721e32cf79c 177 FILE *streamIn;
kylepost3 1:d721e32cf79c 178 streamIn = fopen("/local/IMG_0000.bmp", "r");
kylepost3 1:d721e32cf79c 179 if (streamIn == (FILE *)0){
kylepost3 1:d721e32cf79c 180 printf("File opening error ocurred. Exiting program.\n");
kylepost3 1:d721e32cf79c 181 exit(0);
kylepost3 1:d721e32cf79c 182 }
kylepost3 1:d721e32cf79c 183 int i = 0;
kylepost3 1:d721e32cf79c 184 int byte = 0;
kylepost3 1:d721e32cf79c 185 int count = 0;
kylepost3 1:d721e32cf79c 186
kylepost3 1:d721e32cf79c 187 int red = 0;
kylepost3 1:d721e32cf79c 188 int green = 0;
kylepost3 1:d721e32cf79c 189 int blue = 0;
kylepost3 1:d721e32cf79c 190
kylepost3 1:d721e32cf79c 191 int left = 0;
kylepost3 1:d721e32cf79c 192 int middle = 0;
kylepost3 1:d721e32cf79c 193 int right = 0;
kylepost3 1:d721e32cf79c 194
kylepost3 1:d721e32cf79c 195 int sum = 0;
kylepost3 1:d721e32cf79c 196
kylepost3 1:d721e32cf79c 197
kylepost3 1:d721e32cf79c 198 for(i=0;i<54;i++) byte = getc(streamIn); // strip out BMP header THIS IS THE KEY
kylepost3 1:d721e32cf79c 199
kylepost3 1:d721e32cf79c 200 for(int y=0;y<height;y++){
kylepost3 1:d721e32cf79c 201 for(int x=0;x<width;x++){
kylepost3 1:d721e32cf79c 202
kylepost3 1:d721e32cf79c 203 blue = getc(streamIn); // use BMP 24bit with no alpha channel
kylepost3 1:d721e32cf79c 204 green = getc(streamIn); // BMP uses BGR but we want RGB, grab byte-by-byte
kylepost3 1:d721e32cf79c 205 red = getc(streamIn); // reverse-order array indexing fixes RGB issue...
kylepost3 1:d721e32cf79c 206
kylepost3 1:d721e32cf79c 207
kylepost3 1:d721e32cf79c 208 if ((red > 150) && (green < 150) && (blue < 150)) // IS the pixel red??
kylepost3 1:d721e32cf79c 209 {
kylepost3 1:d721e32cf79c 210 //printf("red ");
kylepost3 1:d721e32cf79c 211 if (x < 21) left++;
kylepost3 1:d721e32cf79c 212 else if ((x > 21) && (x < 42)) middle++;
kylepost3 1:d721e32cf79c 213 else right++;
kylepost3 1:d721e32cf79c 214 }
kylepost3 1:d721e32cf79c 215 count++;
kylepost3 1:d721e32cf79c 216 }
kylepost3 1:d721e32cf79c 217 }
kylepost3 1:d721e32cf79c 218
kylepost3 1:d721e32cf79c 219 fclose(streamIn);
kylepost3 1:d721e32cf79c 220
kylepost3 1:d721e32cf79c 221 sum = left + right + middle;
kylepost3 1:d721e32cf79c 222
kylepost3 1:d721e32cf79c 223 printf("Total: %i\r\n", sum);
kylepost3 1:d721e32cf79c 224
kylepost3 1:d721e32cf79c 225 return sum;
kylepost3 1:d721e32cf79c 226
kylepost3 1:d721e32cf79c 227 }
kylepost3 1:d721e32cf79c 228
kylepost3 1:d721e32cf79c 229 void camera_init(){
kylepost3 1:d721e32cf79c 230
kylepost3 1:d721e32cf79c 231 printf("\r\n\n\n");
kylepost3 1:d721e32cf79c 232 cam1.setImageSize(ImageSize160x120);
kylepost3 1:d721e32cf79c 233 DEBMSG("Camera module");
kylepost3 1:d721e32cf79c 234 NEWLINE();
kylepost3 1:d721e32cf79c 235 DEBMSG("Resetting...");
kylepost3 1:d721e32cf79c 236 NEWLINE();
kylepost3 1:d721e32cf79c 237 wait(1);
kylepost3 1:d721e32cf79c 238
kylepost3 1:d721e32cf79c 239 if (cam1.reset() == 0) {
kylepost3 1:d721e32cf79c 240 DEBMSG("Reset OK.");
kylepost3 1:d721e32cf79c 241 NEWLINE();
kylepost3 1:d721e32cf79c 242 } else {
kylepost3 1:d721e32cf79c 243 DEBMSG("Reset fail.");
kylepost3 1:d721e32cf79c 244 NEWLINE();
kylepost3 1:d721e32cf79c 245 error("Reset fail.");
kylepost3 1:d721e32cf79c 246 }
kylepost3 1:d721e32cf79c 247 //wait(1);
kylepost3 1:d721e32cf79c 248 }
kylepost3 1:d721e32cf79c 249
kylepost3 1:d721e32cf79c 250 void take_picture(){
kylepost3 1:d721e32cf79c 251 char fname[64];
kylepost3 1:d721e32cf79c 252 int cnt = 0;
kylepost3 1:d721e32cf79c 253 snprintf(fname, sizeof(fname) - 1, FILENAME, cnt);
kylepost3 1:d721e32cf79c 254 int r = capture(&cam1, fname);
kylepost3 1:d721e32cf79c 255 if (r == 0) {
kylepost3 1:d721e32cf79c 256 ;
kylepost3 1:d721e32cf79c 257 //DEBMSG("[%04d]:OK.", cnt);
kylepost3 1:d721e32cf79c 258 //NEWLINE();
kylepost3 1:d721e32cf79c 259 } else {
kylepost3 1:d721e32cf79c 260 DEBMSG("[%04d]:NG. (code=%d)", cnt, r);
kylepost3 1:d721e32cf79c 261 NEWLINE();
kylepost3 1:d721e32cf79c 262 error("Failure.");
kylepost3 1:d721e32cf79c 263 }
kylepost3 1:d721e32cf79c 264 }
kylepost3 1:d721e32cf79c 265
EricWieser 0:ded454e83f81 266
EricWieser 0:ded454e83f81 267 int main()
EricWieser 0:ded454e83f81 268 {
kylepost3 1:d721e32cf79c 269 int sum = 0;
kylepost3 1:d721e32cf79c 270 camera_init();
kylepost3 1:d721e32cf79c 271
kylepost3 1:d721e32cf79c 272 int fired = 0;
kylepost3 1:d721e32cf79c 273
kylepost3 1:d721e32cf79c 274 Latch();
kylepost3 1:d721e32cf79c 275
kylepost3 1:d721e32cf79c 276
kylepost3 1:d721e32cf79c 277 while(1){
kylepost3 1:d721e32cf79c 278 fired = 0;
kylepost3 1:d721e32cf79c 279 sum = 0;
kylepost3 1:d721e32cf79c 280 for(float i = 0.1; i< .9; i+= .10){ //Scans left to right
kylepost3 1:d721e32cf79c 281 Panning = i;
kylepost3 1:d721e32cf79c 282 wait(.01);
kylepost3 1:d721e32cf79c 283
kylepost3 1:d721e32cf79c 284 take_picture();
kylepost3 1:d721e32cf79c 285 convert_to_bmp();
kylepost3 1:d721e32cf79c 286 sum = read_picture(); // Returns total number of red pixels
kylepost3 1:d721e32cf79c 287 printf("sum that matters: %i\r\n", sum);
kylepost3 1:d721e32cf79c 288 if (sum > 400){ // If the number of red pixels is above the threshold: Fire and reload
kylepost3 1:d721e32cf79c 289 i-=.1;
kylepost3 1:d721e32cf79c 290 Panning = i;
kylepost3 1:d721e32cf79c 291 Fire();
kylepost3 1:d721e32cf79c 292 dcreload();
kylepost3 1:d721e32cf79c 293 Latch();
kylepost3 1:d721e32cf79c 294 dcrelease();
kylepost3 1:d721e32cf79c 295 fired = 1;
kylepost3 1:d721e32cf79c 296 break;
kylepost3 1:d721e32cf79c 297 }
kylepost3 1:d721e32cf79c 298 }
EricWieser 0:ded454e83f81 299
kylepost3 1:d721e32cf79c 300
kylepost3 1:d721e32cf79c 301
kylepost3 1:d721e32cf79c 302 for(float i = 0.9; i> .1; i-= .10){ //Scans right to left
kylepost3 1:d721e32cf79c 303 if (fired == 1)
kylepost3 1:d721e32cf79c 304 break;
kylepost3 1:d721e32cf79c 305
kylepost3 1:d721e32cf79c 306 Panning = i;
kylepost3 1:d721e32cf79c 307 wait(.01);
kylepost3 1:d721e32cf79c 308
kylepost3 1:d721e32cf79c 309 take_picture();
kylepost3 1:d721e32cf79c 310 convert_to_bmp();
kylepost3 1:d721e32cf79c 311 sum = read_picture(); // Returns total number of red pixels
kylepost3 1:d721e32cf79c 312
kylepost3 1:d721e32cf79c 313 if (sum > 400){ // If the number of red pixels is above the threshold: Fire and reload
kylepost3 1:d721e32cf79c 314 i+=.1;
kylepost3 1:d721e32cf79c 315 Panning = i;
kylepost3 1:d721e32cf79c 316 Fire();
kylepost3 1:d721e32cf79c 317 dcreload();
kylepost3 1:d721e32cf79c 318 Latch();
kylepost3 1:d721e32cf79c 319 dcrelease();
kylepost3 1:d721e32cf79c 320 break;
kylepost3 1:d721e32cf79c 321 }
kylepost3 1:d721e32cf79c 322 }
kylepost3 1:d721e32cf79c 323
EricWieser 0:ded454e83f81 324 }
kylepost3 1:d721e32cf79c 325
EricWieser 0:ded454e83f81 326 }