Khayhen Sarveswarah
MBED file for Heriot-Watt Malaysia System Project 2016
Dependencies: PinDetect SDFileSystem ShiftReg TCS3472_I2C TextLCD mRotaryEncoder mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "PinDetect.h" 00003 #include "SDFileSystem.h" 00004 #include "mRotaryEncoder.h" 00005 #include "TextLCD.h" 00006 #include "TCS3472_I2C.h" 00007 #include "ShiftReg.h" 00008 #include <stdlib.h> 00009 #include <string> 00010 #include <vector> 00011 00012 #define PI 3.14159265 00013 00014 /////////////////////////////////////////////////// Pin Decleration /////////////////////////////////////////////////// 00015 // IR Trigger 00016 AnalogIn IR_Trigger(p16); 00017 00018 // Shift Registers (74Shift_RegN) 00019 ShiftReg Shift_Reg(p17, p20, p22); // Data, Store, Clock 00020 00021 // SD_Card module 00022 SDFileSystem sd(p11, p12, p13, p14, "sd"); // MOSI, MISO, SCK, CS 00023 00024 // Host PC Communication channels 00025 Serial pc(USBTX, USBRX); // tx, rx 00026 00027 // Arm Signal 00028 PinDetect ArmOut(p27, PullDown); 00029 DigitalOut ArmIn(p23); 00030 00031 // LCD Display 00032 I2C i2c_lcd(p9,p10); // SDA, SCL 00033 TextLCD_I2C lcd(&i2c_lcd, 0x4E, TextLCD::LCD20x4); // I2C bus, PCF8574 Slaveaddress, LCD Type 00034 00035 // RGB LED 00036 PwmOut RGB_Red(p24); 00037 PwmOut RGB_Green(p25); 00038 //PwmOut RGB_Blue(p26); 00039 00040 // Rotary Encoder 00041 mRotaryEncoder RE(p5,p6,p7,PullUp,1000); // Pin A, Pin B, SW 00042 00043 // Speaker 00044 //AnalogOut Speaker(p18); 00045 00046 // Slide LEDs 00047 DigitalOut SL1(p19); 00048 DigitalOut SL2(p28); 00049 00050 // Colour Sensor (TCS3472) 00051 TCS3472_I2C Colour_sensor(p9, p10); // SDA, SCL 00052 00053 // VL6180X SATEL 00054 00055 // IR Sensor 00056 AnalogIn IR_Sensor(p18); 00057 00058 00059 //////////////////////////////////////////////// Custom LCD Characters //////////////////////////////////////////////// 00060 char pointer[] = {0x10,0x18,0x1c,0x1e,0x1e,0x1c,0x18,0x10}; // |> 00061 00062 00063 ////////////////////////////////////////////////////// Functions ////////////////////////////////////////////////////// 00064 void Operation_mode(); 00065 void Maintenance_mode(); 00066 void BeginSorting(); 00067 void Object_Hazardous(); 00068 void Object_NonHazardous(); 00069 00070 void sw(); 00071 void ButtonPressed(); 00072 void ButtonHeld(); 00073 bool read_file_names(char *dir); 00074 int LCD_Select(vector<string> list); 00075 float AreaScan(int resolution); 00076 float Distance_sensor(); 00077 float avgDistance_sensor(); 00078 float Area(float a, float b, int steps); 00079 void Phases1(int phase); 00080 void Phases2(int phase); 00081 void Motor1_Stepping(int steps, bool dir, int count); 00082 void Motor2_Stepping(int steps, bool dir, int count); 00083 void intStringSplit(char in[100], int* int_Strings, float* float_Strings); 00084 bool int_numcmp(int numof, int in_number, int cmp_number[], int tolerance[]); 00085 bool float_numcmp(int numof, float in_number, float cmp_number[], float tolerance[]); 00086 void SweeperArmCC(); 00087 void SweeperArmACC(); 00088 00089 ////////////////////////////////////////////////////// Variables ////////////////////////////////////////////////////// 00090 string PC_SDlist[] = {"Connect to PC", 00091 "Access to SD"}; 00092 string SD_Optionslist[] = {"Load", 00093 "Rename", 00094 "Remove", 00095 "Back"}; 00096 vector<string> PC_SD(PC_SDlist, PC_SDlist + 2); //Selection between PC and SD 00097 vector<string> Filenames; //Filenamess are stored in a vector string 00098 vector<string> SD_Options(SD_Optionslist, SD_Optionslist + 4); //Options to pick when SD file selected 00099 vector<string> SD_Content; //Store settings from SD card 00100 vector<string> H_Settings; //Stores Hazardous settings 00101 vector<string> Sorted_Data; //Stores data of sorted items 00102 00103 int Select = 0; 00104 bool Sort = false; 00105 bool Stop = false; 00106 int PC_SDmode = 0; 00107 int Filenumber = 0; 00108 int SD_Optionnumber = 0; 00109 char Command[512]; 00110 int Steps1 = 1; 00111 int Steps2 = 1; 00112 int rgb_readings[4]; 00113 float Area_readings; 00114 00115 ///////////////////////////////////////////////////// Main Program ///////////////////////////////////////////////////// 00116 int main() 00117 { 00118 00119 00120 RGB_Red = 1.0; 00121 //RGB_Blue = 1.0; 00122 RGB_Green = 1.0; 00123 lcd.setCursor(TextLCD::CurOff_BlkOff); 00124 lcd.cls(); 00125 lcd.locate(4,1); 00126 lcd.printf("MIRACLE"); 00127 lcd.locate(9,2); 00128 lcd.printf("TECH"); 00129 lcd.setUDC(0,pointer); 00130 00131 SweeperArmCC(); 00132 Colour_sensor.enablePowerAndRGBC(); 00133 Colour_sensor.setIntegrationTime( 100 ); 00134 00135 RE.Set(0); 00136 RE.attachSW(&sw); 00137 ArmOut.attach_asserted(&ButtonPressed); 00138 ArmOut.attach_asserted_held(&ButtonHeld); 00139 ArmOut.setSampleFrequency(); 00140 wait(1); 00141 lcd.cls(); 00142 00143 ////////////////////////////////SYSTEM BEGIN//////////////////////////////// 00144 //Selecting between PC and SD_Card mode 00145 while(true) 00146 { 00147 lcd.cls(); 00148 PC_SDmode = LCD_Select(PC_SD); 00149 switch(PC_SDmode) 00150 { 00151 //////////////////////////////////////////PC mode////////////////////////////////////////// 00152 case 0: 00153 { 00154 lcd.locate(5,1); 00155 lcd.printf("WAITING TO"); 00156 lcd.locate(4,2); 00157 lcd.printf("CONNECT "); 00158 while(true) 00159 { 00160 lcd.locate(12,2); 00161 lcd.printf(". "); 00162 wait(0.2); 00163 lcd.locate(12,2); 00164 lcd.printf(".. "); 00165 wait(0.2); 00166 lcd.locate(12,2); 00167 lcd.printf("... "); 00168 wait(0.2); 00169 lcd.locate(12,2); 00170 lcd.printf(".... "); 00171 wait(0.2); 00172 if(pc.readable()) 00173 { 00174 pc.scanf("%s",Command); 00175 lcd.cls(); 00176 lcd.locate(5,1); 00177 lcd.printf("%s",Command); 00178 wait(1); 00179 while(true) 00180 { 00181 if(pc.readable()) 00182 { 00183 lcd.cls(); 00184 lcd.locate(8,1); 00185 lcd.printf("MODE"); 00186 lcd.locate(7,2); 00187 lcd.printf("SELECT"); 00188 pc.scanf("%s", Command); 00189 if(strcmp(Command,"Operation") == 0) Operation_mode(); 00190 else if(strcmp(Command,"Maintenance") == 0) Maintenance_mode(); 00191 else if(strcmp(Command,"Disconnect") == 0) break; 00192 else 00193 { 00194 lcd.cls(); 00195 lcd.locate(7,1); 00196 lcd.printf("ERROR!"); 00197 wait(2); 00198 } 00199 } 00200 } 00201 break; 00202 } 00203 } 00204 00205 00206 } 00207 00208 //////////////////////////////////////////SD Card mode////////////////////////////////////////// 00209 case 1: 00210 { 00211 sd.mount(); 00212 lcd.cls(); 00213 Filenames.clear(); 00214 00215 while(true) 00216 { 00217 // print Filenames strings from vector using an iterator 00218 Filenames.clear(); 00219 read_file_names("/sd"); 00220 Filenumber = LCD_Select(Filenames); 00221 while(true) 00222 { 00223 if(Filenumber == 0) break; 00224 SD_Optionnumber = LCD_Select(SD_Options); 00225 switch(SD_Optionnumber) 00226 { 00227 case 0 : //Load settings 00228 { 00229 lcd.cls(); 00230 lcd.locate(0,0); 00231 lcd.printf("%s", Filenames.at(Filenumber).c_str()); 00232 00233 string source_directory = "/sd/"; 00234 string file_directory = Filenames.at(Filenumber); 00235 string directory = source_directory + file_directory; 00236 SD_Content.clear(); 00237 FILE *fp = fopen(directory.c_str(), "r"); 00238 while(!feof(fp)) 00239 { 00240 char word[128]; 00241 fgets(word,128,fp); 00242 SD_Content.push_back(word); 00243 } 00244 00245 int NumberofSettings = atoi(SD_Content.at(0).c_str()); 00246 00247 if(NumberofSettings == (SD_Content.size() - 3)) 00248 { 00249 lcd.locate(0,1); 00250 lcd.printf("Num of Settings : %d", NumberofSettings); 00251 } 00252 else 00253 { 00254 lcd.locate(7,2); 00255 lcd.printf("ERROR!"); 00256 wait(1); 00257 break; 00258 } 00259 00260 H_Settings.clear(); 00261 for(int i = 1; i < (SD_Content.size()-1); i++) 00262 { 00263 H_Settings.push_back(SD_Content.at(i)); 00264 } 00265 00266 lcd.locate(0,3); 00267 lcd.printf("%d", H_Settings.size()); 00268 BeginSorting(); 00269 00270 } 00271 00272 case 1 : 00273 { 00274 //Rename file 00275 } 00276 00277 case 2 : //Delete File 00278 { 00279 sd.remove(Filenames.at(Filenumber).c_str()); 00280 break; 00281 } 00282 00283 case 3 : break; //Go Back 00284 } 00285 break; 00286 } 00287 if(Filenumber == 0) break; 00288 } 00289 sd.unmount(); 00290 break; 00291 } 00292 00293 default: 00294 { 00295 lcd.locate(7,1); 00296 lcd.printf("ERROR!"); 00297 break; 00298 } 00299 } 00300 } 00301 00302 } 00303 00304 void sw() 00305 { 00306 RE.Set(0); 00307 Select = 1; 00308 } 00309 void ButtonPressed() {Sort = true;} 00310 void ButtonHeld() { Stop = true;} 00311 00312 bool read_file_names(char *dir) 00313 { 00314 bool present; 00315 Filenames.push_back("Back"); 00316 DIR *dp; 00317 struct dirent *dirp; 00318 dp = opendir(dir); 00319 //read all directory and file names in current directory into filename vector 00320 if((dirp = readdir(dp)) != NULL) present = true; 00321 else present = false; 00322 00323 while((dirp = readdir(dp)) != NULL) { 00324 Filenames.push_back(string(dirp->d_name)); 00325 } 00326 closedir(dp); 00327 return present; 00328 } 00329 00330 int LCD_Select(vector<string> list) 00331 { 00332 lcd.cls(); 00333 int RE_val_c = 0; 00334 int RE_val_p = 1; 00335 int Selected; 00336 int size = list.size(); 00337 00338 if(size > 4) 00339 { 00340 for(int i = 0; i < 4; i++) 00341 { 00342 lcd.locate(1,i); 00343 lcd.printf("%s", list.at(i).c_str()); 00344 } 00345 while(true) 00346 { 00347 RE_val_c = RE.Get(); 00348 00349 if(RE_val_c != RE_val_p) 00350 { 00351 if(RE_val_c > size-1) 00352 { 00353 RE.Set(0); 00354 RE_val_c = 0; 00355 lcd.cls(); 00356 for(int i = 0; i < 4; i++) 00357 { 00358 lcd.locate(1,i); 00359 lcd.printf("%s", list.at(i).c_str()); 00360 } 00361 } 00362 else if(RE_val_c < 0) 00363 { 00364 RE.Set(size-1); 00365 RE_val_c = size-1; 00366 lcd.cls(); 00367 lcd.locate(1,0); 00368 lcd.printf("%s", list.at(RE_val_c - 3).c_str()); 00369 lcd.locate(1,1); 00370 lcd.printf("%s", list.at(RE_val_c - 2).c_str()); 00371 lcd.locate(1,2); 00372 lcd.printf("%s", list.at(RE_val_c - 1).c_str()); 00373 lcd.locate(1,3); 00374 lcd.printf("%s", list.at(RE_val_c - 0).c_str()); 00375 } 00376 if(RE_val_c > 2) 00377 { 00378 lcd.cls(); 00379 lcd.locate(1,0); 00380 lcd.printf("%s", list.at(RE_val_c - 3).c_str()); 00381 lcd.locate(1,1); 00382 lcd.printf("%s", list.at(RE_val_c - 2).c_str()); 00383 lcd.locate(1,2); 00384 lcd.printf("%s", list.at(RE_val_c - 1).c_str()); 00385 lcd.locate(1,3); 00386 lcd.printf("%s", list.at(RE_val_c - 0).c_str()); 00387 } 00388 00389 00390 00391 for(int i = 0; i < 4; i++) 00392 { 00393 lcd.locate(0,i); 00394 lcd.printf(" "); 00395 } 00396 lcd.locate(0,RE_val_c); 00397 lcd.putc(0); 00398 RE_val_p = RE_val_c; 00399 } 00400 lcd.locate(19,0); 00401 lcd.printf("%d", RE_val_c); 00402 if(Select) 00403 { 00404 Selected = RE_val_c; 00405 Select = 0; 00406 lcd.cls(); 00407 return Selected; 00408 } 00409 } 00410 } 00411 00412 else 00413 { 00414 for(int i = 0; i < size; i++) 00415 { 00416 lcd.locate(1,i); 00417 lcd.printf("%s", list.at(i).c_str()); 00418 } 00419 while(true) 00420 { 00421 RE_val_c = RE.Get(); 00422 00423 00424 if(RE_val_c != RE_val_p) 00425 { 00426 //if(RE_val_c > 3) 00427 // { 00428 // lcd.cls(); 00429 // lcd.locate(1,0); 00430 // lcd.printf("%s", list.at(RE_val_c - 3).c_str()); 00431 // lcd.locate(1,1); 00432 // lcd.printf("%s", list.at(RE_val_c - 2).c_str()); 00433 // lcd.locate(1,2); 00434 // lcd.printf("%s", list.at(RE_val_c - 1).c_str()); 00435 // lcd.locate(1,3); 00436 // lcd.printf("%s", list.at(RE_val_c - 0).c_str()); 00437 // } 00438 00439 00440 if(RE_val_c > size-1) {RE.Set(0); RE_val_c = 0;} 00441 else if(RE_val_c < 0) {RE.Set(size-1); RE_val_c = size-1;} 00442 for(int i = 0; i < 4; i++) 00443 { 00444 lcd.locate(0,i); 00445 lcd.printf(" "); 00446 } 00447 lcd.locate(0,RE_val_c); 00448 lcd.putc(0); 00449 RE_val_p = RE_val_c; 00450 } 00451 lcd.locate(19,0); 00452 lcd.printf("%d", RE_val_c); 00453 if(Select) 00454 { 00455 Selected = RE_val_c; 00456 Select = 0; 00457 lcd.cls(); 00458 return Selected; 00459 } 00460 } 00461 } 00462 00463 00464 } 00465 00466 void Phases1(int phase) 00467 { 00468 switch(phase) 00469 { 00470 case 1 : 00471 Shift_Reg.ShiftByte(0x80, ShiftReg::MSBFirst); Shift_Reg.Latch(); 00472 break; 00473 00474 case 2 : 00475 Shift_Reg.ShiftByte(0xC0, ShiftReg::MSBFirst); Shift_Reg.Latch(); 00476 break; 00477 00478 case 3 : 00479 Shift_Reg.ShiftByte(0x40, ShiftReg::MSBFirst); Shift_Reg.Latch(); 00480 break; 00481 00482 case 4 : 00483 Shift_Reg.ShiftByte(0x60, ShiftReg::MSBFirst); Shift_Reg.Latch(); 00484 break; 00485 00486 case 5 : 00487 Shift_Reg.ShiftByte(0x20, ShiftReg::MSBFirst); Shift_Reg.Latch(); 00488 break; 00489 00490 case 6 : 00491 Shift_Reg.ShiftByte(0x30, ShiftReg::MSBFirst); Shift_Reg.Latch(); 00492 break; 00493 00494 case 7 : 00495 Shift_Reg.ShiftByte(0x10, ShiftReg::MSBFirst); Shift_Reg.Latch(); 00496 break; 00497 00498 case 8 : 00499 Shift_Reg.ShiftByte(0x90, ShiftReg::MSBFirst); Shift_Reg.Latch(); 00500 break; 00501 00502 default : 00503 Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst); Shift_Reg.Latch(); 00504 break; 00505 } 00506 } 00507 00508 void Motor1_Stepping(int steps, bool dir, int count) 00509 { 00510 for(int i = 0; i < steps; i++) 00511 { 00512 Phases1(count); 00513 if(dir) 00514 { 00515 if(count < 8) count++; 00516 else count = 1; 00517 wait(0.001); 00518 } 00519 00520 else 00521 { 00522 if(count > 1) count--; 00523 else count = 8; 00524 wait(0.001); 00525 } 00526 } 00527 } 00528 00529 void Phases2(int phase) 00530 { 00531 switch(phase) 00532 { 00533 case 1 : 00534 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst); 00535 Shift_Reg.ShiftByte(0x08, ShiftReg::MSBFirst); 00536 Shift_Reg.Latch(); 00537 break; 00538 00539 case 2 : 00540 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst); 00541 Shift_Reg.ShiftByte(0x0C, ShiftReg::MSBFirst); 00542 Shift_Reg.Latch(); 00543 break; 00544 00545 case 3 : 00546 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst); 00547 Shift_Reg.ShiftByte(0x04, ShiftReg::MSBFirst); 00548 Shift_Reg.Latch(); 00549 break; 00550 00551 case 4 : 00552 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst); 00553 Shift_Reg.ShiftByte(0x06, ShiftReg::MSBFirst); 00554 Shift_Reg.Latch(); 00555 break; 00556 00557 case 5 : 00558 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst); 00559 Shift_Reg.ShiftByte(0x02, ShiftReg::MSBFirst); 00560 Shift_Reg.Latch(); 00561 break; 00562 00563 case 6 : 00564 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst); 00565 Shift_Reg.ShiftByte(0x03, ShiftReg::MSBFirst); 00566 Shift_Reg.Latch(); 00567 break; 00568 00569 case 7 : 00570 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst); 00571 Shift_Reg.ShiftByte(0x01, ShiftReg::MSBFirst); 00572 Shift_Reg.Latch(); 00573 break; 00574 00575 case 8 : 00576 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst); 00577 Shift_Reg.ShiftByte(0x09, ShiftReg::MSBFirst); 00578 Shift_Reg.Latch(); 00579 break; 00580 00581 default : 00582 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst); 00583 Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst); 00584 Shift_Reg.Latch(); 00585 break; 00586 } 00587 } 00588 00589 void Motor2_Stepping(int steps, bool dir, int count) 00590 { 00591 for(int i = 0; i < steps; i++) 00592 { 00593 Phases2(count); 00594 if(dir) 00595 { 00596 if(count < 8) count++; 00597 else count = 1; 00598 wait(0.001); 00599 } 00600 00601 else 00602 { 00603 if(count > 1) count--; 00604 else count = 8; 00605 wait(0.001); 00606 } 00607 } 00608 } 00609 00610 void intStringSplit(string in_string, int* int_Strings, float* float_Strings) 00611 { 00612 char in[128]; 00613 strcpy(in, in_string.c_str()); 00614 int y = 0; 00615 00616 for(int i = 0; i < 6; i++) 00617 { 00618 char number[10] = " "; 00619 for(int x = 0; x < 11; x++) 00620 { 00621 if(in[y] == ',') break; 00622 00623 else 00624 { 00625 number[x] = in[y]; 00626 y++; 00627 } 00628 } 00629 00630 if(i<4) int_Strings[i] = atoi(number); 00631 else float_Strings[i - 4] = atof(number); 00632 y++; 00633 } 00634 } 00635 00636 bool int_numcmp(int numof, int in_number, int cmp_number[], int tolerance[]) 00637 { 00638 int min[numof]; 00639 int max[numof]; 00640 int sum = 0; 00641 bool answer; 00642 00643 for(int x = 1; x < numof; x++) 00644 { 00645 min[x] = cmp_number[x] - tolerance[x]; 00646 max[x] = cmp_number[x] + tolerance[x]; 00647 00648 if(in_number >= min[x] && in_number <= max[x]) sum = sum + 1; 00649 else sum = sum + 0; 00650 } 00651 00652 if(sum == 0) answer = false; 00653 else answer = true; 00654 00655 return answer; 00656 } 00657 00658 bool float_numcmp(int numof, float in_number, float cmp_number[], float tolerance[]) 00659 { 00660 float min[numof]; 00661 float max[numof]; 00662 int sum = 0; 00663 bool answer; 00664 00665 for(int x = 1; x < numof; x++) 00666 { 00667 min[x] = cmp_number[x] - tolerance[x]; 00668 max[x] = cmp_number[x] + tolerance[x]; 00669 00670 if(in_number >= min[x] && in_number <= max[x]) sum = sum + 1; 00671 else sum = sum + 0; 00672 } 00673 00674 if(sum == 0) answer = false; 00675 else answer = true; 00676 00677 return answer; 00678 } 00679 00680 void SweeperArmCC() 00681 { 00682 00683 } 00684 00685 void SweeperArmACC() 00686 { 00687 00688 } 00689 00690 float AreaScan(int Reso) 00691 { 00692 float Sum = 0; 00693 float current_radius = 0; 00694 float previous_radius = 0; 00695 float Distance_center = 9.8; 00696 int numofsteps = 4096/Reso; 00697 previous_radius = Distance_center - avgDistance_sensor(); 00698 for(int i = 0; i < Reso; i++) 00699 { 00700 Motor1_Stepping(numofsteps,false,Steps1); 00701 current_radius = Distance_center - Distance_sensor(); 00702 Sum = Sum + Area(previous_radius,current_radius,numofsteps); 00703 previous_radius = current_radius; 00704 lcd.locate(0,3); 00705 lcd.printf("D:%fcm", current_radius); 00706 } 00707 Motor1_Stepping(4096,true,Steps1); 00708 Phases1(0); 00709 return Sum; 00710 } 00711 00712 float Distance_sensor() 00713 { 00714 float Input = IR_Sensor.read(); 00715 float Distance = (43.048*Input*Input) - (67.794*Input) + 31.306; 00716 return Distance; 00717 } 00718 00719 float avgDistance_sensor() 00720 { 00721 float Distance = 0; 00722 for(int x = 0; x < 3; x++) 00723 { 00724 Distance = Distance + Distance_sensor(); 00725 wait(0.005); 00726 } 00727 Distance = Distance/3; 00728 return Distance; 00729 } 00730 00731 float Area(float a, float b, int steps) 00732 { 00733 const double degrees = 360; 00734 const double numofsteps = 4096; 00735 double param = (degrees*steps/numofsteps); 00736 float result = sin (param*PI/180); 00737 float area = 0.5*(a*b*result); 00738 00739 return area; 00740 } 00741 00742 void Operation_mode() 00743 { 00744 while(true) 00745 { 00746 lcd.cls(); 00747 lcd.locate(4,1); 00748 lcd.printf("Operation"); 00749 lcd.locate(11,2); 00750 lcd.printf("Mode"); 00751 char OperationCommand[128]; 00752 pc.scanf("%s", OperationCommand); 00753 00754 if(strcmp(OperationCommand,"Start") == 0) 00755 { 00756 lcd.cls(); 00757 lcd.locate(0,0); 00758 lcd.printf("Start Sorting \n"); 00759 pc.printf("Begin"); 00760 00761 pc.scanf("%s", Command); 00762 int NumberofSettings = atoi(Command); 00763 lcd.locate(0,1); 00764 lcd.printf("%d Settings", NumberofSettings); 00765 pc.printf("OK"); 00766 H_Settings.clear(); 00767 for(int x = 0; x < (NumberofSettings + 1); x++) 00768 { 00769 pc.scanf("%s",Command); 00770 H_Settings.push_back(Command); 00771 pc.printf("OK"); 00772 lcd.locate(0,3); 00773 lcd.printf("%s", Command); 00774 } 00775 00776 BeginSorting(); 00777 00778 } 00779 00780 else if(strcmp(OperationCommand,"ColourScan") == 0) 00781 { 00782 lcd.cls(); 00783 lcd.locate(0,0); 00784 lcd.printf("Scanning Colour"); 00785 Colour_sensor.getAllColors( rgb_readings ); 00786 pc.printf("%d,%d,%d", rgb_readings[0],rgb_readings[1],rgb_readings[2]); 00787 lcd.locate(0,1); 00788 lcd.printf("Red : %d", rgb_readings[0]); 00789 lcd.locate(0,2); 00790 lcd.printf("Green : %d", rgb_readings[1]); 00791 lcd.locate(0,3); 00792 lcd.printf("Blue : %d", rgb_readings[2]); 00793 wait(2); 00794 } 00795 00796 else if(strcmp(OperationCommand,"AreaScan") == 0) 00797 { 00798 lcd.cls(); 00799 pc.scanf("%s", Command); 00800 int reso = atoi(Command); 00801 lcd.locate(0,0); 00802 lcd.printf("Scanning Area...."); 00803 lcd.locate(0,1); 00804 lcd.printf("Res: %d", reso); 00805 float area = AreaScan(reso); 00806 lcd.locate(0,3); 00807 lcd.printf(" "); 00808 lcd.locate(0,2); 00809 //pc.printf("OK"); 00810 wait(1.6); 00811 pc.printf("%f", area); 00812 lcd.printf("Area: %fcm^2", area); 00813 wait(2); 00814 } 00815 00816 else if(strcmp(OperationCommand,"Return") == 0) break; 00817 00818 else 00819 { 00820 00821 } 00822 00823 } 00824 } 00825 00826 void Maintenance_mode() 00827 { 00828 while(true) 00829 { 00830 lcd.cls(); 00831 lcd.locate(0,0); 00832 lcd.printf("Maintenance Mode"); 00833 char MaintenanceCommand[128]; 00834 pc.scanf("%s", MaintenanceCommand); 00835 00836 if(strcmp(MaintenanceCommand,"ColourSensor") == 0) 00837 { 00838 lcd.locate(0,0); 00839 lcd.printf("Scanning Colour"); 00840 Colour_sensor.getAllColors( rgb_readings ); 00841 pc.printf("%d,%d,%d", rgb_readings[0],rgb_readings[1],rgb_readings[2]); 00842 lcd.locate(0,1); 00843 lcd.printf("Red : %d", rgb_readings[0]); 00844 lcd.locate(0,2); 00845 lcd.printf("Red : %d", rgb_readings[1]); 00846 lcd.locate(0,3); 00847 lcd.printf("Red : %d", rgb_readings[2]); 00848 wait(1); 00849 } 00850 00851 else if(strcmp(MaintenanceCommand,"SweeperArm") == 0) 00852 { 00853 00854 } 00855 00856 else if(strcmp(MaintenanceCommand,"ArmSweepLeft") == 0) 00857 { 00858 SweeperArmCC(); 00859 wait(1.275); 00860 } 00861 00862 00863 else if(strcmp(MaintenanceCommand,"ArmSweepRight") == 0) 00864 { 00865 SweeperArmACC(); 00866 wait(1.275); 00867 } 00868 00869 else if(strcmp(MaintenanceCommand,"LP") == 0) 00870 { 00871 int stepper; 00872 pc.printf("OK"); 00873 scanf("%s", MaintenanceCommand); 00874 stepper = atoi(MaintenanceCommand); 00875 Motor1_Stepping(stepper, false, Steps1); 00876 } 00877 00878 else if(strcmp(MaintenanceCommand,"RP") == 0) 00879 { 00880 int stepper; 00881 pc.printf("OK"); 00882 scanf("%s", MaintenanceCommand); 00883 stepper = atoi(MaintenanceCommand); 00884 Motor1_Stepping(stepper, true, Steps1); 00885 } 00886 00887 else if(strcmp(MaintenanceCommand,"uIR") == 0) 00888 { 00889 int stepper; 00890 pc.printf("OK"); 00891 scanf("%s", MaintenanceCommand); 00892 stepper = atoi(MaintenanceCommand); 00893 Motor2_Stepping(stepper, true, Steps2); 00894 } 00895 00896 else if(strcmp(MaintenanceCommand,"dIR") == 0) 00897 { 00898 int stepper; 00899 pc.printf("OK"); 00900 scanf("%s", MaintenanceCommand); 00901 stepper = atoi(MaintenanceCommand); 00902 Motor2_Stepping(stepper, false, Steps2); 00903 } 00904 00905 else if(strcmp(MaintenanceCommand,"IRSensor") == 0) 00906 { 00907 float value = Distance_sensor(); 00908 pc.printf("%fcm",value); 00909 } 00910 00911 else if(strcmp(MaintenanceCommand,"AreaScan") == 0) 00912 { 00913 pc.printf("OK"); 00914 lcd.locate(0,0); 00915 lcd.printf("Scanning Area"); 00916 pc.scanf("%s", Command); 00917 int Reso = atoi(Command); 00918 lcd.locate(0,1); 00919 lcd.printf("Resolution : %d points", Reso); 00920 float val = AreaScan(Reso); 00921 lcd.locate(0,3); 00922 lcd.printf(" "); 00923 lcd.locate(0,2); 00924 lcd.printf("Area: %fcm^2",val); 00925 pc.printf("%fcm^2",val); 00926 wait(1); 00927 } 00928 else if(strcmp(MaintenanceCommand,"Return") == 0) break; 00929 00930 else 00931 { 00932 //pc.printf("ERROR"); 00933 00934 } 00935 00936 } 00937 } 00938 00939 void BeginSorting() 00940 { 00941 lcd.cls(); 00942 int Resolution; 00943 int Settings_int[4]; 00944 float Settings_float[2]; 00945 int r[128]; 00946 int g[128]; 00947 int b[128]; 00948 int Colour_tolerance[128]; 00949 float Area[128]; 00950 float Area_tolerance[128]; 00951 00952 Resolution = atoi(H_Settings.at(0).c_str()); 00953 for(int x = 1; x < H_Settings.size(); x++) 00954 { 00955 intStringSplit(H_Settings.at(x), Settings_int, Settings_float); 00956 r[x] = Settings_int[0]; 00957 g[x] = Settings_int[1]; 00958 b[x] = Settings_int[2]; 00959 Colour_tolerance[x] = Settings_int[3]; 00960 Area[x] = Settings_float[0]; 00961 Area_tolerance[x] = Settings_float[1]; 00962 } 00963 lcd.locate(0,2); 00964 lcd.printf("Settings Sorted"); 00965 00966 bool r_check; 00967 bool g_check; 00968 bool b_check; 00969 bool a_check; 00970 bool Hazard = 0; 00971 Sort = false; 00972 Stop = false; 00973 while(true) 00974 { 00975 if(Sort) 00976 { 00977 wait(1); 00978 if(!Stop) 00979 { 00980 lcd.cls(); 00981 Colour_sensor.getAllColors( rgb_readings ); 00982 r_check = int_numcmp(H_Settings.size(), rgb_readings[0], r, Colour_tolerance); 00983 g_check = int_numcmp(H_Settings.size(), rgb_readings[1], g, Colour_tolerance); 00984 b_check = int_numcmp(H_Settings.size(), rgb_readings[2], b, Colour_tolerance); 00985 00986 Area_readings = AreaScan(Resolution); 00987 a_check = float_numcmp(H_Settings.size(), Area_readings, Area, Area_tolerance); 00988 lcd.locate(0,0); 00989 lcd.printf("Red : %d", rgb_readings[0]); 00990 lcd.locate(0,1); 00991 lcd.printf("Green : %d", rgb_readings[1]); 00992 lcd.locate(0,2); 00993 lcd.printf("Blue : %d", rgb_readings[2]); 00994 lcd.locate(0,3); 00995 lcd.printf("Area : %f", Area_readings); 00996 if(r_check && g_check && b_check && a_check) Hazard = 1; 00997 else Hazard = 0; 00998 pc.printf("%d,%d,%d, ,%f, , , ,%d",rgb_readings[0], rgb_readings[1], rgb_readings[2], Area_readings, Hazard);//, Colour_tolerance[x]); 00999 //string fej("%d,%d,%d, ,%d, , ,%d",rgb_readings[0], rgb_readings[1], rgb_readings[2], Area_readings, Hazard); 01000 01001 ArmIn = 1; 01002 if(Hazard) Object_Hazardous(); 01003 else Object_NonHazardous(); 01004 Sort = false; 01005 } 01006 } 01007 else 01008 { 01009 RGB_Red = 1.0; 01010 RGB_Green = 1.0; 01011 } 01012 01013 if(Stop) 01014 { 01015 RGB_Red = 1.0; 01016 RGB_Green = 1.0; 01017 Stop = false; 01018 break; 01019 } 01020 } 01021 01022 } 01023 01024 01025 01026 void Object_Hazardous() 01027 { 01028 SL1 = 1; 01029 RGB_Red = 0.95; 01030 //RGB_Blue = 1.0; 01031 RGB_Green = 1.0; 01032 SweeperArmACC(); 01033 wait(1); 01034 ArmIn = 0; 01035 SL1 = 0; 01036 } 01037 01038 void Object_NonHazardous() 01039 { 01040 SL2 = 1; 01041 RGB_Red = 1.0; 01042 //RGB_Blue = 1.0; 01043 RGB_Green = 0.95; 01044 SweeperArmCC(); 01045 wait(1); 01046 ArmIn = 0; 01047 SL2 = 0; 01048 }
Generated on Wed Jul 13 2022 16:44:38 by
1.7.2