Zhen Liu / Mbed 2 deprecated 4180_Final_Project

ECE 4180 Project

Dependencies:   Camera_LS_Y201 SDFileSystem mbed

Fork of 4180_Final_Project by Paul Wilson

main.cpp@1:dcd6c9be9e4b, 2015-04-30 (annotated)

Committer:
lzzcd001
Date:
Thu Apr 30 19:01:22 2015 +0000
Revision:
1:dcd6c9be9e4b
Parent:
0:21c11d775cc4 
ECE 4180 Project

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pwilson39 0:21c11d775cc4 1 #include "mbed.h"
lzzcd001 1:dcd6c9be9e4b 2 #include "Camera_LS_Y201.h"
lzzcd001 1:dcd6c9be9e4b 3 #include "SDFileSystem.h"
lzzcd001 1:dcd6c9be9e4b 4 #include "HMC6352.h"
lzzcd001 1:dcd6c9be9e4b 5
lzzcd001 1:dcd6c9be9e4b 6 #define DEBMSG printf
lzzcd001 1:dcd6c9be9e4b 7 #define NEWLINE() printf("\r\n")
lzzcd001 1:dcd6c9be9e4b 8
lzzcd001 1:dcd6c9be9e4b 9 #define USE_SDCARD 1
pwilson39 0:21c11d775cc4 10
lzzcd001 1:dcd6c9be9e4b 11 #if USE_SDCARD
lzzcd001 1:dcd6c9be9e4b 12 #define FILENAME "/sd/IMG_%04d.jpg"
lzzcd001 1:dcd6c9be9e4b 13 SDFileSystem fs(p5, p6, p7, p8, "sd");
lzzcd001 1:dcd6c9be9e4b 14 #else
lzzcd001 1:dcd6c9be9e4b 15 #define FILENAME "/local/IMG_%04d.jpg"
lzzcd001 1:dcd6c9be9e4b 16 LocalFileSystem fs("local");
lzzcd001 1:dcd6c9be9e4b 17 #endif
lzzcd001 1:dcd6c9be9e4b 18 Camera_LS_Y201 cam1(p13, p14);
lzzcd001 1:dcd6c9be9e4b 19
lzzcd001 1:dcd6c9be9e4b 20 typedef struct work {
lzzcd001 1:dcd6c9be9e4b 21 FILE *fp;
lzzcd001 1:dcd6c9be9e4b 22 } work_t;
lzzcd001 1:dcd6c9be9e4b 23
lzzcd001 1:dcd6c9be9e4b 24 work_t work;
lzzcd001 1:dcd6c9be9e4b 25
lzzcd001 1:dcd6c9be9e4b 26 Serial device(p28, p27); // tx, rx
lzzcd001 1:dcd6c9be9e4b 27 AnalogIn sensor(p18);
pwilson39 0:21c11d775cc4 28 DigitalOut led1(LED1);
pwilson39 0:21c11d775cc4 29 DigitalOut led2(LED2);
pwilson39 0:21c11d775cc4 30 DigitalOut led3(LED3);
pwilson39 0:21c11d775cc4 31 DigitalOut led4(LED4);
lzzcd001 1:dcd6c9be9e4b 32 Serial pc(USBTX, USBRX);
lzzcd001 1:dcd6c9be9e4b 33 AnalogIn front(p20);
lzzcd001 1:dcd6c9be9e4b 34 AnalogIn back(p19);
pwilson39 0:21c11d775cc4 35
pwilson39 0:21c11d775cc4 36 // Definitions of iRobot Create OpenInterface Command Numbers
pwilson39 0:21c11d775cc4 37 // See the Create OpenInterface manual for a complete list
pwilson39 0:21c11d775cc4 38
pwilson39 0:21c11d775cc4 39
pwilson39 0:21c11d775cc4 40 // Create Command // Arguments
pwilson39 0:21c11d775cc4 41 const char Start = 128;
pwilson39 0:21c11d775cc4 42 const char SafeMode = 131;
pwilson39 0:21c11d775cc4 43 const char FullMode = 132;
pwilson39 0:21c11d775cc4 44 const char Drive = 137; // 4: [Vel. Hi] [Vel Low] [Rad. Hi] [Rad. Low]
pwilson39 0:21c11d775cc4 45 const char DriveDirect = 145; // 4: [Right Hi] [Right Low] [Left Hi] [Left Low]
pwilson39 0:21c11d775cc4 46 const char Demo = 136; // 2: Run Demo x
pwilson39 0:21c11d775cc4 47 const char Sensors = 142; // 1: Sensor Packet ID
pwilson39 0:21c11d775cc4 48 const char CoverandDock = 143; // 1: Return to Charger
pwilson39 0:21c11d775cc4 49 const char SensorStream = 148; // x+1: [# of packets requested] IDs of requested packets to stream
pwilson39 0:21c11d775cc4 50 const char QueryList = 149; // x+1: [# of packets requested] IDs of requested packets to stream
pwilson39 0:21c11d775cc4 51 const char StreamPause = 150; // 1: 0 = stop stream, 1 = start stream
pwilson39 0:21c11d775cc4 52 const char PlaySong = 141;
pwilson39 0:21c11d775cc4 53 const char Song = 140;
pwilson39 0:21c11d775cc4 54 /* iRobot Create Sensor IDs */
pwilson39 0:21c11d775cc4 55 const char BumpsandDrops = 7;
pwilson39 0:21c11d775cc4 56 const char Distance = 19;
pwilson39 0:21c11d775cc4 57 const char Angle = 20;
pwilson39 0:21c11d775cc4 58
pwilson39 0:21c11d775cc4 59 int speed_left = 200;
pwilson39 0:21c11d775cc4 60 int speed_right = 200;
lzzcd001 1:dcd6c9be9e4b 61 int imgNum = 0;
pwilson39 0:21c11d775cc4 62 void start();
pwilson39 0:21c11d775cc4 63 void forward();
pwilson39 0:21c11d775cc4 64 void reverse();
pwilson39 0:21c11d775cc4 65 void left();
pwilson39 0:21c11d775cc4 66 void right();
pwilson39 0:21c11d775cc4 67 void stop();
pwilson39 0:21c11d775cc4 68 void playsong();
pwilson39 0:21c11d775cc4 69 void charger();
pwilson39 0:21c11d775cc4 70 void testDistance();
lzzcd001 1:dcd6c9be9e4b 71 int frontDetect();
lzzcd001 1:dcd6c9be9e4b 72 int backDetect();
lzzcd001 1:dcd6c9be9e4b 73 void detectProcess();
lzzcd001 1:dcd6c9be9e4b 74 void sendCommand();
lzzcd001 1:dcd6c9be9e4b 75 void callback_func();
lzzcd001 1:dcd6c9be9e4b 76 int capture();
lzzcd001 1:dcd6c9be9e4b 77 void picture();
lzzcd001 1:dcd6c9be9e4b 78 void moveAndPhoto(int dir);
lzzcd001 1:dcd6c9be9e4b 79
lzzcd001 1:dcd6c9be9e4b 80
pwilson39 0:21c11d775cc4 81 // Demo to move around using basic commands
pwilson39 0:21c11d775cc4 82 int main() {
pwilson39 0:21c11d775cc4 83 // wait for Create to power up to accept serial commands
pwilson39 0:21c11d775cc4 84 wait(5);
pwilson39 0:21c11d775cc4 85 // set baud rate for Create factory default
pwilson39 0:21c11d775cc4 86 device.baud(57600);
lzzcd001 1:dcd6c9be9e4b 87 pc.baud(9600);
pwilson39 0:21c11d775cc4 88 // Start command mode and select sensor data to send back
pwilson39 0:21c11d775cc4 89 start();
pwilson39 0:21c11d775cc4 90 wait(.5);
lzzcd001 1:dcd6c9be9e4b 91 // Camera initialization
lzzcd001 1:dcd6c9be9e4b 92 DEBMSG("Camera module");
lzzcd001 1:dcd6c9be9e4b 93 NEWLINE();
lzzcd001 1:dcd6c9be9e4b 94 DEBMSG("Resetting...");
lzzcd001 1:dcd6c9be9e4b 95 NEWLINE();
lzzcd001 1:dcd6c9be9e4b 96 wait(1);
lzzcd001 1:dcd6c9be9e4b 97 if (cam1.reset() == 0) {
lzzcd001 1:dcd6c9be9e4b 98 DEBMSG("Reset OK.");
lzzcd001 1:dcd6c9be9e4b 99 NEWLINE();
lzzcd001 1:dcd6c9be9e4b 100 } else {
lzzcd001 1:dcd6c9be9e4b 101 DEBMSG("Reset fail.");
lzzcd001 1:dcd6c9be9e4b 102 NEWLINE();
lzzcd001 1:dcd6c9be9e4b 103 error("Reset fail.");
lzzcd001 1:dcd6c9be9e4b 104 }
lzzcd001 1:dcd6c9be9e4b 105
lzzcd001 1:dcd6c9be9e4b 106 wait(1);
lzzcd001 1:dcd6c9be9e4b 107 // sonar detection
lzzcd001 1:dcd6c9be9e4b 108 float frontFirst;
lzzcd001 1:dcd6c9be9e4b 109 float backFirst;
lzzcd001 1:dcd6c9be9e4b 110 float frontIncoming;
lzzcd001 1:dcd6c9be9e4b 111 float backIncoming;
lzzcd001 1:dcd6c9be9e4b 112 frontFirst = frontDetect();
lzzcd001 1:dcd6c9be9e4b 113 backFirst = backDetect();
lzzcd001 1:dcd6c9be9e4b 114 printf("front first: %f \n", frontFirst);
lzzcd001 1:dcd6c9be9e4b 115 printf("back first: %f \n", backFirst);
lzzcd001 1:dcd6c9be9e4b 116
lzzcd001 1:dcd6c9be9e4b 117 while(1) {
lzzcd001 1:dcd6c9be9e4b 118 wait(0.001);
lzzcd001 1:dcd6c9be9e4b 119 frontIncoming = frontDetect();
lzzcd001 1:dcd6c9be9e4b 120 if (abs(frontIncoming - frontFirst) > 20) {
lzzcd001 1:dcd6c9be9e4b 121 if (frontIncoming < 200 && frontFirst < 200) {
lzzcd001 1:dcd6c9be9e4b 122 printf("1st: %f \n", frontFirst);
lzzcd001 1:dcd6c9be9e4b 123 printf("2nd: %f \n", frontIncoming);
lzzcd001 1:dcd6c9be9e4b 124 led3 = 0;
lzzcd001 1:dcd6c9be9e4b 125 led1 = 1;
lzzcd001 1:dcd6c9be9e4b 126 printf("first: %f \n", frontFirst);
lzzcd001 1:dcd6c9be9e4b 127 printf("incoming: %f \n", frontIncoming);
lzzcd001 1:dcd6c9be9e4b 128 moveAndPhoto(1);
lzzcd001 1:dcd6c9be9e4b 129 frontFirst = frontDetect();
lzzcd001 1:dcd6c9be9e4b 130 led1 = 0;
lzzcd001 1:dcd6c9be9e4b 131 }
lzzcd001 1:dcd6c9be9e4b 132 else {
lzzcd001 1:dcd6c9be9e4b 133 led3 = 1;
lzzcd001 1:dcd6c9be9e4b 134 frontFirst = frontIncoming;
lzzcd001 1:dcd6c9be9e4b 135 }
lzzcd001 1:dcd6c9be9e4b 136 } else {
lzzcd001 1:dcd6c9be9e4b 137 frontFirst = frontIncoming;
lzzcd001 1:dcd6c9be9e4b 138 }
lzzcd001 1:dcd6c9be9e4b 139 backIncoming = backDetect();
lzzcd001 1:dcd6c9be9e4b 140 if (abs(backIncoming - backFirst) > 20) {
lzzcd001 1:dcd6c9be9e4b 141 if (backIncoming < 200 && backFirst < 200) {
lzzcd001 1:dcd6c9be9e4b 142 led3 = 0;
lzzcd001 1:dcd6c9be9e4b 143 led1 = 1;
lzzcd001 1:dcd6c9be9e4b 144 printf("first: %f \n", backFirst);
lzzcd001 1:dcd6c9be9e4b 145 printf("incoming: %f \n", backIncoming);
lzzcd001 1:dcd6c9be9e4b 146 moveAndPhoto(0);
lzzcd001 1:dcd6c9be9e4b 147 backFirst = backDetect();
lzzcd001 1:dcd6c9be9e4b 148 backFirst = backDetect();
lzzcd001 1:dcd6c9be9e4b 149 led1 = 0;
lzzcd001 1:dcd6c9be9e4b 150 }
lzzcd001 1:dcd6c9be9e4b 151 else {
lzzcd001 1:dcd6c9be9e4b 152 led3 = 1;
lzzcd001 1:dcd6c9be9e4b 153 backFirst = backIncoming;
lzzcd001 1:dcd6c9be9e4b 154 }
lzzcd001 1:dcd6c9be9e4b 155 } else {
lzzcd001 1:dcd6c9be9e4b 156 backFirst = backIncoming;
lzzcd001 1:dcd6c9be9e4b 157 }
lzzcd001 1:dcd6c9be9e4b 158 }
lzzcd001 1:dcd6c9be9e4b 159
pwilson39 0:21c11d775cc4 160
lzzcd001 1:dcd6c9be9e4b 161 /*
pwilson39 0:21c11d775cc4 162 forward();
pwilson39 0:21c11d775cc4 163 wait(.5);
pwilson39 0:21c11d775cc4 164 stop();
lzzcd001 1:dcd6c9be9e4b 165 bool flag = true;
lzzcd001 1:dcd6c9be9e4b 166 */
lzzcd001 1:dcd6c9be9e4b 167 // testDistance();
lzzcd001 1:dcd6c9be9e4b 168 // wait(10);
lzzcd001 1:dcd6c9be9e4b 169 // playsong();
pwilson39 0:21c11d775cc4 170 // Move around with motor commands
pwilson39 0:21c11d775cc4 171 //forward();
pwilson39 0:21c11d775cc4 172 // wait(.5);
pwilson39 0:21c11d775cc4 173 // stop();
pwilson39 0:21c11d775cc4 174 // wait(.1);
pwilson39 0:21c11d775cc4 175 // reverse();
pwilson39 0:21c11d775cc4 176 // wait(.5);
pwilson39 0:21c11d775cc4 177 // left();
pwilson39 0:21c11d775cc4 178 // wait(1);
pwilson39 0:21c11d775cc4 179 // stop();
pwilson39 0:21c11d775cc4 180 // wait(.1);
pwilson39 0:21c11d775cc4 181 // right();
pwilson39 0:21c11d775cc4 182 // wait(1);
pwilson39 0:21c11d775cc4 183 // stop();
pwilson39 0:21c11d775cc4 184 // wait(.5);
pwilson39 0:21c11d775cc4 185 // Play a song
pwilson39 0:21c11d775cc4 186 // playsong();
pwilson39 0:21c11d775cc4 187 // wait(10);
pwilson39 0:21c11d775cc4 188 // Search for battery charger IR beacon
lzzcd001 1:dcd6c9be9e4b 189 // charger();
pwilson39 0:21c11d775cc4 190 }
pwilson39 0:21c11d775cc4 191
pwilson39 0:21c11d775cc4 192
pwilson39 0:21c11d775cc4 193 // Start - send start and safe mode, start streaming sensor data
pwilson39 0:21c11d775cc4 194 void start() {
pwilson39 0:21c11d775cc4 195 // device.printf("%c%c", Start, SafeMode);
pwilson39 0:21c11d775cc4 196 device.putc(Start);
pwilson39 0:21c11d775cc4 197 device.putc(SafeMode);
pwilson39 0:21c11d775cc4 198 wait(.5);
pwilson39 0:21c11d775cc4 199 // device.printf("%c%c%c", SensorStream, char(1), BumpsandDrops);
pwilson39 0:21c11d775cc4 200 device.putc(SensorStream);
pwilson39 0:21c11d775cc4 201 device.putc(1);
pwilson39 0:21c11d775cc4 202 device.putc(BumpsandDrops);
pwilson39 0:21c11d775cc4 203 wait(.5);
pwilson39 0:21c11d775cc4 204 }
pwilson39 0:21c11d775cc4 205 // Stop - turn off drive motors
pwilson39 0:21c11d775cc4 206 void stop() {
pwilson39 0:21c11d775cc4 207 device.printf("%c%c%c%c%c", DriveDirect, char(0), char(0), char(0), char(0));
pwilson39 0:21c11d775cc4 208 }
pwilson39 0:21c11d775cc4 209 // Forward - turn on drive motors
pwilson39 0:21c11d775cc4 210 void forward() {
pwilson39 0:21c11d775cc4 211 device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF), char(speed_right&0xFF),
pwilson39 0:21c11d775cc4 212 char((speed_left>>8)&0xFF), char(speed_left&0xFF));
pwilson39 0:21c11d775cc4 213
pwilson39 0:21c11d775cc4 214 }
pwilson39 0:21c11d775cc4 215 // Reverse - reverse drive motors
pwilson39 0:21c11d775cc4 216 void reverse() {
pwilson39 0:21c11d775cc4 217 device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF), char((-speed_right)&0xFF),
pwilson39 0:21c11d775cc4 218 char(((-speed_left)>>8)&0xFF), char((-speed_left)&0xFF));
pwilson39 0:21c11d775cc4 219
pwilson39 0:21c11d775cc4 220 }
pwilson39 0:21c11d775cc4 221 // Left - drive motors set to rotate to left
pwilson39 0:21c11d775cc4 222 void left() {
pwilson39 0:21c11d775cc4 223 device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF), char(speed_right&0xFF),
pwilson39 0:21c11d775cc4 224 char(((-speed_left)>>8)&0xFF), char((-speed_left)&0xFF));
pwilson39 0:21c11d775cc4 225 }
pwilson39 0:21c11d775cc4 226 // Right - drive motors set to rotate to right
pwilson39 0:21c11d775cc4 227 void right() {
pwilson39 0:21c11d775cc4 228 device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF), char((-speed_right)&0xFF),
pwilson39 0:21c11d775cc4 229 char((speed_left>>8)&0xFF), char(speed_left&0xFF));
pwilson39 0:21c11d775cc4 230
pwilson39 0:21c11d775cc4 231 }
pwilson39 0:21c11d775cc4 232 // Charger - search and return to charger using IR beacons (if found)
pwilson39 0:21c11d775cc4 233 void charger() {
pwilson39 0:21c11d775cc4 234 device.printf("%c%c", Demo, char(1));
pwilson39 0:21c11d775cc4 235 }
pwilson39 0:21c11d775cc4 236 // Play Song - define and play a song
pwilson39 0:21c11d775cc4 237 void playsong() { // Send out notes & duration to define song and then play song
pwilson39 0:21c11d775cc4 238
pwilson39 0:21c11d775cc4 239 device.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
pwilson39 0:21c11d775cc4 240 Song, char(0), char(16), char(91), char(24), char(89), char(12), char(87), char(36), char(87),
pwilson39 0:21c11d775cc4 241 char(24), char(89), char(12), char(91), char(24), char(91), char(12), char(91), char(12), char(89),
pwilson39 0:21c11d775cc4 242 char(12),char(87), char(12), char(89), char(12), char(91), char(12), char(89), char(12), char(87),
pwilson39 0:21c11d775cc4 243 char(24), char(86), char(12), char(87), char(48));
pwilson39 0:21c11d775cc4 244
pwilson39 0:21c11d775cc4 245 wait(.2);
pwilson39 0:21c11d775cc4 246 device.printf("%c%c", PlaySong, char(0));
pwilson39 0:21c11d775cc4 247 }
pwilson39 0:21c11d775cc4 248
pwilson39 0:21c11d775cc4 249 void testDistance() {
pwilson39 0:21c11d775cc4 250 int timer = 0;
pwilson39 0:21c11d775cc4 251 while(timer < 100000000) {
lzzcd001 1:dcd6c9be9e4b 252 if (sensor > 0.75) {
pwilson39 0:21c11d775cc4 253 led1 = 1;
pwilson39 0:21c11d775cc4 254 led2 = 1;
pwilson39 0:21c11d775cc4 255 led3 = 1;
pwilson39 0:21c11d775cc4 256 led4 = 1;
pwilson39 0:21c11d775cc4 257 forward();
pwilson39 0:21c11d775cc4 258 } else if(sensor > 0.50){
pwilson39 0:21c11d775cc4 259 led1 = 1;
pwilson39 0:21c11d775cc4 260 led2 = 1;
pwilson39 0:21c11d775cc4 261 led3 = 1;
pwilson39 0:21c11d775cc4 262 led4 = 0;
pwilson39 0:21c11d775cc4 263 left();
pwilson39 0:21c11d775cc4 264 wait(1);
pwilson39 0:21c11d775cc4 265 forward();
pwilson39 0:21c11d775cc4 266 wait(1);
pwilson39 0:21c11d775cc4 267 stop();
pwilson39 0:21c11d775cc4 268 } else if(sensor > 0.25){
pwilson39 0:21c11d775cc4 269 led1 = 1;
pwilson39 0:21c11d775cc4 270 led2 = 1;
pwilson39 0:21c11d775cc4 271 led3 = 0;
pwilson39 0:21c11d775cc4 272 led4 = 0;
pwilson39 0:21c11d775cc4 273 right();
pwilson39 0:21c11d775cc4 274 wait(1);
pwilson39 0:21c11d775cc4 275 forward();
pwilson39 0:21c11d775cc4 276 wait(1);
pwilson39 0:21c11d775cc4 277 stop();
pwilson39 0:21c11d775cc4 278 } else if(sensor > 0.1){
pwilson39 0:21c11d775cc4 279 led1 = 1;
pwilson39 0:21c11d775cc4 280 led2 = 0;
pwilson39 0:21c11d775cc4 281 led3 = 0;
pwilson39 0:21c11d775cc4 282 led4 = 0;
pwilson39 0:21c11d775cc4 283 // forward();
pwilson39 0:21c11d775cc4 284 stop();
pwilson39 0:21c11d775cc4 285 } else {
pwilson39 0:21c11d775cc4 286 led1 = 0;
pwilson39 0:21c11d775cc4 287 led2 = 0;
pwilson39 0:21c11d775cc4 288 led3 = 0;
pwilson39 0:21c11d775cc4 289 led4 = 0;
pwilson39 0:21c11d775cc4 290 stop();
pwilson39 0:21c11d775cc4 291 }
pwilson39 0:21c11d775cc4 292 wait(1);
pwilson39 0:21c11d775cc4 293 timer = timer + 1;
pwilson39 0:21c11d775cc4 294 timer = timer - 1;
pwilson39 0:21c11d775cc4 295 }
lzzcd001 1:dcd6c9be9e4b 296 }
lzzcd001 1:dcd6c9be9e4b 297
lzzcd001 1:dcd6c9be9e4b 298 int frontDetect() {
lzzcd001 1:dcd6c9be9e4b 299
lzzcd001 1:dcd6c9be9e4b 300 float sum = 0;
lzzcd001 1:dcd6c9be9e4b 301 front.read(); //first reading not reliable
lzzcd001 1:dcd6c9be9e4b 302 for (int i = 0; i < 5; i++) {
lzzcd001 1:dcd6c9be9e4b 303 wait(0.01);
lzzcd001 1:dcd6c9be9e4b 304 float temp = front.read() * 1000.0;
lzzcd001 1:dcd6c9be9e4b 305 sum += temp;
lzzcd001 1:dcd6c9be9e4b 306 }
lzzcd001 1:dcd6c9be9e4b 307 return sum/5;
lzzcd001 1:dcd6c9be9e4b 308 }
lzzcd001 1:dcd6c9be9e4b 309
lzzcd001 1:dcd6c9be9e4b 310 int backDetect() {
lzzcd001 1:dcd6c9be9e4b 311
lzzcd001 1:dcd6c9be9e4b 312 float sum = 0;
lzzcd001 1:dcd6c9be9e4b 313 back.read(); //first reading not reliable
lzzcd001 1:dcd6c9be9e4b 314 for (int i = 0; i < 5; i++) {
lzzcd001 1:dcd6c9be9e4b 315 wait(0.01);
lzzcd001 1:dcd6c9be9e4b 316 float temp = back.read() * 1000.0;
lzzcd001 1:dcd6c9be9e4b 317 sum += temp;
lzzcd001 1:dcd6c9be9e4b 318 }
lzzcd001 1:dcd6c9be9e4b 319 return sum/5;
lzzcd001 1:dcd6c9be9e4b 320 }
lzzcd001 1:dcd6c9be9e4b 321 /*
lzzcd001 1:dcd6c9be9e4b 322 bool detectProcess() {
lzzcd001 1:dcd6c9be9e4b 323 float first;
lzzcd001 1:dcd6c9be9e4b 324 float incoming;
lzzcd001 1:dcd6c9be9e4b 325 float threshold;
lzzcd001 1:dcd6c9be9e4b 326 first = detect();
lzzcd001 1:dcd6c9be9e4b 327 while(1) {
lzzcd001 1:dcd6c9be9e4b 328 wait(0.001);
lzzcd001 1:dcd6c9be9e4b 329 incoming = detect();
lzzcd001 1:dcd6c9be9e4b 330 if (abs(incoming - first) > 15) {
lzzcd001 1:dcd6c9be9e4b 331 led1 = 1;
lzzcd001 1:dcd6c9be9e4b 332 return true;
lzzcd001 1:dcd6c9be9e4b 333 } else {
lzzcd001 1:dcd6c9be9e4b 334 first = incoming;
lzzcd001 1:dcd6c9be9e4b 335 }
lzzcd001 1:dcd6c9be9e4b 336 }
lzzcd001 1:dcd6c9be9e4b 337 }
lzzcd001 1:dcd6c9be9e4b 338 */
lzzcd001 1:dcd6c9be9e4b 339
lzzcd001 1:dcd6c9be9e4b 340 void sendCommand() {
lzzcd001 1:dcd6c9be9e4b 341
lzzcd001 1:dcd6c9be9e4b 342 }
lzzcd001 1:dcd6c9be9e4b 343
lzzcd001 1:dcd6c9be9e4b 344
lzzcd001 1:dcd6c9be9e4b 345 /**
lzzcd001 1:dcd6c9be9e4b 346 * Callback function for readJpegFileContent.
lzzcd001 1:dcd6c9be9e4b 347 *
lzzcd001 1:dcd6c9be9e4b 348 * @param buf A pointer to a buffer.
lzzcd001 1:dcd6c9be9e4b 349 * @param siz A size of the buffer.
lzzcd001 1:dcd6c9be9e4b 350 */
lzzcd001 1:dcd6c9be9e4b 351 void callback_func(int done, int total, uint8_t *buf, size_t siz) {
lzzcd001 1:dcd6c9be9e4b 352 fwrite(buf, siz, 1, work.fp);
lzzcd001 1:dcd6c9be9e4b 353
lzzcd001 1:dcd6c9be9e4b 354 static int n = 0;
lzzcd001 1:dcd6c9be9e4b 355 int tmp = done * 100 / total;
lzzcd001 1:dcd6c9be9e4b 356 if (n != tmp) {
lzzcd001 1:dcd6c9be9e4b 357 n = tmp;
lzzcd001 1:dcd6c9be9e4b 358 DEBMSG("Writing...: %3d%%", n);
lzzcd001 1:dcd6c9be9e4b 359 NEWLINE();
lzzcd001 1:dcd6c9be9e4b 360 }
lzzcd001 1:dcd6c9be9e4b 361 }
lzzcd001 1:dcd6c9be9e4b 362
lzzcd001 1:dcd6c9be9e4b 363 /**
lzzcd001 1:dcd6c9be9e4b 364 * Capture.
lzzcd001 1:dcd6c9be9e4b 365 *
lzzcd001 1:dcd6c9be9e4b 366 * @param cam A pointer to a camera object.
lzzcd001 1:dcd6c9be9e4b 367 * @param filename The file name.
lzzcd001 1:dcd6c9be9e4b 368 *
lzzcd001 1:dcd6c9be9e4b 369 * @return Return 0 if it succeed.
lzzcd001 1:dcd6c9be9e4b 370 */
lzzcd001 1:dcd6c9be9e4b 371 int capture(Camera_LS_Y201 *cam, char *filename) {
lzzcd001 1:dcd6c9be9e4b 372 /*
lzzcd001 1:dcd6c9be9e4b 373 * Take a picture.
lzzcd001 1:dcd6c9be9e4b 374 */
lzzcd001 1:dcd6c9be9e4b 375 if (cam->takePicture() != 0) {
lzzcd001 1:dcd6c9be9e4b 376 return -1;
lzzcd001 1:dcd6c9be9e4b 377 }
lzzcd001 1:dcd6c9be9e4b 378 DEBMSG("Captured.");
lzzcd001 1:dcd6c9be9e4b 379 NEWLINE();
lzzcd001 1:dcd6c9be9e4b 380
lzzcd001 1:dcd6c9be9e4b 381 /*
lzzcd001 1:dcd6c9be9e4b 382 * Open file.
lzzcd001 1:dcd6c9be9e4b 383 */
lzzcd001 1:dcd6c9be9e4b 384 work.fp = fopen(filename, "wb");
lzzcd001 1:dcd6c9be9e4b 385 if (work.fp == NULL) {
lzzcd001 1:dcd6c9be9e4b 386 return -2;
lzzcd001 1:dcd6c9be9e4b 387 }
lzzcd001 1:dcd6c9be9e4b 388
lzzcd001 1:dcd6c9be9e4b 389 /*
lzzcd001 1:dcd6c9be9e4b 390 * Read the content.
lzzcd001 1:dcd6c9be9e4b 391 */
lzzcd001 1:dcd6c9be9e4b 392 DEBMSG("%s", filename);
lzzcd001 1:dcd6c9be9e4b 393 NEWLINE();
lzzcd001 1:dcd6c9be9e4b 394 if (cam->readJpegFileContent(callback_func) != 0) {
lzzcd001 1:dcd6c9be9e4b 395 fclose(work.fp);
lzzcd001 1:dcd6c9be9e4b 396 return -3;
lzzcd001 1:dcd6c9be9e4b 397 }
lzzcd001 1:dcd6c9be9e4b 398 fclose(work.fp);
lzzcd001 1:dcd6c9be9e4b 399
lzzcd001 1:dcd6c9be9e4b 400 /*
lzzcd001 1:dcd6c9be9e4b 401 * Stop taking pictures.
lzzcd001 1:dcd6c9be9e4b 402 */
lzzcd001 1:dcd6c9be9e4b 403 cam->stopTakingPictures();
lzzcd001 1:dcd6c9be9e4b 404
lzzcd001 1:dcd6c9be9e4b 405 return 0;
lzzcd001 1:dcd6c9be9e4b 406 }
lzzcd001 1:dcd6c9be9e4b 407
lzzcd001 1:dcd6c9be9e4b 408 /**
lzzcd001 1:dcd6c9be9e4b 409 * Entry point.
lzzcd001 1:dcd6c9be9e4b 410 */
lzzcd001 1:dcd6c9be9e4b 411
lzzcd001 1:dcd6c9be9e4b 412
lzzcd001 1:dcd6c9be9e4b 413
lzzcd001 1:dcd6c9be9e4b 414
lzzcd001 1:dcd6c9be9e4b 415
lzzcd001 1:dcd6c9be9e4b 416 void picture() {
lzzcd001 1:dcd6c9be9e4b 417 char fname[64];
lzzcd001 1:dcd6c9be9e4b 418 snprintf(fname, sizeof(fname) - 1, FILENAME, imgNum);
lzzcd001 1:dcd6c9be9e4b 419 int r = capture(&cam1, fname);
lzzcd001 1:dcd6c9be9e4b 420 if (r == 0) {
lzzcd001 1:dcd6c9be9e4b 421 DEBMSG("[%04d]:OK.", imgNum);
lzzcd001 1:dcd6c9be9e4b 422 NEWLINE();
lzzcd001 1:dcd6c9be9e4b 423 } else {
lzzcd001 1:dcd6c9be9e4b 424 DEBMSG("[%04d]:NG. (code=%d)", imgNum, r);
lzzcd001 1:dcd6c9be9e4b 425 NEWLINE();
lzzcd001 1:dcd6c9be9e4b 426 error("Failure.");
lzzcd001 1:dcd6c9be9e4b 427 }
lzzcd001 1:dcd6c9be9e4b 428 wait(3);
lzzcd001 1:dcd6c9be9e4b 429 imgNum++;
lzzcd001 1:dcd6c9be9e4b 430 }
lzzcd001 1:dcd6c9be9e4b 431
lzzcd001 1:dcd6c9be9e4b 432 void moveAndPhoto(int dir) {
lzzcd001 1:dcd6c9be9e4b 433 // dir: 1 stands for counter-clockwise, 2 for clockwise
lzzcd001 1:dcd6c9be9e4b 434 led2 = 1;
lzzcd001 1:dcd6c9be9e4b 435 if (dir == 1) {
lzzcd001 1:dcd6c9be9e4b 436 left(); // rotate counter-clockwise
lzzcd001 1:dcd6c9be9e4b 437 wait(1.3);
lzzcd001 1:dcd6c9be9e4b 438 stop(); // stop
lzzcd001 1:dcd6c9be9e4b 439 } else if (dir == 0) { // moving clockwise
lzzcd001 1:dcd6c9be9e4b 440 right();
lzzcd001 1:dcd6c9be9e4b 441 wait(1.3);
lzzcd001 1:dcd6c9be9e4b 442 stop();
lzzcd001 1:dcd6c9be9e4b 443 }
lzzcd001 1:dcd6c9be9e4b 444 led2 = 0;
lzzcd001 1:dcd6c9be9e4b 445 picture(); // take picture
lzzcd001 1:dcd6c9be9e4b 446
lzzcd001 1:dcd6c9be9e4b 447 }