Khayhen Sarveswarah / Mbed 2 deprecated System_Project_V6

MBED file for Heriot-Watt Malaysia System Project 2016

Dependencies:   PinDetect SDFileSystem ShiftReg TCS3472_I2C TextLCD mRotaryEncoder mbed

main.cpp@0:e052961f153d, 2017-01-16 (annotated)

Committer:
Khayhen
Date:
Mon Jan 16 02:02:13 2017 +0000
Revision:
0:e052961f153d
Change LCD file

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Khayhen 0:e052961f153d 1 #include "mbed.h"
Khayhen 0:e052961f153d 2 #include "PinDetect.h"
Khayhen 0:e052961f153d 3 #include "SDFileSystem.h"
Khayhen 0:e052961f153d 4 #include "mRotaryEncoder.h"
Khayhen 0:e052961f153d 5 #include "TextLCD.h"
Khayhen 0:e052961f153d 6 #include "TCS3472_I2C.h"
Khayhen 0:e052961f153d 7 #include "ShiftReg.h"
Khayhen 0:e052961f153d 8 #include <stdlib.h>
Khayhen 0:e052961f153d 9 #include <string>
Khayhen 0:e052961f153d 10 #include <vector>
Khayhen 0:e052961f153d 11
Khayhen 0:e052961f153d 12 #define PI 3.14159265
Khayhen 0:e052961f153d 13
Khayhen 0:e052961f153d 14 /////////////////////////////////////////////////// Pin Decleration ///////////////////////////////////////////////////
Khayhen 0:e052961f153d 15 // IR Trigger
Khayhen 0:e052961f153d 16 AnalogIn IR_Trigger(p16);
Khayhen 0:e052961f153d 17
Khayhen 0:e052961f153d 18 // Shift Registers (74Shift_RegN)
Khayhen 0:e052961f153d 19 ShiftReg Shift_Reg(p17, p20, p22); // Data, Store, Clock
Khayhen 0:e052961f153d 20
Khayhen 0:e052961f153d 21 // SD_Card module
Khayhen 0:e052961f153d 22 SDFileSystem sd(p11, p12, p13, p14, "sd"); // MOSI, MISO, SCK, CS
Khayhen 0:e052961f153d 23
Khayhen 0:e052961f153d 24 // Host PC Communication channels
Khayhen 0:e052961f153d 25 Serial pc(USBTX, USBRX); // tx, rx
Khayhen 0:e052961f153d 26
Khayhen 0:e052961f153d 27 // Arm Signal
Khayhen 0:e052961f153d 28 PinDetect ArmOut(p27, PullDown);
Khayhen 0:e052961f153d 29 DigitalOut ArmIn(p23);
Khayhen 0:e052961f153d 30
Khayhen 0:e052961f153d 31 // LCD Display
Khayhen 0:e052961f153d 32 I2C i2c_lcd(p9,p10); // SDA, SCL
Khayhen 0:e052961f153d 33 TextLCD_I2C lcd(&i2c_lcd, 0x4E, TextLCD::LCD20x4); // I2C bus, PCF8574 Slaveaddress, LCD Type
Khayhen 0:e052961f153d 34
Khayhen 0:e052961f153d 35 // RGB LED
Khayhen 0:e052961f153d 36 PwmOut RGB_Red(p24);
Khayhen 0:e052961f153d 37 PwmOut RGB_Green(p25);
Khayhen 0:e052961f153d 38 //PwmOut RGB_Blue(p26);
Khayhen 0:e052961f153d 39
Khayhen 0:e052961f153d 40 // Rotary Encoder
Khayhen 0:e052961f153d 41 mRotaryEncoder RE(p5,p6,p7,PullUp,1000); // Pin A, Pin B, SW
Khayhen 0:e052961f153d 42
Khayhen 0:e052961f153d 43 // Speaker
Khayhen 0:e052961f153d 44 //AnalogOut Speaker(p18);
Khayhen 0:e052961f153d 45
Khayhen 0:e052961f153d 46 // Slide LEDs
Khayhen 0:e052961f153d 47 DigitalOut SL1(p19);
Khayhen 0:e052961f153d 48 DigitalOut SL2(p28);
Khayhen 0:e052961f153d 49
Khayhen 0:e052961f153d 50 // Colour Sensor (TCS3472)
Khayhen 0:e052961f153d 51 TCS3472_I2C Colour_sensor(p9, p10); // SDA, SCL
Khayhen 0:e052961f153d 52
Khayhen 0:e052961f153d 53 // VL6180X SATEL
Khayhen 0:e052961f153d 54
Khayhen 0:e052961f153d 55 // IR Sensor
Khayhen 0:e052961f153d 56 AnalogIn IR_Sensor(p18);
Khayhen 0:e052961f153d 57
Khayhen 0:e052961f153d 58
Khayhen 0:e052961f153d 59 //////////////////////////////////////////////// Custom LCD Characters ////////////////////////////////////////////////
Khayhen 0:e052961f153d 60 char pointer[] = {0x10,0x18,0x1c,0x1e,0x1e,0x1c,0x18,0x10}; // |>
Khayhen 0:e052961f153d 61
Khayhen 0:e052961f153d 62
Khayhen 0:e052961f153d 63 ////////////////////////////////////////////////////// Functions //////////////////////////////////////////////////////
Khayhen 0:e052961f153d 64 void Operation_mode();
Khayhen 0:e052961f153d 65 void Maintenance_mode();
Khayhen 0:e052961f153d 66 void BeginSorting();
Khayhen 0:e052961f153d 67 void Object_Hazardous();
Khayhen 0:e052961f153d 68 void Object_NonHazardous();
Khayhen 0:e052961f153d 69
Khayhen 0:e052961f153d 70 void sw();
Khayhen 0:e052961f153d 71 void ButtonPressed();
Khayhen 0:e052961f153d 72 void ButtonHeld();
Khayhen 0:e052961f153d 73 bool read_file_names(char *dir);
Khayhen 0:e052961f153d 74 int LCD_Select(vector<string> list);
Khayhen 0:e052961f153d 75 float AreaScan(int resolution);
Khayhen 0:e052961f153d 76 float Distance_sensor();
Khayhen 0:e052961f153d 77 float avgDistance_sensor();
Khayhen 0:e052961f153d 78 float Area(float a, float b, int steps);
Khayhen 0:e052961f153d 79 void Phases1(int phase);
Khayhen 0:e052961f153d 80 void Phases2(int phase);
Khayhen 0:e052961f153d 81 void Motor1_Stepping(int steps, bool dir, int count);
Khayhen 0:e052961f153d 82 void Motor2_Stepping(int steps, bool dir, int count);
Khayhen 0:e052961f153d 83 void intStringSplit(char in[100], int* int_Strings, float* float_Strings);
Khayhen 0:e052961f153d 84 bool int_numcmp(int numof, int in_number, int cmp_number[], int tolerance[]);
Khayhen 0:e052961f153d 85 bool float_numcmp(int numof, float in_number, float cmp_number[], float tolerance[]);
Khayhen 0:e052961f153d 86 void SweeperArmCC();
Khayhen 0:e052961f153d 87 void SweeperArmACC();
Khayhen 0:e052961f153d 88
Khayhen 0:e052961f153d 89 ////////////////////////////////////////////////////// Variables //////////////////////////////////////////////////////
Khayhen 0:e052961f153d 90 string PC_SDlist[] = {"Connect to PC",
Khayhen 0:e052961f153d 91 "Access to SD"};
Khayhen 0:e052961f153d 92 string SD_Optionslist[] = {"Load",
Khayhen 0:e052961f153d 93 "Rename",
Khayhen 0:e052961f153d 94 "Remove",
Khayhen 0:e052961f153d 95 "Back"};
Khayhen 0:e052961f153d 96 vector<string> PC_SD(PC_SDlist, PC_SDlist + 2); //Selection between PC and SD
Khayhen 0:e052961f153d 97 vector<string> Filenames; //Filenamess are stored in a vector string
Khayhen 0:e052961f153d 98 vector<string> SD_Options(SD_Optionslist, SD_Optionslist + 4); //Options to pick when SD file selected
Khayhen 0:e052961f153d 99 vector<string> SD_Content; //Store settings from SD card
Khayhen 0:e052961f153d 100 vector<string> H_Settings; //Stores Hazardous settings
Khayhen 0:e052961f153d 101 vector<string> Sorted_Data; //Stores data of sorted items
Khayhen 0:e052961f153d 102
Khayhen 0:e052961f153d 103 int Select = 0;
Khayhen 0:e052961f153d 104 bool Sort = false;
Khayhen 0:e052961f153d 105 bool Stop = false;
Khayhen 0:e052961f153d 106 int PC_SDmode = 0;
Khayhen 0:e052961f153d 107 int Filenumber = 0;
Khayhen 0:e052961f153d 108 int SD_Optionnumber = 0;
Khayhen 0:e052961f153d 109 char Command[512];
Khayhen 0:e052961f153d 110 int Steps1 = 1;
Khayhen 0:e052961f153d 111 int Steps2 = 1;
Khayhen 0:e052961f153d 112 int rgb_readings[4];
Khayhen 0:e052961f153d 113 float Area_readings;
Khayhen 0:e052961f153d 114
Khayhen 0:e052961f153d 115 ///////////////////////////////////////////////////// Main Program /////////////////////////////////////////////////////
Khayhen 0:e052961f153d 116 int main()
Khayhen 0:e052961f153d 117 {
Khayhen 0:e052961f153d 118
Khayhen 0:e052961f153d 119
Khayhen 0:e052961f153d 120 RGB_Red = 1.0;
Khayhen 0:e052961f153d 121 //RGB_Blue = 1.0;
Khayhen 0:e052961f153d 122 RGB_Green = 1.0;
Khayhen 0:e052961f153d 123 lcd.setCursor(TextLCD::CurOff_BlkOff);
Khayhen 0:e052961f153d 124 lcd.cls();
Khayhen 0:e052961f153d 125 lcd.locate(4,1);
Khayhen 0:e052961f153d 126 lcd.printf("MIRACLE");
Khayhen 0:e052961f153d 127 lcd.locate(9,2);
Khayhen 0:e052961f153d 128 lcd.printf("TECH");
Khayhen 0:e052961f153d 129 lcd.setUDC(0,pointer);
Khayhen 0:e052961f153d 130
Khayhen 0:e052961f153d 131 SweeperArmCC();
Khayhen 0:e052961f153d 132 Colour_sensor.enablePowerAndRGBC();
Khayhen 0:e052961f153d 133 Colour_sensor.setIntegrationTime( 100 );
Khayhen 0:e052961f153d 134
Khayhen 0:e052961f153d 135 RE.Set(0);
Khayhen 0:e052961f153d 136 RE.attachSW(&sw);
Khayhen 0:e052961f153d 137 ArmOut.attach_asserted(&ButtonPressed);
Khayhen 0:e052961f153d 138 ArmOut.attach_asserted_held(&ButtonHeld);
Khayhen 0:e052961f153d 139 ArmOut.setSampleFrequency();
Khayhen 0:e052961f153d 140 wait(1);
Khayhen 0:e052961f153d 141 lcd.cls();
Khayhen 0:e052961f153d 142
Khayhen 0:e052961f153d 143 ////////////////////////////////SYSTEM BEGIN////////////////////////////////
Khayhen 0:e052961f153d 144 //Selecting between PC and SD_Card mode
Khayhen 0:e052961f153d 145 while(true)
Khayhen 0:e052961f153d 146 {
Khayhen 0:e052961f153d 147 lcd.cls();
Khayhen 0:e052961f153d 148 PC_SDmode = LCD_Select(PC_SD);
Khayhen 0:e052961f153d 149 switch(PC_SDmode)
Khayhen 0:e052961f153d 150 {
Khayhen 0:e052961f153d 151 //////////////////////////////////////////PC mode//////////////////////////////////////////
Khayhen 0:e052961f153d 152 case 0:
Khayhen 0:e052961f153d 153 {
Khayhen 0:e052961f153d 154 lcd.locate(5,1);
Khayhen 0:e052961f153d 155 lcd.printf("WAITING TO");
Khayhen 0:e052961f153d 156 lcd.locate(4,2);
Khayhen 0:e052961f153d 157 lcd.printf("CONNECT ");
Khayhen 0:e052961f153d 158 while(true)
Khayhen 0:e052961f153d 159 {
Khayhen 0:e052961f153d 160 lcd.locate(12,2);
Khayhen 0:e052961f153d 161 lcd.printf(". ");
Khayhen 0:e052961f153d 162 wait(0.2);
Khayhen 0:e052961f153d 163 lcd.locate(12,2);
Khayhen 0:e052961f153d 164 lcd.printf(".. ");
Khayhen 0:e052961f153d 165 wait(0.2);
Khayhen 0:e052961f153d 166 lcd.locate(12,2);
Khayhen 0:e052961f153d 167 lcd.printf("... ");
Khayhen 0:e052961f153d 168 wait(0.2);
Khayhen 0:e052961f153d 169 lcd.locate(12,2);
Khayhen 0:e052961f153d 170 lcd.printf(".... ");
Khayhen 0:e052961f153d 171 wait(0.2);
Khayhen 0:e052961f153d 172 if(pc.readable())
Khayhen 0:e052961f153d 173 {
Khayhen 0:e052961f153d 174 pc.scanf("%s",Command);
Khayhen 0:e052961f153d 175 lcd.cls();
Khayhen 0:e052961f153d 176 lcd.locate(5,1);
Khayhen 0:e052961f153d 177 lcd.printf("%s",Command);
Khayhen 0:e052961f153d 178 wait(1);
Khayhen 0:e052961f153d 179 while(true)
Khayhen 0:e052961f153d 180 {
Khayhen 0:e052961f153d 181 if(pc.readable())
Khayhen 0:e052961f153d 182 {
Khayhen 0:e052961f153d 183 lcd.cls();
Khayhen 0:e052961f153d 184 lcd.locate(8,1);
Khayhen 0:e052961f153d 185 lcd.printf("MODE");
Khayhen 0:e052961f153d 186 lcd.locate(7,2);
Khayhen 0:e052961f153d 187 lcd.printf("SELECT");
Khayhen 0:e052961f153d 188 pc.scanf("%s", Command);
Khayhen 0:e052961f153d 189 if(strcmp(Command,"Operation") == 0) Operation_mode();
Khayhen 0:e052961f153d 190 else if(strcmp(Command,"Maintenance") == 0) Maintenance_mode();
Khayhen 0:e052961f153d 191 else if(strcmp(Command,"Disconnect") == 0) break;
Khayhen 0:e052961f153d 192 else
Khayhen 0:e052961f153d 193 {
Khayhen 0:e052961f153d 194 lcd.cls();
Khayhen 0:e052961f153d 195 lcd.locate(7,1);
Khayhen 0:e052961f153d 196 lcd.printf("ERROR!");
Khayhen 0:e052961f153d 197 wait(2);
Khayhen 0:e052961f153d 198 }
Khayhen 0:e052961f153d 199 }
Khayhen 0:e052961f153d 200 }
Khayhen 0:e052961f153d 201 break;
Khayhen 0:e052961f153d 202 }
Khayhen 0:e052961f153d 203 }
Khayhen 0:e052961f153d 204
Khayhen 0:e052961f153d 205
Khayhen 0:e052961f153d 206 }
Khayhen 0:e052961f153d 207
Khayhen 0:e052961f153d 208 //////////////////////////////////////////SD Card mode//////////////////////////////////////////
Khayhen 0:e052961f153d 209 case 1:
Khayhen 0:e052961f153d 210 {
Khayhen 0:e052961f153d 211 sd.mount();
Khayhen 0:e052961f153d 212 lcd.cls();
Khayhen 0:e052961f153d 213 Filenames.clear();
Khayhen 0:e052961f153d 214
Khayhen 0:e052961f153d 215 while(true)
Khayhen 0:e052961f153d 216 {
Khayhen 0:e052961f153d 217 // print Filenames strings from vector using an iterator
Khayhen 0:e052961f153d 218 Filenames.clear();
Khayhen 0:e052961f153d 219 read_file_names("/sd");
Khayhen 0:e052961f153d 220 Filenumber = LCD_Select(Filenames);
Khayhen 0:e052961f153d 221 while(true)
Khayhen 0:e052961f153d 222 {
Khayhen 0:e052961f153d 223 if(Filenumber == 0) break;
Khayhen 0:e052961f153d 224 SD_Optionnumber = LCD_Select(SD_Options);
Khayhen 0:e052961f153d 225 switch(SD_Optionnumber)
Khayhen 0:e052961f153d 226 {
Khayhen 0:e052961f153d 227 case 0 : //Load settings
Khayhen 0:e052961f153d 228 {
Khayhen 0:e052961f153d 229 lcd.cls();
Khayhen 0:e052961f153d 230 lcd.locate(0,0);
Khayhen 0:e052961f153d 231 lcd.printf("%s", Filenames.at(Filenumber).c_str());
Khayhen 0:e052961f153d 232
Khayhen 0:e052961f153d 233 string source_directory = "/sd/";
Khayhen 0:e052961f153d 234 string file_directory = Filenames.at(Filenumber);
Khayhen 0:e052961f153d 235 string directory = source_directory + file_directory;
Khayhen 0:e052961f153d 236 SD_Content.clear();
Khayhen 0:e052961f153d 237 FILE *fp = fopen(directory.c_str(), "r");
Khayhen 0:e052961f153d 238 while(!feof(fp))
Khayhen 0:e052961f153d 239 {
Khayhen 0:e052961f153d 240 char word[128];
Khayhen 0:e052961f153d 241 fgets(word,128,fp);
Khayhen 0:e052961f153d 242 SD_Content.push_back(word);
Khayhen 0:e052961f153d 243 }
Khayhen 0:e052961f153d 244
Khayhen 0:e052961f153d 245 int NumberofSettings = atoi(SD_Content.at(0).c_str());
Khayhen 0:e052961f153d 246
Khayhen 0:e052961f153d 247 if(NumberofSettings == (SD_Content.size() - 3))
Khayhen 0:e052961f153d 248 {
Khayhen 0:e052961f153d 249 lcd.locate(0,1);
Khayhen 0:e052961f153d 250 lcd.printf("Num of Settings : %d", NumberofSettings);
Khayhen 0:e052961f153d 251 }
Khayhen 0:e052961f153d 252 else
Khayhen 0:e052961f153d 253 {
Khayhen 0:e052961f153d 254 lcd.locate(7,2);
Khayhen 0:e052961f153d 255 lcd.printf("ERROR!");
Khayhen 0:e052961f153d 256 wait(1);
Khayhen 0:e052961f153d 257 break;
Khayhen 0:e052961f153d 258 }
Khayhen 0:e052961f153d 259
Khayhen 0:e052961f153d 260 H_Settings.clear();
Khayhen 0:e052961f153d 261 for(int i = 1; i < (SD_Content.size()-1); i++)
Khayhen 0:e052961f153d 262 {
Khayhen 0:e052961f153d 263 H_Settings.push_back(SD_Content.at(i));
Khayhen 0:e052961f153d 264 }
Khayhen 0:e052961f153d 265
Khayhen 0:e052961f153d 266 lcd.locate(0,3);
Khayhen 0:e052961f153d 267 lcd.printf("%d", H_Settings.size());
Khayhen 0:e052961f153d 268 BeginSorting();
Khayhen 0:e052961f153d 269
Khayhen 0:e052961f153d 270 }
Khayhen 0:e052961f153d 271
Khayhen 0:e052961f153d 272 case 1 :
Khayhen 0:e052961f153d 273 {
Khayhen 0:e052961f153d 274 //Rename file
Khayhen 0:e052961f153d 275 }
Khayhen 0:e052961f153d 276
Khayhen 0:e052961f153d 277 case 2 : //Delete File
Khayhen 0:e052961f153d 278 {
Khayhen 0:e052961f153d 279 sd.remove(Filenames.at(Filenumber).c_str());
Khayhen 0:e052961f153d 280 break;
Khayhen 0:e052961f153d 281 }
Khayhen 0:e052961f153d 282
Khayhen 0:e052961f153d 283 case 3 : break; //Go Back
Khayhen 0:e052961f153d 284 }
Khayhen 0:e052961f153d 285 break;
Khayhen 0:e052961f153d 286 }
Khayhen 0:e052961f153d 287 if(Filenumber == 0) break;
Khayhen 0:e052961f153d 288 }
Khayhen 0:e052961f153d 289 sd.unmount();
Khayhen 0:e052961f153d 290 break;
Khayhen 0:e052961f153d 291 }
Khayhen 0:e052961f153d 292
Khayhen 0:e052961f153d 293 default:
Khayhen 0:e052961f153d 294 {
Khayhen 0:e052961f153d 295 lcd.locate(7,1);
Khayhen 0:e052961f153d 296 lcd.printf("ERROR!");
Khayhen 0:e052961f153d 297 break;
Khayhen 0:e052961f153d 298 }
Khayhen 0:e052961f153d 299 }
Khayhen 0:e052961f153d 300 }
Khayhen 0:e052961f153d 301
Khayhen 0:e052961f153d 302 }
Khayhen 0:e052961f153d 303
Khayhen 0:e052961f153d 304 void sw()
Khayhen 0:e052961f153d 305 {
Khayhen 0:e052961f153d 306 RE.Set(0);
Khayhen 0:e052961f153d 307 Select = 1;
Khayhen 0:e052961f153d 308 }
Khayhen 0:e052961f153d 309 void ButtonPressed() {Sort = true;}
Khayhen 0:e052961f153d 310 void ButtonHeld() { Stop = true;}
Khayhen 0:e052961f153d 311
Khayhen 0:e052961f153d 312 bool read_file_names(char *dir)
Khayhen 0:e052961f153d 313 {
Khayhen 0:e052961f153d 314 bool present;
Khayhen 0:e052961f153d 315 Filenames.push_back("Back");
Khayhen 0:e052961f153d 316 DIR *dp;
Khayhen 0:e052961f153d 317 struct dirent *dirp;
Khayhen 0:e052961f153d 318 dp = opendir(dir);
Khayhen 0:e052961f153d 319 //read all directory and file names in current directory into filename vector
Khayhen 0:e052961f153d 320 if((dirp = readdir(dp)) != NULL) present = true;
Khayhen 0:e052961f153d 321 else present = false;
Khayhen 0:e052961f153d 322
Khayhen 0:e052961f153d 323 while((dirp = readdir(dp)) != NULL) {
Khayhen 0:e052961f153d 324 Filenames.push_back(string(dirp->d_name));
Khayhen 0:e052961f153d 325 }
Khayhen 0:e052961f153d 326 closedir(dp);
Khayhen 0:e052961f153d 327 return present;
Khayhen 0:e052961f153d 328 }
Khayhen 0:e052961f153d 329
Khayhen 0:e052961f153d 330 int LCD_Select(vector<string> list)
Khayhen 0:e052961f153d 331 {
Khayhen 0:e052961f153d 332 lcd.cls();
Khayhen 0:e052961f153d 333 int RE_val_c = 0;
Khayhen 0:e052961f153d 334 int RE_val_p = 1;
Khayhen 0:e052961f153d 335 int Selected;
Khayhen 0:e052961f153d 336 int size = list.size();
Khayhen 0:e052961f153d 337
Khayhen 0:e052961f153d 338 if(size > 4)
Khayhen 0:e052961f153d 339 {
Khayhen 0:e052961f153d 340 for(int i = 0; i < 4; i++)
Khayhen 0:e052961f153d 341 {
Khayhen 0:e052961f153d 342 lcd.locate(1,i);
Khayhen 0:e052961f153d 343 lcd.printf("%s", list.at(i).c_str());
Khayhen 0:e052961f153d 344 }
Khayhen 0:e052961f153d 345 while(true)
Khayhen 0:e052961f153d 346 {
Khayhen 0:e052961f153d 347 RE_val_c = RE.Get();
Khayhen 0:e052961f153d 348
Khayhen 0:e052961f153d 349 if(RE_val_c != RE_val_p)
Khayhen 0:e052961f153d 350 {
Khayhen 0:e052961f153d 351 if(RE_val_c > size-1)
Khayhen 0:e052961f153d 352 {
Khayhen 0:e052961f153d 353 RE.Set(0);
Khayhen 0:e052961f153d 354 RE_val_c = 0;
Khayhen 0:e052961f153d 355 lcd.cls();
Khayhen 0:e052961f153d 356 for(int i = 0; i < 4; i++)
Khayhen 0:e052961f153d 357 {
Khayhen 0:e052961f153d 358 lcd.locate(1,i);
Khayhen 0:e052961f153d 359 lcd.printf("%s", list.at(i).c_str());
Khayhen 0:e052961f153d 360 }
Khayhen 0:e052961f153d 361 }
Khayhen 0:e052961f153d 362 else if(RE_val_c < 0)
Khayhen 0:e052961f153d 363 {
Khayhen 0:e052961f153d 364 RE.Set(size-1);
Khayhen 0:e052961f153d 365 RE_val_c = size-1;
Khayhen 0:e052961f153d 366 lcd.cls();
Khayhen 0:e052961f153d 367 lcd.locate(1,0);
Khayhen 0:e052961f153d 368 lcd.printf("%s", list.at(RE_val_c - 3).c_str());
Khayhen 0:e052961f153d 369 lcd.locate(1,1);
Khayhen 0:e052961f153d 370 lcd.printf("%s", list.at(RE_val_c - 2).c_str());
Khayhen 0:e052961f153d 371 lcd.locate(1,2);
Khayhen 0:e052961f153d 372 lcd.printf("%s", list.at(RE_val_c - 1).c_str());
Khayhen 0:e052961f153d 373 lcd.locate(1,3);
Khayhen 0:e052961f153d 374 lcd.printf("%s", list.at(RE_val_c - 0).c_str());
Khayhen 0:e052961f153d 375 }
Khayhen 0:e052961f153d 376 if(RE_val_c > 2)
Khayhen 0:e052961f153d 377 {
Khayhen 0:e052961f153d 378 lcd.cls();
Khayhen 0:e052961f153d 379 lcd.locate(1,0);
Khayhen 0:e052961f153d 380 lcd.printf("%s", list.at(RE_val_c - 3).c_str());
Khayhen 0:e052961f153d 381 lcd.locate(1,1);
Khayhen 0:e052961f153d 382 lcd.printf("%s", list.at(RE_val_c - 2).c_str());
Khayhen 0:e052961f153d 383 lcd.locate(1,2);
Khayhen 0:e052961f153d 384 lcd.printf("%s", list.at(RE_val_c - 1).c_str());
Khayhen 0:e052961f153d 385 lcd.locate(1,3);
Khayhen 0:e052961f153d 386 lcd.printf("%s", list.at(RE_val_c - 0).c_str());
Khayhen 0:e052961f153d 387 }
Khayhen 0:e052961f153d 388
Khayhen 0:e052961f153d 389
Khayhen 0:e052961f153d 390
Khayhen 0:e052961f153d 391 for(int i = 0; i < 4; i++)
Khayhen 0:e052961f153d 392 {
Khayhen 0:e052961f153d 393 lcd.locate(0,i);
Khayhen 0:e052961f153d 394 lcd.printf(" ");
Khayhen 0:e052961f153d 395 }
Khayhen 0:e052961f153d 396 lcd.locate(0,RE_val_c);
Khayhen 0:e052961f153d 397 lcd.putc(0);
Khayhen 0:e052961f153d 398 RE_val_p = RE_val_c;
Khayhen 0:e052961f153d 399 }
Khayhen 0:e052961f153d 400 lcd.locate(19,0);
Khayhen 0:e052961f153d 401 lcd.printf("%d", RE_val_c);
Khayhen 0:e052961f153d 402 if(Select)
Khayhen 0:e052961f153d 403 {
Khayhen 0:e052961f153d 404 Selected = RE_val_c;
Khayhen 0:e052961f153d 405 Select = 0;
Khayhen 0:e052961f153d 406 lcd.cls();
Khayhen 0:e052961f153d 407 return Selected;
Khayhen 0:e052961f153d 408 }
Khayhen 0:e052961f153d 409 }
Khayhen 0:e052961f153d 410 }
Khayhen 0:e052961f153d 411
Khayhen 0:e052961f153d 412 else
Khayhen 0:e052961f153d 413 {
Khayhen 0:e052961f153d 414 for(int i = 0; i < size; i++)
Khayhen 0:e052961f153d 415 {
Khayhen 0:e052961f153d 416 lcd.locate(1,i);
Khayhen 0:e052961f153d 417 lcd.printf("%s", list.at(i).c_str());
Khayhen 0:e052961f153d 418 }
Khayhen 0:e052961f153d 419 while(true)
Khayhen 0:e052961f153d 420 {
Khayhen 0:e052961f153d 421 RE_val_c = RE.Get();
Khayhen 0:e052961f153d 422
Khayhen 0:e052961f153d 423
Khayhen 0:e052961f153d 424 if(RE_val_c != RE_val_p)
Khayhen 0:e052961f153d 425 {
Khayhen 0:e052961f153d 426 //if(RE_val_c > 3)
Khayhen 0:e052961f153d 427 // {
Khayhen 0:e052961f153d 428 // lcd.cls();
Khayhen 0:e052961f153d 429 // lcd.locate(1,0);
Khayhen 0:e052961f153d 430 // lcd.printf("%s", list.at(RE_val_c - 3).c_str());
Khayhen 0:e052961f153d 431 // lcd.locate(1,1);
Khayhen 0:e052961f153d 432 // lcd.printf("%s", list.at(RE_val_c - 2).c_str());
Khayhen 0:e052961f153d 433 // lcd.locate(1,2);
Khayhen 0:e052961f153d 434 // lcd.printf("%s", list.at(RE_val_c - 1).c_str());
Khayhen 0:e052961f153d 435 // lcd.locate(1,3);
Khayhen 0:e052961f153d 436 // lcd.printf("%s", list.at(RE_val_c - 0).c_str());
Khayhen 0:e052961f153d 437 // }
Khayhen 0:e052961f153d 438
Khayhen 0:e052961f153d 439
Khayhen 0:e052961f153d 440 if(RE_val_c > size-1) {RE.Set(0); RE_val_c = 0;}
Khayhen 0:e052961f153d 441 else if(RE_val_c < 0) {RE.Set(size-1); RE_val_c = size-1;}
Khayhen 0:e052961f153d 442 for(int i = 0; i < 4; i++)
Khayhen 0:e052961f153d 443 {
Khayhen 0:e052961f153d 444 lcd.locate(0,i);
Khayhen 0:e052961f153d 445 lcd.printf(" ");
Khayhen 0:e052961f153d 446 }
Khayhen 0:e052961f153d 447 lcd.locate(0,RE_val_c);
Khayhen 0:e052961f153d 448 lcd.putc(0);
Khayhen 0:e052961f153d 449 RE_val_p = RE_val_c;
Khayhen 0:e052961f153d 450 }
Khayhen 0:e052961f153d 451 lcd.locate(19,0);
Khayhen 0:e052961f153d 452 lcd.printf("%d", RE_val_c);
Khayhen 0:e052961f153d 453 if(Select)
Khayhen 0:e052961f153d 454 {
Khayhen 0:e052961f153d 455 Selected = RE_val_c;
Khayhen 0:e052961f153d 456 Select = 0;
Khayhen 0:e052961f153d 457 lcd.cls();
Khayhen 0:e052961f153d 458 return Selected;
Khayhen 0:e052961f153d 459 }
Khayhen 0:e052961f153d 460 }
Khayhen 0:e052961f153d 461 }
Khayhen 0:e052961f153d 462
Khayhen 0:e052961f153d 463
Khayhen 0:e052961f153d 464 }
Khayhen 0:e052961f153d 465
Khayhen 0:e052961f153d 466 void Phases1(int phase)
Khayhen 0:e052961f153d 467 {
Khayhen 0:e052961f153d 468 switch(phase)
Khayhen 0:e052961f153d 469 {
Khayhen 0:e052961f153d 470 case 1 :
Khayhen 0:e052961f153d 471 Shift_Reg.ShiftByte(0x80, ShiftReg::MSBFirst); Shift_Reg.Latch();
Khayhen 0:e052961f153d 472 break;
Khayhen 0:e052961f153d 473
Khayhen 0:e052961f153d 474 case 2 :
Khayhen 0:e052961f153d 475 Shift_Reg.ShiftByte(0xC0, ShiftReg::MSBFirst); Shift_Reg.Latch();
Khayhen 0:e052961f153d 476 break;
Khayhen 0:e052961f153d 477
Khayhen 0:e052961f153d 478 case 3 :
Khayhen 0:e052961f153d 479 Shift_Reg.ShiftByte(0x40, ShiftReg::MSBFirst); Shift_Reg.Latch();
Khayhen 0:e052961f153d 480 break;
Khayhen 0:e052961f153d 481
Khayhen 0:e052961f153d 482 case 4 :
Khayhen 0:e052961f153d 483 Shift_Reg.ShiftByte(0x60, ShiftReg::MSBFirst); Shift_Reg.Latch();
Khayhen 0:e052961f153d 484 break;
Khayhen 0:e052961f153d 485
Khayhen 0:e052961f153d 486 case 5 :
Khayhen 0:e052961f153d 487 Shift_Reg.ShiftByte(0x20, ShiftReg::MSBFirst); Shift_Reg.Latch();
Khayhen 0:e052961f153d 488 break;
Khayhen 0:e052961f153d 489
Khayhen 0:e052961f153d 490 case 6 :
Khayhen 0:e052961f153d 491 Shift_Reg.ShiftByte(0x30, ShiftReg::MSBFirst); Shift_Reg.Latch();
Khayhen 0:e052961f153d 492 break;
Khayhen 0:e052961f153d 493
Khayhen 0:e052961f153d 494 case 7 :
Khayhen 0:e052961f153d 495 Shift_Reg.ShiftByte(0x10, ShiftReg::MSBFirst); Shift_Reg.Latch();
Khayhen 0:e052961f153d 496 break;
Khayhen 0:e052961f153d 497
Khayhen 0:e052961f153d 498 case 8 :
Khayhen 0:e052961f153d 499 Shift_Reg.ShiftByte(0x90, ShiftReg::MSBFirst); Shift_Reg.Latch();
Khayhen 0:e052961f153d 500 break;
Khayhen 0:e052961f153d 501
Khayhen 0:e052961f153d 502 default :
Khayhen 0:e052961f153d 503 Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst); Shift_Reg.Latch();
Khayhen 0:e052961f153d 504 break;
Khayhen 0:e052961f153d 505 }
Khayhen 0:e052961f153d 506 }
Khayhen 0:e052961f153d 507
Khayhen 0:e052961f153d 508 void Motor1_Stepping(int steps, bool dir, int count)
Khayhen 0:e052961f153d 509 {
Khayhen 0:e052961f153d 510 for(int i = 0; i < steps; i++)
Khayhen 0:e052961f153d 511 {
Khayhen 0:e052961f153d 512 Phases1(count);
Khayhen 0:e052961f153d 513 if(dir)
Khayhen 0:e052961f153d 514 {
Khayhen 0:e052961f153d 515 if(count < 8) count++;
Khayhen 0:e052961f153d 516 else count = 1;
Khayhen 0:e052961f153d 517 wait(0.001);
Khayhen 0:e052961f153d 518 }
Khayhen 0:e052961f153d 519
Khayhen 0:e052961f153d 520 else
Khayhen 0:e052961f153d 521 {
Khayhen 0:e052961f153d 522 if(count > 1) count--;
Khayhen 0:e052961f153d 523 else count = 8;
Khayhen 0:e052961f153d 524 wait(0.001);
Khayhen 0:e052961f153d 525 }
Khayhen 0:e052961f153d 526 }
Khayhen 0:e052961f153d 527 }
Khayhen 0:e052961f153d 528
Khayhen 0:e052961f153d 529 void Phases2(int phase)
Khayhen 0:e052961f153d 530 {
Khayhen 0:e052961f153d 531 switch(phase)
Khayhen 0:e052961f153d 532 {
Khayhen 0:e052961f153d 533 case 1 :
Khayhen 0:e052961f153d 534 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 535 Shift_Reg.ShiftByte(0x08, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 536 Shift_Reg.Latch();
Khayhen 0:e052961f153d 537 break;
Khayhen 0:e052961f153d 538
Khayhen 0:e052961f153d 539 case 2 :
Khayhen 0:e052961f153d 540 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 541 Shift_Reg.ShiftByte(0x0C, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 542 Shift_Reg.Latch();
Khayhen 0:e052961f153d 543 break;
Khayhen 0:e052961f153d 544
Khayhen 0:e052961f153d 545 case 3 :
Khayhen 0:e052961f153d 546 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 547 Shift_Reg.ShiftByte(0x04, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 548 Shift_Reg.Latch();
Khayhen 0:e052961f153d 549 break;
Khayhen 0:e052961f153d 550
Khayhen 0:e052961f153d 551 case 4 :
Khayhen 0:e052961f153d 552 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 553 Shift_Reg.ShiftByte(0x06, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 554 Shift_Reg.Latch();
Khayhen 0:e052961f153d 555 break;
Khayhen 0:e052961f153d 556
Khayhen 0:e052961f153d 557 case 5 :
Khayhen 0:e052961f153d 558 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 559 Shift_Reg.ShiftByte(0x02, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 560 Shift_Reg.Latch();
Khayhen 0:e052961f153d 561 break;
Khayhen 0:e052961f153d 562
Khayhen 0:e052961f153d 563 case 6 :
Khayhen 0:e052961f153d 564 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 565 Shift_Reg.ShiftByte(0x03, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 566 Shift_Reg.Latch();
Khayhen 0:e052961f153d 567 break;
Khayhen 0:e052961f153d 568
Khayhen 0:e052961f153d 569 case 7 :
Khayhen 0:e052961f153d 570 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 571 Shift_Reg.ShiftByte(0x01, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 572 Shift_Reg.Latch();
Khayhen 0:e052961f153d 573 break;
Khayhen 0:e052961f153d 574
Khayhen 0:e052961f153d 575 case 8 :
Khayhen 0:e052961f153d 576 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 577 Shift_Reg.ShiftByte(0x09, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 578 Shift_Reg.Latch();
Khayhen 0:e052961f153d 579 break;
Khayhen 0:e052961f153d 580
Khayhen 0:e052961f153d 581 default :
Khayhen 0:e052961f153d 582 //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 583 Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
Khayhen 0:e052961f153d 584 Shift_Reg.Latch();
Khayhen 0:e052961f153d 585 break;
Khayhen 0:e052961f153d 586 }
Khayhen 0:e052961f153d 587 }
Khayhen 0:e052961f153d 588
Khayhen 0:e052961f153d 589 void Motor2_Stepping(int steps, bool dir, int count)
Khayhen 0:e052961f153d 590 {
Khayhen 0:e052961f153d 591 for(int i = 0; i < steps; i++)
Khayhen 0:e052961f153d 592 {
Khayhen 0:e052961f153d 593 Phases2(count);
Khayhen 0:e052961f153d 594 if(dir)
Khayhen 0:e052961f153d 595 {
Khayhen 0:e052961f153d 596 if(count < 8) count++;
Khayhen 0:e052961f153d 597 else count = 1;
Khayhen 0:e052961f153d 598 wait(0.001);
Khayhen 0:e052961f153d 599 }
Khayhen 0:e052961f153d 600
Khayhen 0:e052961f153d 601 else
Khayhen 0:e052961f153d 602 {
Khayhen 0:e052961f153d 603 if(count > 1) count--;
Khayhen 0:e052961f153d 604 else count = 8;
Khayhen 0:e052961f153d 605 wait(0.001);
Khayhen 0:e052961f153d 606 }
Khayhen 0:e052961f153d 607 }
Khayhen 0:e052961f153d 608 }
Khayhen 0:e052961f153d 609
Khayhen 0:e052961f153d 610 void intStringSplit(string in_string, int* int_Strings, float* float_Strings)
Khayhen 0:e052961f153d 611 {
Khayhen 0:e052961f153d 612 char in[128];
Khayhen 0:e052961f153d 613 strcpy(in, in_string.c_str());
Khayhen 0:e052961f153d 614 int y = 0;
Khayhen 0:e052961f153d 615
Khayhen 0:e052961f153d 616 for(int i = 0; i < 6; i++)
Khayhen 0:e052961f153d 617 {
Khayhen 0:e052961f153d 618 char number[10] = " ";
Khayhen 0:e052961f153d 619 for(int x = 0; x < 11; x++)
Khayhen 0:e052961f153d 620 {
Khayhen 0:e052961f153d 621 if(in[y] == ',') break;
Khayhen 0:e052961f153d 622
Khayhen 0:e052961f153d 623 else
Khayhen 0:e052961f153d 624 {
Khayhen 0:e052961f153d 625 number[x] = in[y];
Khayhen 0:e052961f153d 626 y++;
Khayhen 0:e052961f153d 627 }
Khayhen 0:e052961f153d 628 }
Khayhen 0:e052961f153d 629
Khayhen 0:e052961f153d 630 if(i<4) int_Strings[i] = atoi(number);
Khayhen 0:e052961f153d 631 else float_Strings[i - 4] = atof(number);
Khayhen 0:e052961f153d 632 y++;
Khayhen 0:e052961f153d 633 }
Khayhen 0:e052961f153d 634 }
Khayhen 0:e052961f153d 635
Khayhen 0:e052961f153d 636 bool int_numcmp(int numof, int in_number, int cmp_number[], int tolerance[])
Khayhen 0:e052961f153d 637 {
Khayhen 0:e052961f153d 638 int min[numof];
Khayhen 0:e052961f153d 639 int max[numof];
Khayhen 0:e052961f153d 640 int sum = 0;
Khayhen 0:e052961f153d 641 bool answer;
Khayhen 0:e052961f153d 642
Khayhen 0:e052961f153d 643 for(int x = 1; x < numof; x++)
Khayhen 0:e052961f153d 644 {
Khayhen 0:e052961f153d 645 min[x] = cmp_number[x] - tolerance[x];
Khayhen 0:e052961f153d 646 max[x] = cmp_number[x] + tolerance[x];
Khayhen 0:e052961f153d 647
Khayhen 0:e052961f153d 648 if(in_number >= min[x] && in_number <= max[x]) sum = sum + 1;
Khayhen 0:e052961f153d 649 else sum = sum + 0;
Khayhen 0:e052961f153d 650 }
Khayhen 0:e052961f153d 651
Khayhen 0:e052961f153d 652 if(sum == 0) answer = false;
Khayhen 0:e052961f153d 653 else answer = true;
Khayhen 0:e052961f153d 654
Khayhen 0:e052961f153d 655 return answer;
Khayhen 0:e052961f153d 656 }
Khayhen 0:e052961f153d 657
Khayhen 0:e052961f153d 658 bool float_numcmp(int numof, float in_number, float cmp_number[], float tolerance[])
Khayhen 0:e052961f153d 659 {
Khayhen 0:e052961f153d 660 float min[numof];
Khayhen 0:e052961f153d 661 float max[numof];
Khayhen 0:e052961f153d 662 int sum = 0;
Khayhen 0:e052961f153d 663 bool answer;
Khayhen 0:e052961f153d 664
Khayhen 0:e052961f153d 665 for(int x = 1; x < numof; x++)
Khayhen 0:e052961f153d 666 {
Khayhen 0:e052961f153d 667 min[x] = cmp_number[x] - tolerance[x];
Khayhen 0:e052961f153d 668 max[x] = cmp_number[x] + tolerance[x];
Khayhen 0:e052961f153d 669
Khayhen 0:e052961f153d 670 if(in_number >= min[x] && in_number <= max[x]) sum = sum + 1;
Khayhen 0:e052961f153d 671 else sum = sum + 0;
Khayhen 0:e052961f153d 672 }
Khayhen 0:e052961f153d 673
Khayhen 0:e052961f153d 674 if(sum == 0) answer = false;
Khayhen 0:e052961f153d 675 else answer = true;
Khayhen 0:e052961f153d 676
Khayhen 0:e052961f153d 677 return answer;
Khayhen 0:e052961f153d 678 }
Khayhen 0:e052961f153d 679
Khayhen 0:e052961f153d 680 void SweeperArmCC()
Khayhen 0:e052961f153d 681 {
Khayhen 0:e052961f153d 682
Khayhen 0:e052961f153d 683 }
Khayhen 0:e052961f153d 684
Khayhen 0:e052961f153d 685 void SweeperArmACC()
Khayhen 0:e052961f153d 686 {
Khayhen 0:e052961f153d 687
Khayhen 0:e052961f153d 688 }
Khayhen 0:e052961f153d 689
Khayhen 0:e052961f153d 690 float AreaScan(int Reso)
Khayhen 0:e052961f153d 691 {
Khayhen 0:e052961f153d 692 float Sum = 0;
Khayhen 0:e052961f153d 693 float current_radius = 0;
Khayhen 0:e052961f153d 694 float previous_radius = 0;
Khayhen 0:e052961f153d 695 float Distance_center = 9.8;
Khayhen 0:e052961f153d 696 int numofsteps = 4096/Reso;
Khayhen 0:e052961f153d 697 previous_radius = Distance_center - avgDistance_sensor();
Khayhen 0:e052961f153d 698 for(int i = 0; i < Reso; i++)
Khayhen 0:e052961f153d 699 {
Khayhen 0:e052961f153d 700 Motor1_Stepping(numofsteps,false,Steps1);
Khayhen 0:e052961f153d 701 current_radius = Distance_center - Distance_sensor();
Khayhen 0:e052961f153d 702 Sum = Sum + Area(previous_radius,current_radius,numofsteps);
Khayhen 0:e052961f153d 703 previous_radius = current_radius;
Khayhen 0:e052961f153d 704 lcd.locate(0,3);
Khayhen 0:e052961f153d 705 lcd.printf("D:%fcm", current_radius);
Khayhen 0:e052961f153d 706 }
Khayhen 0:e052961f153d 707 Motor1_Stepping(4096,true,Steps1);
Khayhen 0:e052961f153d 708 Phases1(0);
Khayhen 0:e052961f153d 709 return Sum;
Khayhen 0:e052961f153d 710 }
Khayhen 0:e052961f153d 711
Khayhen 0:e052961f153d 712 float Distance_sensor()
Khayhen 0:e052961f153d 713 {
Khayhen 0:e052961f153d 714 float Input = IR_Sensor.read();
Khayhen 0:e052961f153d 715 float Distance = (43.048*Input*Input) - (67.794*Input) + 31.306;
Khayhen 0:e052961f153d 716 return Distance;
Khayhen 0:e052961f153d 717 }
Khayhen 0:e052961f153d 718
Khayhen 0:e052961f153d 719 float avgDistance_sensor()
Khayhen 0:e052961f153d 720 {
Khayhen 0:e052961f153d 721 float Distance = 0;
Khayhen 0:e052961f153d 722 for(int x = 0; x < 3; x++)
Khayhen 0:e052961f153d 723 {
Khayhen 0:e052961f153d 724 Distance = Distance + Distance_sensor();
Khayhen 0:e052961f153d 725 wait(0.005);
Khayhen 0:e052961f153d 726 }
Khayhen 0:e052961f153d 727 Distance = Distance/3;
Khayhen 0:e052961f153d 728 return Distance;
Khayhen 0:e052961f153d 729 }
Khayhen 0:e052961f153d 730
Khayhen 0:e052961f153d 731 float Area(float a, float b, int steps)
Khayhen 0:e052961f153d 732 {
Khayhen 0:e052961f153d 733 const double degrees = 360;
Khayhen 0:e052961f153d 734 const double numofsteps = 4096;
Khayhen 0:e052961f153d 735 double param = (degrees*steps/numofsteps);
Khayhen 0:e052961f153d 736 float result = sin (param*PI/180);
Khayhen 0:e052961f153d 737 float area = 0.5*(a*b*result);
Khayhen 0:e052961f153d 738
Khayhen 0:e052961f153d 739 return area;
Khayhen 0:e052961f153d 740 }
Khayhen 0:e052961f153d 741
Khayhen 0:e052961f153d 742 void Operation_mode()
Khayhen 0:e052961f153d 743 {
Khayhen 0:e052961f153d 744 while(true)
Khayhen 0:e052961f153d 745 {
Khayhen 0:e052961f153d 746 lcd.cls();
Khayhen 0:e052961f153d 747 lcd.locate(4,1);
Khayhen 0:e052961f153d 748 lcd.printf("Operation");
Khayhen 0:e052961f153d 749 lcd.locate(11,2);
Khayhen 0:e052961f153d 750 lcd.printf("Mode");
Khayhen 0:e052961f153d 751 char OperationCommand[128];
Khayhen 0:e052961f153d 752 pc.scanf("%s", OperationCommand);
Khayhen 0:e052961f153d 753
Khayhen 0:e052961f153d 754 if(strcmp(OperationCommand,"Start") == 0)
Khayhen 0:e052961f153d 755 {
Khayhen 0:e052961f153d 756 lcd.cls();
Khayhen 0:e052961f153d 757 lcd.locate(0,0);
Khayhen 0:e052961f153d 758 lcd.printf("Start Sorting \n");
Khayhen 0:e052961f153d 759 pc.printf("Begin");
Khayhen 0:e052961f153d 760
Khayhen 0:e052961f153d 761 pc.scanf("%s", Command);
Khayhen 0:e052961f153d 762 int NumberofSettings = atoi(Command);
Khayhen 0:e052961f153d 763 lcd.locate(0,1);
Khayhen 0:e052961f153d 764 lcd.printf("%d Settings", NumberofSettings);
Khayhen 0:e052961f153d 765 pc.printf("OK");
Khayhen 0:e052961f153d 766 H_Settings.clear();
Khayhen 0:e052961f153d 767 for(int x = 0; x < (NumberofSettings + 1); x++)
Khayhen 0:e052961f153d 768 {
Khayhen 0:e052961f153d 769 pc.scanf("%s",Command);
Khayhen 0:e052961f153d 770 H_Settings.push_back(Command);
Khayhen 0:e052961f153d 771 pc.printf("OK");
Khayhen 0:e052961f153d 772 lcd.locate(0,3);
Khayhen 0:e052961f153d 773 lcd.printf("%s", Command);
Khayhen 0:e052961f153d 774 }
Khayhen 0:e052961f153d 775
Khayhen 0:e052961f153d 776 BeginSorting();
Khayhen 0:e052961f153d 777
Khayhen 0:e052961f153d 778 }
Khayhen 0:e052961f153d 779
Khayhen 0:e052961f153d 780 else if(strcmp(OperationCommand,"ColourScan") == 0)
Khayhen 0:e052961f153d 781 {
Khayhen 0:e052961f153d 782 lcd.cls();
Khayhen 0:e052961f153d 783 lcd.locate(0,0);
Khayhen 0:e052961f153d 784 lcd.printf("Scanning Colour");
Khayhen 0:e052961f153d 785 Colour_sensor.getAllColors( rgb_readings );
Khayhen 0:e052961f153d 786 pc.printf("%d,%d,%d", rgb_readings[0],rgb_readings[1],rgb_readings[2]);
Khayhen 0:e052961f153d 787 lcd.locate(0,1);
Khayhen 0:e052961f153d 788 lcd.printf("Red : %d", rgb_readings[0]);
Khayhen 0:e052961f153d 789 lcd.locate(0,2);
Khayhen 0:e052961f153d 790 lcd.printf("Green : %d", rgb_readings[1]);
Khayhen 0:e052961f153d 791 lcd.locate(0,3);
Khayhen 0:e052961f153d 792 lcd.printf("Blue : %d", rgb_readings[2]);
Khayhen 0:e052961f153d 793 wait(2);
Khayhen 0:e052961f153d 794 }
Khayhen 0:e052961f153d 795
Khayhen 0:e052961f153d 796 else if(strcmp(OperationCommand,"AreaScan") == 0)
Khayhen 0:e052961f153d 797 {
Khayhen 0:e052961f153d 798 lcd.cls();
Khayhen 0:e052961f153d 799 pc.scanf("%s", Command);
Khayhen 0:e052961f153d 800 int reso = atoi(Command);
Khayhen 0:e052961f153d 801 lcd.locate(0,0);
Khayhen 0:e052961f153d 802 lcd.printf("Scanning Area....");
Khayhen 0:e052961f153d 803 lcd.locate(0,1);
Khayhen 0:e052961f153d 804 lcd.printf("Res: %d", reso);
Khayhen 0:e052961f153d 805 float area = AreaScan(reso);
Khayhen 0:e052961f153d 806 lcd.locate(0,3);
Khayhen 0:e052961f153d 807 lcd.printf(" ");
Khayhen 0:e052961f153d 808 lcd.locate(0,2);
Khayhen 0:e052961f153d 809 //pc.printf("OK");
Khayhen 0:e052961f153d 810 wait(1.6);
Khayhen 0:e052961f153d 811 pc.printf("%f", area);
Khayhen 0:e052961f153d 812 lcd.printf("Area: %fcm^2", area);
Khayhen 0:e052961f153d 813 wait(2);
Khayhen 0:e052961f153d 814 }
Khayhen 0:e052961f153d 815
Khayhen 0:e052961f153d 816 else if(strcmp(OperationCommand,"Return") == 0) break;
Khayhen 0:e052961f153d 817
Khayhen 0:e052961f153d 818 else
Khayhen 0:e052961f153d 819 {
Khayhen 0:e052961f153d 820
Khayhen 0:e052961f153d 821 }
Khayhen 0:e052961f153d 822
Khayhen 0:e052961f153d 823 }
Khayhen 0:e052961f153d 824 }
Khayhen 0:e052961f153d 825
Khayhen 0:e052961f153d 826 void Maintenance_mode()
Khayhen 0:e052961f153d 827 {
Khayhen 0:e052961f153d 828 while(true)
Khayhen 0:e052961f153d 829 {
Khayhen 0:e052961f153d 830 lcd.cls();
Khayhen 0:e052961f153d 831 lcd.locate(0,0);
Khayhen 0:e052961f153d 832 lcd.printf("Maintenance Mode");
Khayhen 0:e052961f153d 833 char MaintenanceCommand[128];
Khayhen 0:e052961f153d 834 pc.scanf("%s", MaintenanceCommand);
Khayhen 0:e052961f153d 835
Khayhen 0:e052961f153d 836 if(strcmp(MaintenanceCommand,"ColourSensor") == 0)
Khayhen 0:e052961f153d 837 {
Khayhen 0:e052961f153d 838 lcd.locate(0,0);
Khayhen 0:e052961f153d 839 lcd.printf("Scanning Colour");
Khayhen 0:e052961f153d 840 Colour_sensor.getAllColors( rgb_readings );
Khayhen 0:e052961f153d 841 pc.printf("%d,%d,%d", rgb_readings[0],rgb_readings[1],rgb_readings[2]);
Khayhen 0:e052961f153d 842 lcd.locate(0,1);
Khayhen 0:e052961f153d 843 lcd.printf("Red : %d", rgb_readings[0]);
Khayhen 0:e052961f153d 844 lcd.locate(0,2);
Khayhen 0:e052961f153d 845 lcd.printf("Red : %d", rgb_readings[1]);
Khayhen 0:e052961f153d 846 lcd.locate(0,3);
Khayhen 0:e052961f153d 847 lcd.printf("Red : %d", rgb_readings[2]);
Khayhen 0:e052961f153d 848 wait(1);
Khayhen 0:e052961f153d 849 }
Khayhen 0:e052961f153d 850
Khayhen 0:e052961f153d 851 else if(strcmp(MaintenanceCommand,"SweeperArm") == 0)
Khayhen 0:e052961f153d 852 {
Khayhen 0:e052961f153d 853
Khayhen 0:e052961f153d 854 }
Khayhen 0:e052961f153d 855
Khayhen 0:e052961f153d 856 else if(strcmp(MaintenanceCommand,"ArmSweepLeft") == 0)
Khayhen 0:e052961f153d 857 {
Khayhen 0:e052961f153d 858 SweeperArmCC();
Khayhen 0:e052961f153d 859 wait(1.275);
Khayhen 0:e052961f153d 860 }
Khayhen 0:e052961f153d 861
Khayhen 0:e052961f153d 862
Khayhen 0:e052961f153d 863 else if(strcmp(MaintenanceCommand,"ArmSweepRight") == 0)
Khayhen 0:e052961f153d 864 {
Khayhen 0:e052961f153d 865 SweeperArmACC();
Khayhen 0:e052961f153d 866 wait(1.275);
Khayhen 0:e052961f153d 867 }
Khayhen 0:e052961f153d 868
Khayhen 0:e052961f153d 869 else if(strcmp(MaintenanceCommand,"LP") == 0)
Khayhen 0:e052961f153d 870 {
Khayhen 0:e052961f153d 871 int stepper;
Khayhen 0:e052961f153d 872 pc.printf("OK");
Khayhen 0:e052961f153d 873 scanf("%s", MaintenanceCommand);
Khayhen 0:e052961f153d 874 stepper = atoi(MaintenanceCommand);
Khayhen 0:e052961f153d 875 Motor1_Stepping(stepper, false, Steps1);
Khayhen 0:e052961f153d 876 }
Khayhen 0:e052961f153d 877
Khayhen 0:e052961f153d 878 else if(strcmp(MaintenanceCommand,"RP") == 0)
Khayhen 0:e052961f153d 879 {
Khayhen 0:e052961f153d 880 int stepper;
Khayhen 0:e052961f153d 881 pc.printf("OK");
Khayhen 0:e052961f153d 882 scanf("%s", MaintenanceCommand);
Khayhen 0:e052961f153d 883 stepper = atoi(MaintenanceCommand);
Khayhen 0:e052961f153d 884 Motor1_Stepping(stepper, true, Steps1);
Khayhen 0:e052961f153d 885 }
Khayhen 0:e052961f153d 886
Khayhen 0:e052961f153d 887 else if(strcmp(MaintenanceCommand,"uIR") == 0)
Khayhen 0:e052961f153d 888 {
Khayhen 0:e052961f153d 889 int stepper;
Khayhen 0:e052961f153d 890 pc.printf("OK");
Khayhen 0:e052961f153d 891 scanf("%s", MaintenanceCommand);
Khayhen 0:e052961f153d 892 stepper = atoi(MaintenanceCommand);
Khayhen 0:e052961f153d 893 Motor2_Stepping(stepper, true, Steps2);
Khayhen 0:e052961f153d 894 }
Khayhen 0:e052961f153d 895
Khayhen 0:e052961f153d 896 else if(strcmp(MaintenanceCommand,"dIR") == 0)
Khayhen 0:e052961f153d 897 {
Khayhen 0:e052961f153d 898 int stepper;
Khayhen 0:e052961f153d 899 pc.printf("OK");
Khayhen 0:e052961f153d 900 scanf("%s", MaintenanceCommand);
Khayhen 0:e052961f153d 901 stepper = atoi(MaintenanceCommand);
Khayhen 0:e052961f153d 902 Motor2_Stepping(stepper, false, Steps2);
Khayhen 0:e052961f153d 903 }
Khayhen 0:e052961f153d 904
Khayhen 0:e052961f153d 905 else if(strcmp(MaintenanceCommand,"IRSensor") == 0)
Khayhen 0:e052961f153d 906 {
Khayhen 0:e052961f153d 907 float value = Distance_sensor();
Khayhen 0:e052961f153d 908 pc.printf("%fcm",value);
Khayhen 0:e052961f153d 909 }
Khayhen 0:e052961f153d 910
Khayhen 0:e052961f153d 911 else if(strcmp(MaintenanceCommand,"AreaScan") == 0)
Khayhen 0:e052961f153d 912 {
Khayhen 0:e052961f153d 913 pc.printf("OK");
Khayhen 0:e052961f153d 914 lcd.locate(0,0);
Khayhen 0:e052961f153d 915 lcd.printf("Scanning Area");
Khayhen 0:e052961f153d 916 pc.scanf("%s", Command);
Khayhen 0:e052961f153d 917 int Reso = atoi(Command);
Khayhen 0:e052961f153d 918 lcd.locate(0,1);
Khayhen 0:e052961f153d 919 lcd.printf("Resolution : %d points", Reso);
Khayhen 0:e052961f153d 920 float val = AreaScan(Reso);
Khayhen 0:e052961f153d 921 lcd.locate(0,3);
Khayhen 0:e052961f153d 922 lcd.printf(" ");
Khayhen 0:e052961f153d 923 lcd.locate(0,2);
Khayhen 0:e052961f153d 924 lcd.printf("Area: %fcm^2",val);
Khayhen 0:e052961f153d 925 pc.printf("%fcm^2",val);
Khayhen 0:e052961f153d 926 wait(1);
Khayhen 0:e052961f153d 927 }
Khayhen 0:e052961f153d 928 else if(strcmp(MaintenanceCommand,"Return") == 0) break;
Khayhen 0:e052961f153d 929
Khayhen 0:e052961f153d 930 else
Khayhen 0:e052961f153d 931 {
Khayhen 0:e052961f153d 932 //pc.printf("ERROR");
Khayhen 0:e052961f153d 933
Khayhen 0:e052961f153d 934 }
Khayhen 0:e052961f153d 935
Khayhen 0:e052961f153d 936 }
Khayhen 0:e052961f153d 937 }
Khayhen 0:e052961f153d 938
Khayhen 0:e052961f153d 939 void BeginSorting()
Khayhen 0:e052961f153d 940 {
Khayhen 0:e052961f153d 941 lcd.cls();
Khayhen 0:e052961f153d 942 int Resolution;
Khayhen 0:e052961f153d 943 int Settings_int[4];
Khayhen 0:e052961f153d 944 float Settings_float[2];
Khayhen 0:e052961f153d 945 int r[128];
Khayhen 0:e052961f153d 946 int g[128];
Khayhen 0:e052961f153d 947 int b[128];
Khayhen 0:e052961f153d 948 int Colour_tolerance[128];
Khayhen 0:e052961f153d 949 float Area[128];
Khayhen 0:e052961f153d 950 float Area_tolerance[128];
Khayhen 0:e052961f153d 951
Khayhen 0:e052961f153d 952 Resolution = atoi(H_Settings.at(0).c_str());
Khayhen 0:e052961f153d 953 for(int x = 1; x < H_Settings.size(); x++)
Khayhen 0:e052961f153d 954 {
Khayhen 0:e052961f153d 955 intStringSplit(H_Settings.at(x), Settings_int, Settings_float);
Khayhen 0:e052961f153d 956 r[x] = Settings_int[0];
Khayhen 0:e052961f153d 957 g[x] = Settings_int[1];
Khayhen 0:e052961f153d 958 b[x] = Settings_int[2];
Khayhen 0:e052961f153d 959 Colour_tolerance[x] = Settings_int[3];
Khayhen 0:e052961f153d 960 Area[x] = Settings_float[0];
Khayhen 0:e052961f153d 961 Area_tolerance[x] = Settings_float[1];
Khayhen 0:e052961f153d 962 }
Khayhen 0:e052961f153d 963 lcd.locate(0,2);
Khayhen 0:e052961f153d 964 lcd.printf("Settings Sorted");
Khayhen 0:e052961f153d 965
Khayhen 0:e052961f153d 966 bool r_check;
Khayhen 0:e052961f153d 967 bool g_check;
Khayhen 0:e052961f153d 968 bool b_check;
Khayhen 0:e052961f153d 969 bool a_check;
Khayhen 0:e052961f153d 970 bool Hazard = 0;
Khayhen 0:e052961f153d 971 Sort = false;
Khayhen 0:e052961f153d 972 Stop = false;
Khayhen 0:e052961f153d 973 while(true)
Khayhen 0:e052961f153d 974 {
Khayhen 0:e052961f153d 975 if(Sort)
Khayhen 0:e052961f153d 976 {
Khayhen 0:e052961f153d 977 wait(1);
Khayhen 0:e052961f153d 978 if(!Stop)
Khayhen 0:e052961f153d 979 {
Khayhen 0:e052961f153d 980 lcd.cls();
Khayhen 0:e052961f153d 981 Colour_sensor.getAllColors( rgb_readings );
Khayhen 0:e052961f153d 982 r_check = int_numcmp(H_Settings.size(), rgb_readings[0], r, Colour_tolerance);
Khayhen 0:e052961f153d 983 g_check = int_numcmp(H_Settings.size(), rgb_readings[1], g, Colour_tolerance);
Khayhen 0:e052961f153d 984 b_check = int_numcmp(H_Settings.size(), rgb_readings[2], b, Colour_tolerance);
Khayhen 0:e052961f153d 985
Khayhen 0:e052961f153d 986 Area_readings = AreaScan(Resolution);
Khayhen 0:e052961f153d 987 a_check = float_numcmp(H_Settings.size(), Area_readings, Area, Area_tolerance);
Khayhen 0:e052961f153d 988 lcd.locate(0,0);
Khayhen 0:e052961f153d 989 lcd.printf("Red : %d", rgb_readings[0]);
Khayhen 0:e052961f153d 990 lcd.locate(0,1);
Khayhen 0:e052961f153d 991 lcd.printf("Green : %d", rgb_readings[1]);
Khayhen 0:e052961f153d 992 lcd.locate(0,2);
Khayhen 0:e052961f153d 993 lcd.printf("Blue : %d", rgb_readings[2]);
Khayhen 0:e052961f153d 994 lcd.locate(0,3);
Khayhen 0:e052961f153d 995 lcd.printf("Area : %f", Area_readings);
Khayhen 0:e052961f153d 996 if(r_check && g_check && b_check && a_check) Hazard = 1;
Khayhen 0:e052961f153d 997 else Hazard = 0;
Khayhen 0:e052961f153d 998 pc.printf("%d,%d,%d, ,%f, , , ,%d",rgb_readings[0], rgb_readings[1], rgb_readings[2], Area_readings, Hazard);//, Colour_tolerance[x]);
Khayhen 0:e052961f153d 999 //string fej("%d,%d,%d, ,%d, , ,%d",rgb_readings[0], rgb_readings[1], rgb_readings[2], Area_readings, Hazard);
Khayhen 0:e052961f153d 1000
Khayhen 0:e052961f153d 1001 ArmIn = 1;
Khayhen 0:e052961f153d 1002 if(Hazard) Object_Hazardous();
Khayhen 0:e052961f153d 1003 else Object_NonHazardous();
Khayhen 0:e052961f153d 1004 Sort = false;
Khayhen 0:e052961f153d 1005 }
Khayhen 0:e052961f153d 1006 }
Khayhen 0:e052961f153d 1007 else
Khayhen 0:e052961f153d 1008 {
Khayhen 0:e052961f153d 1009 RGB_Red = 1.0;
Khayhen 0:e052961f153d 1010 RGB_Green = 1.0;
Khayhen 0:e052961f153d 1011 }
Khayhen 0:e052961f153d 1012
Khayhen 0:e052961f153d 1013 if(Stop)
Khayhen 0:e052961f153d 1014 {
Khayhen 0:e052961f153d 1015 RGB_Red = 1.0;
Khayhen 0:e052961f153d 1016 RGB_Green = 1.0;
Khayhen 0:e052961f153d 1017 Stop = false;
Khayhen 0:e052961f153d 1018 break;
Khayhen 0:e052961f153d 1019 }
Khayhen 0:e052961f153d 1020 }
Khayhen 0:e052961f153d 1021
Khayhen 0:e052961f153d 1022 }
Khayhen 0:e052961f153d 1023
Khayhen 0:e052961f153d 1024
Khayhen 0:e052961f153d 1025
Khayhen 0:e052961f153d 1026 void Object_Hazardous()
Khayhen 0:e052961f153d 1027 {
Khayhen 0:e052961f153d 1028 SL1 = 1;
Khayhen 0:e052961f153d 1029 RGB_Red = 0.95;
Khayhen 0:e052961f153d 1030 //RGB_Blue = 1.0;
Khayhen 0:e052961f153d 1031 RGB_Green = 1.0;
Khayhen 0:e052961f153d 1032 SweeperArmACC();
Khayhen 0:e052961f153d 1033 wait(1);
Khayhen 0:e052961f153d 1034 ArmIn = 0;
Khayhen 0:e052961f153d 1035 SL1 = 0;
Khayhen 0:e052961f153d 1036 }
Khayhen 0:e052961f153d 1037
Khayhen 0:e052961f153d 1038 void Object_NonHazardous()
Khayhen 0:e052961f153d 1039 {
Khayhen 0:e052961f153d 1040 SL2 = 1;
Khayhen 0:e052961f153d 1041 RGB_Red = 1.0;
Khayhen 0:e052961f153d 1042 //RGB_Blue = 1.0;
Khayhen 0:e052961f153d 1043 RGB_Green = 0.95;
Khayhen 0:e052961f153d 1044 SweeperArmCC();
Khayhen 0:e052961f153d 1045 wait(1);
Khayhen 0:e052961f153d 1046 ArmIn = 0;
Khayhen 0:e052961f153d 1047 SL2 = 0;
Khayhen 0:e052961f153d 1048 }