Khayhen Sarveswarah
MBED file for Heriot-Watt Malaysia System Project 2016
Dependencies: PinDetect SDFileSystem ShiftReg TCS3472_I2C TextLCD mRotaryEncoder mbed
Diff: main.cpp
- Revision:
- 0:e052961f153d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Jan 16 02:02:13 2017 +0000
@@ -0,0 +1,1048 @@
+#include "mbed.h"
+#include "PinDetect.h"
+#include "SDFileSystem.h"
+#include "mRotaryEncoder.h"
+#include "TextLCD.h"
+#include "TCS3472_I2C.h"
+#include "ShiftReg.h"
+#include <stdlib.h>
+#include <string>
+#include <vector>
+
+#define PI 3.14159265
+
+/////////////////////////////////////////////////// Pin Decleration ///////////////////////////////////////////////////
+// IR Trigger
+AnalogIn IR_Trigger(p16);
+
+// Shift Registers (74Shift_RegN)
+ShiftReg Shift_Reg(p17, p20, p22); // Data, Store, Clock
+
+// SD_Card module
+SDFileSystem sd(p11, p12, p13, p14, "sd"); // MOSI, MISO, SCK, CS
+
+// Host PC Communication channels
+Serial pc(USBTX, USBRX); // tx, rx
+
+// Arm Signal
+PinDetect ArmOut(p27, PullDown);
+DigitalOut ArmIn(p23);
+
+// LCD Display
+I2C i2c_lcd(p9,p10); // SDA, SCL
+TextLCD_I2C lcd(&i2c_lcd, 0x4E, TextLCD::LCD20x4); // I2C bus, PCF8574 Slaveaddress, LCD Type
+
+// RGB LED
+PwmOut RGB_Red(p24);
+PwmOut RGB_Green(p25);
+//PwmOut RGB_Blue(p26);
+
+// Rotary Encoder
+mRotaryEncoder RE(p5,p6,p7,PullUp,1000); // Pin A, Pin B, SW
+
+// Speaker
+//AnalogOut Speaker(p18);
+
+// Slide LEDs
+DigitalOut SL1(p19);
+DigitalOut SL2(p28);
+
+// Colour Sensor (TCS3472)
+TCS3472_I2C Colour_sensor(p9, p10); // SDA, SCL
+
+// VL6180X SATEL
+
+// IR Sensor
+AnalogIn IR_Sensor(p18);
+
+
+//////////////////////////////////////////////// Custom LCD Characters ////////////////////////////////////////////////
+char pointer[] = {0x10,0x18,0x1c,0x1e,0x1e,0x1c,0x18,0x10}; // |>
+
+
+////////////////////////////////////////////////////// Functions //////////////////////////////////////////////////////
+void Operation_mode();
+void Maintenance_mode();
+void BeginSorting();
+void Object_Hazardous();
+void Object_NonHazardous();
+
+void sw();
+void ButtonPressed();
+void ButtonHeld();
+bool read_file_names(char *dir);
+int LCD_Select(vector<string> list);
+float AreaScan(int resolution);
+float Distance_sensor();
+float avgDistance_sensor();
+float Area(float a, float b, int steps);
+void Phases1(int phase);
+void Phases2(int phase);
+void Motor1_Stepping(int steps, bool dir, int count);
+void Motor2_Stepping(int steps, bool dir, int count);
+void intStringSplit(char in[100], int* int_Strings, float* float_Strings);
+bool int_numcmp(int numof, int in_number, int cmp_number[], int tolerance[]);
+bool float_numcmp(int numof, float in_number, float cmp_number[], float tolerance[]);
+void SweeperArmCC();
+void SweeperArmACC();
+
+////////////////////////////////////////////////////// Variables //////////////////////////////////////////////////////
+string PC_SDlist[] = {"Connect to PC",
+ "Access to SD"};
+string SD_Optionslist[] = {"Load",
+ "Rename",
+ "Remove",
+ "Back"};
+vector<string> PC_SD(PC_SDlist, PC_SDlist + 2); //Selection between PC and SD
+vector<string> Filenames; //Filenamess are stored in a vector string
+vector<string> SD_Options(SD_Optionslist, SD_Optionslist + 4); //Options to pick when SD file selected
+vector<string> SD_Content; //Store settings from SD card
+vector<string> H_Settings; //Stores Hazardous settings
+vector<string> Sorted_Data; //Stores data of sorted items
+
+int Select = 0;
+bool Sort = false;
+bool Stop = false;
+int PC_SDmode = 0;
+int Filenumber = 0;
+int SD_Optionnumber = 0;
+char Command[512];
+int Steps1 = 1;
+int Steps2 = 1;
+int rgb_readings[4];
+float Area_readings;
+
+///////////////////////////////////////////////////// Main Program /////////////////////////////////////////////////////
+int main()
+{
+
+
+ RGB_Red = 1.0;
+ //RGB_Blue = 1.0;
+ RGB_Green = 1.0;
+ lcd.setCursor(TextLCD::CurOff_BlkOff);
+ lcd.cls();
+ lcd.locate(4,1);
+ lcd.printf("MIRACLE");
+ lcd.locate(9,2);
+ lcd.printf("TECH");
+ lcd.setUDC(0,pointer);
+
+ SweeperArmCC();
+ Colour_sensor.enablePowerAndRGBC();
+ Colour_sensor.setIntegrationTime( 100 );
+
+ RE.Set(0);
+ RE.attachSW(&sw);
+ ArmOut.attach_asserted(&ButtonPressed);
+ ArmOut.attach_asserted_held(&ButtonHeld);
+ ArmOut.setSampleFrequency();
+ wait(1);
+ lcd.cls();
+
+ ////////////////////////////////SYSTEM BEGIN////////////////////////////////
+ //Selecting between PC and SD_Card mode
+ while(true)
+ {
+ lcd.cls();
+ PC_SDmode = LCD_Select(PC_SD);
+ switch(PC_SDmode)
+ {
+ //////////////////////////////////////////PC mode//////////////////////////////////////////
+ case 0:
+ {
+ lcd.locate(5,1);
+ lcd.printf("WAITING TO");
+ lcd.locate(4,2);
+ lcd.printf("CONNECT ");
+ while(true)
+ {
+ lcd.locate(12,2);
+ lcd.printf(". ");
+ wait(0.2);
+ lcd.locate(12,2);
+ lcd.printf(".. ");
+ wait(0.2);
+ lcd.locate(12,2);
+ lcd.printf("... ");
+ wait(0.2);
+ lcd.locate(12,2);
+ lcd.printf(".... ");
+ wait(0.2);
+ if(pc.readable())
+ {
+ pc.scanf("%s",Command);
+ lcd.cls();
+ lcd.locate(5,1);
+ lcd.printf("%s",Command);
+ wait(1);
+ while(true)
+ {
+ if(pc.readable())
+ {
+ lcd.cls();
+ lcd.locate(8,1);
+ lcd.printf("MODE");
+ lcd.locate(7,2);
+ lcd.printf("SELECT");
+ pc.scanf("%s", Command);
+ if(strcmp(Command,"Operation") == 0) Operation_mode();
+ else if(strcmp(Command,"Maintenance") == 0) Maintenance_mode();
+ else if(strcmp(Command,"Disconnect") == 0) break;
+ else
+ {
+ lcd.cls();
+ lcd.locate(7,1);
+ lcd.printf("ERROR!");
+ wait(2);
+ }
+ }
+ }
+ break;
+ }
+ }
+
+
+ }
+
+ //////////////////////////////////////////SD Card mode//////////////////////////////////////////
+ case 1:
+ {
+ sd.mount();
+ lcd.cls();
+ Filenames.clear();
+
+ while(true)
+ {
+ // print Filenames strings from vector using an iterator
+ Filenames.clear();
+ read_file_names("/sd");
+ Filenumber = LCD_Select(Filenames);
+ while(true)
+ {
+ if(Filenumber == 0) break;
+ SD_Optionnumber = LCD_Select(SD_Options);
+ switch(SD_Optionnumber)
+ {
+ case 0 : //Load settings
+ {
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("%s", Filenames.at(Filenumber).c_str());
+
+ string source_directory = "/sd/";
+ string file_directory = Filenames.at(Filenumber);
+ string directory = source_directory + file_directory;
+ SD_Content.clear();
+ FILE *fp = fopen(directory.c_str(), "r");
+ while(!feof(fp))
+ {
+ char word[128];
+ fgets(word,128,fp);
+ SD_Content.push_back(word);
+ }
+
+ int NumberofSettings = atoi(SD_Content.at(0).c_str());
+
+ if(NumberofSettings == (SD_Content.size() - 3))
+ {
+ lcd.locate(0,1);
+ lcd.printf("Num of Settings : %d", NumberofSettings);
+ }
+ else
+ {
+ lcd.locate(7,2);
+ lcd.printf("ERROR!");
+ wait(1);
+ break;
+ }
+
+ H_Settings.clear();
+ for(int i = 1; i < (SD_Content.size()-1); i++)
+ {
+ H_Settings.push_back(SD_Content.at(i));
+ }
+
+ lcd.locate(0,3);
+ lcd.printf("%d", H_Settings.size());
+ BeginSorting();
+
+ }
+
+ case 1 :
+ {
+ //Rename file
+ }
+
+ case 2 : //Delete File
+ {
+ sd.remove(Filenames.at(Filenumber).c_str());
+ break;
+ }
+
+ case 3 : break; //Go Back
+ }
+ break;
+ }
+ if(Filenumber == 0) break;
+ }
+ sd.unmount();
+ break;
+ }
+
+ default:
+ {
+ lcd.locate(7,1);
+ lcd.printf("ERROR!");
+ break;
+ }
+ }
+ }
+
+}
+
+void sw()
+{
+ RE.Set(0);
+ Select = 1;
+}
+void ButtonPressed() {Sort = true;}
+void ButtonHeld() { Stop = true;}
+
+bool read_file_names(char *dir)
+{
+ bool present;
+ Filenames.push_back("Back");
+ DIR *dp;
+ struct dirent *dirp;
+ dp = opendir(dir);
+ //read all directory and file names in current directory into filename vector
+ if((dirp = readdir(dp)) != NULL) present = true;
+ else present = false;
+
+ while((dirp = readdir(dp)) != NULL) {
+ Filenames.push_back(string(dirp->d_name));
+ }
+ closedir(dp);
+ return present;
+}
+
+int LCD_Select(vector<string> list)
+{
+ lcd.cls();
+ int RE_val_c = 0;
+ int RE_val_p = 1;
+ int Selected;
+ int size = list.size();
+
+ if(size > 4)
+ {
+ for(int i = 0; i < 4; i++)
+ {
+ lcd.locate(1,i);
+ lcd.printf("%s", list.at(i).c_str());
+ }
+ while(true)
+ {
+ RE_val_c = RE.Get();
+
+ if(RE_val_c != RE_val_p)
+ {
+ if(RE_val_c > size-1)
+ {
+ RE.Set(0);
+ RE_val_c = 0;
+ lcd.cls();
+ for(int i = 0; i < 4; i++)
+ {
+ lcd.locate(1,i);
+ lcd.printf("%s", list.at(i).c_str());
+ }
+ }
+ else if(RE_val_c < 0)
+ {
+ RE.Set(size-1);
+ RE_val_c = size-1;
+ lcd.cls();
+ lcd.locate(1,0);
+ lcd.printf("%s", list.at(RE_val_c - 3).c_str());
+ lcd.locate(1,1);
+ lcd.printf("%s", list.at(RE_val_c - 2).c_str());
+ lcd.locate(1,2);
+ lcd.printf("%s", list.at(RE_val_c - 1).c_str());
+ lcd.locate(1,3);
+ lcd.printf("%s", list.at(RE_val_c - 0).c_str());
+ }
+ if(RE_val_c > 2)
+ {
+ lcd.cls();
+ lcd.locate(1,0);
+ lcd.printf("%s", list.at(RE_val_c - 3).c_str());
+ lcd.locate(1,1);
+ lcd.printf("%s", list.at(RE_val_c - 2).c_str());
+ lcd.locate(1,2);
+ lcd.printf("%s", list.at(RE_val_c - 1).c_str());
+ lcd.locate(1,3);
+ lcd.printf("%s", list.at(RE_val_c - 0).c_str());
+ }
+
+
+
+ for(int i = 0; i < 4; i++)
+ {
+ lcd.locate(0,i);
+ lcd.printf(" ");
+ }
+ lcd.locate(0,RE_val_c);
+ lcd.putc(0);
+ RE_val_p = RE_val_c;
+ }
+ lcd.locate(19,0);
+ lcd.printf("%d", RE_val_c);
+ if(Select)
+ {
+ Selected = RE_val_c;
+ Select = 0;
+ lcd.cls();
+ return Selected;
+ }
+ }
+ }
+
+ else
+ {
+ for(int i = 0; i < size; i++)
+ {
+ lcd.locate(1,i);
+ lcd.printf("%s", list.at(i).c_str());
+ }
+ while(true)
+ {
+ RE_val_c = RE.Get();
+
+
+ if(RE_val_c != RE_val_p)
+ {
+ //if(RE_val_c > 3)
+// {
+// lcd.cls();
+// lcd.locate(1,0);
+// lcd.printf("%s", list.at(RE_val_c - 3).c_str());
+// lcd.locate(1,1);
+// lcd.printf("%s", list.at(RE_val_c - 2).c_str());
+// lcd.locate(1,2);
+// lcd.printf("%s", list.at(RE_val_c - 1).c_str());
+// lcd.locate(1,3);
+// lcd.printf("%s", list.at(RE_val_c - 0).c_str());
+// }
+
+
+ if(RE_val_c > size-1) {RE.Set(0); RE_val_c = 0;}
+ else if(RE_val_c < 0) {RE.Set(size-1); RE_val_c = size-1;}
+ for(int i = 0; i < 4; i++)
+ {
+ lcd.locate(0,i);
+ lcd.printf(" ");
+ }
+ lcd.locate(0,RE_val_c);
+ lcd.putc(0);
+ RE_val_p = RE_val_c;
+ }
+ lcd.locate(19,0);
+ lcd.printf("%d", RE_val_c);
+ if(Select)
+ {
+ Selected = RE_val_c;
+ Select = 0;
+ lcd.cls();
+ return Selected;
+ }
+ }
+ }
+
+
+}
+
+void Phases1(int phase)
+{
+ switch(phase)
+ {
+ case 1 :
+ Shift_Reg.ShiftByte(0x80, ShiftReg::MSBFirst); Shift_Reg.Latch();
+ break;
+
+ case 2 :
+ Shift_Reg.ShiftByte(0xC0, ShiftReg::MSBFirst); Shift_Reg.Latch();
+ break;
+
+ case 3 :
+ Shift_Reg.ShiftByte(0x40, ShiftReg::MSBFirst); Shift_Reg.Latch();
+ break;
+
+ case 4 :
+ Shift_Reg.ShiftByte(0x60, ShiftReg::MSBFirst); Shift_Reg.Latch();
+ break;
+
+ case 5 :
+ Shift_Reg.ShiftByte(0x20, ShiftReg::MSBFirst); Shift_Reg.Latch();
+ break;
+
+ case 6 :
+ Shift_Reg.ShiftByte(0x30, ShiftReg::MSBFirst); Shift_Reg.Latch();
+ break;
+
+ case 7 :
+ Shift_Reg.ShiftByte(0x10, ShiftReg::MSBFirst); Shift_Reg.Latch();
+ break;
+
+ case 8 :
+ Shift_Reg.ShiftByte(0x90, ShiftReg::MSBFirst); Shift_Reg.Latch();
+ break;
+
+ default :
+ Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst); Shift_Reg.Latch();
+ break;
+ }
+}
+
+void Motor1_Stepping(int steps, bool dir, int count)
+{
+ for(int i = 0; i < steps; i++)
+ {
+ Phases1(count);
+ if(dir)
+ {
+ if(count < 8) count++;
+ else count = 1;
+ wait(0.001);
+ }
+
+ else
+ {
+ if(count > 1) count--;
+ else count = 8;
+ wait(0.001);
+ }
+ }
+}
+
+void Phases2(int phase)
+{
+ switch(phase)
+ {
+ case 1 :
+ //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
+ Shift_Reg.ShiftByte(0x08, ShiftReg::MSBFirst);
+ Shift_Reg.Latch();
+ break;
+
+ case 2 :
+ //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
+ Shift_Reg.ShiftByte(0x0C, ShiftReg::MSBFirst);
+ Shift_Reg.Latch();
+ break;
+
+ case 3 :
+ //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
+ Shift_Reg.ShiftByte(0x04, ShiftReg::MSBFirst);
+ Shift_Reg.Latch();
+ break;
+
+ case 4 :
+ //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
+ Shift_Reg.ShiftByte(0x06, ShiftReg::MSBFirst);
+ Shift_Reg.Latch();
+ break;
+
+ case 5 :
+ //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
+ Shift_Reg.ShiftByte(0x02, ShiftReg::MSBFirst);
+ Shift_Reg.Latch();
+ break;
+
+ case 6 :
+ //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
+ Shift_Reg.ShiftByte(0x03, ShiftReg::MSBFirst);
+ Shift_Reg.Latch();
+ break;
+
+ case 7 :
+ //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
+ Shift_Reg.ShiftByte(0x01, ShiftReg::MSBFirst);
+ Shift_Reg.Latch();
+ break;
+
+ case 8 :
+ //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
+ Shift_Reg.ShiftByte(0x09, ShiftReg::MSBFirst);
+ Shift_Reg.Latch();
+ break;
+
+ default :
+ //Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
+ Shift_Reg.ShiftByte(0x00, ShiftReg::MSBFirst);
+ Shift_Reg.Latch();
+ break;
+ }
+}
+
+void Motor2_Stepping(int steps, bool dir, int count)
+{
+ for(int i = 0; i < steps; i++)
+ {
+ Phases2(count);
+ if(dir)
+ {
+ if(count < 8) count++;
+ else count = 1;
+ wait(0.001);
+ }
+
+ else
+ {
+ if(count > 1) count--;
+ else count = 8;
+ wait(0.001);
+ }
+ }
+}
+
+void intStringSplit(string in_string, int* int_Strings, float* float_Strings)
+{
+ char in[128];
+ strcpy(in, in_string.c_str());
+ int y = 0;
+
+ for(int i = 0; i < 6; i++)
+ {
+ char number[10] = " ";
+ for(int x = 0; x < 11; x++)
+ {
+ if(in[y] == ',') break;
+
+ else
+ {
+ number[x] = in[y];
+ y++;
+ }
+ }
+
+ if(i<4) int_Strings[i] = atoi(number);
+ else float_Strings[i - 4] = atof(number);
+ y++;
+ }
+}
+
+bool int_numcmp(int numof, int in_number, int cmp_number[], int tolerance[])
+{
+ int min[numof];
+ int max[numof];
+ int sum = 0;
+ bool answer;
+
+ for(int x = 1; x < numof; x++)
+ {
+ min[x] = cmp_number[x] - tolerance[x];
+ max[x] = cmp_number[x] + tolerance[x];
+
+ if(in_number >= min[x] && in_number <= max[x]) sum = sum + 1;
+ else sum = sum + 0;
+ }
+
+ if(sum == 0) answer = false;
+ else answer = true;
+
+ return answer;
+}
+
+bool float_numcmp(int numof, float in_number, float cmp_number[], float tolerance[])
+{
+ float min[numof];
+ float max[numof];
+ int sum = 0;
+ bool answer;
+
+ for(int x = 1; x < numof; x++)
+ {
+ min[x] = cmp_number[x] - tolerance[x];
+ max[x] = cmp_number[x] + tolerance[x];
+
+ if(in_number >= min[x] && in_number <= max[x]) sum = sum + 1;
+ else sum = sum + 0;
+ }
+
+ if(sum == 0) answer = false;
+ else answer = true;
+
+ return answer;
+}
+
+void SweeperArmCC()
+{
+
+}
+
+void SweeperArmACC()
+{
+
+}
+
+float AreaScan(int Reso)
+{
+ float Sum = 0;
+ float current_radius = 0;
+ float previous_radius = 0;
+ float Distance_center = 9.8;
+ int numofsteps = 4096/Reso;
+ previous_radius = Distance_center - avgDistance_sensor();
+ for(int i = 0; i < Reso; i++)
+ {
+ Motor1_Stepping(numofsteps,false,Steps1);
+ current_radius = Distance_center - Distance_sensor();
+ Sum = Sum + Area(previous_radius,current_radius,numofsteps);
+ previous_radius = current_radius;
+ lcd.locate(0,3);
+ lcd.printf("D:%fcm", current_radius);
+ }
+ Motor1_Stepping(4096,true,Steps1);
+ Phases1(0);
+ return Sum;
+}
+
+float Distance_sensor()
+{
+ float Input = IR_Sensor.read();
+ float Distance = (43.048*Input*Input) - (67.794*Input) + 31.306;
+ return Distance;
+}
+
+float avgDistance_sensor()
+{
+ float Distance = 0;
+ for(int x = 0; x < 3; x++)
+ {
+ Distance = Distance + Distance_sensor();
+ wait(0.005);
+ }
+ Distance = Distance/3;
+ return Distance;
+}
+
+float Area(float a, float b, int steps)
+{
+ const double degrees = 360;
+ const double numofsteps = 4096;
+ double param = (degrees*steps/numofsteps);
+ float result = sin (param*PI/180);
+ float area = 0.5*(a*b*result);
+
+ return area;
+}
+
+void Operation_mode()
+{
+ while(true)
+ {
+ lcd.cls();
+ lcd.locate(4,1);
+ lcd.printf("Operation");
+ lcd.locate(11,2);
+ lcd.printf("Mode");
+ char OperationCommand[128];
+ pc.scanf("%s", OperationCommand);
+
+ if(strcmp(OperationCommand,"Start") == 0)
+ {
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Start Sorting \n");
+ pc.printf("Begin");
+
+ pc.scanf("%s", Command);
+ int NumberofSettings = atoi(Command);
+ lcd.locate(0,1);
+ lcd.printf("%d Settings", NumberofSettings);
+ pc.printf("OK");
+ H_Settings.clear();
+ for(int x = 0; x < (NumberofSettings + 1); x++)
+ {
+ pc.scanf("%s",Command);
+ H_Settings.push_back(Command);
+ pc.printf("OK");
+ lcd.locate(0,3);
+ lcd.printf("%s", Command);
+ }
+
+ BeginSorting();
+
+ }
+
+ else if(strcmp(OperationCommand,"ColourScan") == 0)
+ {
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Scanning Colour");
+ Colour_sensor.getAllColors( rgb_readings );
+ pc.printf("%d,%d,%d", rgb_readings[0],rgb_readings[1],rgb_readings[2]);
+ lcd.locate(0,1);
+ lcd.printf("Red : %d", rgb_readings[0]);
+ lcd.locate(0,2);
+ lcd.printf("Green : %d", rgb_readings[1]);
+ lcd.locate(0,3);
+ lcd.printf("Blue : %d", rgb_readings[2]);
+ wait(2);
+ }
+
+ else if(strcmp(OperationCommand,"AreaScan") == 0)
+ {
+ lcd.cls();
+ pc.scanf("%s", Command);
+ int reso = atoi(Command);
+ lcd.locate(0,0);
+ lcd.printf("Scanning Area....");
+ lcd.locate(0,1);
+ lcd.printf("Res: %d", reso);
+ float area = AreaScan(reso);
+ lcd.locate(0,3);
+ lcd.printf(" ");
+ lcd.locate(0,2);
+ //pc.printf("OK");
+ wait(1.6);
+ pc.printf("%f", area);
+ lcd.printf("Area: %fcm^2", area);
+ wait(2);
+ }
+
+ else if(strcmp(OperationCommand,"Return") == 0) break;
+
+ else
+ {
+
+ }
+
+ }
+}
+
+void Maintenance_mode()
+{
+ while(true)
+ {
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Maintenance Mode");
+ char MaintenanceCommand[128];
+ pc.scanf("%s", MaintenanceCommand);
+
+ if(strcmp(MaintenanceCommand,"ColourSensor") == 0)
+ {
+ lcd.locate(0,0);
+ lcd.printf("Scanning Colour");
+ Colour_sensor.getAllColors( rgb_readings );
+ pc.printf("%d,%d,%d", rgb_readings[0],rgb_readings[1],rgb_readings[2]);
+ lcd.locate(0,1);
+ lcd.printf("Red : %d", rgb_readings[0]);
+ lcd.locate(0,2);
+ lcd.printf("Red : %d", rgb_readings[1]);
+ lcd.locate(0,3);
+ lcd.printf("Red : %d", rgb_readings[2]);
+ wait(1);
+ }
+
+ else if(strcmp(MaintenanceCommand,"SweeperArm") == 0)
+ {
+
+ }
+
+ else if(strcmp(MaintenanceCommand,"ArmSweepLeft") == 0)
+ {
+ SweeperArmCC();
+ wait(1.275);
+ }
+
+
+ else if(strcmp(MaintenanceCommand,"ArmSweepRight") == 0)
+ {
+ SweeperArmACC();
+ wait(1.275);
+ }
+
+ else if(strcmp(MaintenanceCommand,"LP") == 0)
+ {
+ int stepper;
+ pc.printf("OK");
+ scanf("%s", MaintenanceCommand);
+ stepper = atoi(MaintenanceCommand);
+ Motor1_Stepping(stepper, false, Steps1);
+ }
+
+ else if(strcmp(MaintenanceCommand,"RP") == 0)
+ {
+ int stepper;
+ pc.printf("OK");
+ scanf("%s", MaintenanceCommand);
+ stepper = atoi(MaintenanceCommand);
+ Motor1_Stepping(stepper, true, Steps1);
+ }
+
+ else if(strcmp(MaintenanceCommand,"uIR") == 0)
+ {
+ int stepper;
+ pc.printf("OK");
+ scanf("%s", MaintenanceCommand);
+ stepper = atoi(MaintenanceCommand);
+ Motor2_Stepping(stepper, true, Steps2);
+ }
+
+ else if(strcmp(MaintenanceCommand,"dIR") == 0)
+ {
+ int stepper;
+ pc.printf("OK");
+ scanf("%s", MaintenanceCommand);
+ stepper = atoi(MaintenanceCommand);
+ Motor2_Stepping(stepper, false, Steps2);
+ }
+
+ else if(strcmp(MaintenanceCommand,"IRSensor") == 0)
+ {
+ float value = Distance_sensor();
+ pc.printf("%fcm",value);
+ }
+
+ else if(strcmp(MaintenanceCommand,"AreaScan") == 0)
+ {
+ pc.printf("OK");
+ lcd.locate(0,0);
+ lcd.printf("Scanning Area");
+ pc.scanf("%s", Command);
+ int Reso = atoi(Command);
+ lcd.locate(0,1);
+ lcd.printf("Resolution : %d points", Reso);
+ float val = AreaScan(Reso);
+ lcd.locate(0,3);
+ lcd.printf(" ");
+ lcd.locate(0,2);
+ lcd.printf("Area: %fcm^2",val);
+ pc.printf("%fcm^2",val);
+ wait(1);
+ }
+ else if(strcmp(MaintenanceCommand,"Return") == 0) break;
+
+ else
+ {
+ //pc.printf("ERROR");
+
+ }
+
+ }
+}
+
+void BeginSorting()
+{
+ lcd.cls();
+ int Resolution;
+ int Settings_int[4];
+ float Settings_float[2];
+ int r[128];
+ int g[128];
+ int b[128];
+ int Colour_tolerance[128];
+ float Area[128];
+ float Area_tolerance[128];
+
+ Resolution = atoi(H_Settings.at(0).c_str());
+ for(int x = 1; x < H_Settings.size(); x++)
+ {
+ intStringSplit(H_Settings.at(x), Settings_int, Settings_float);
+ r[x] = Settings_int[0];
+ g[x] = Settings_int[1];
+ b[x] = Settings_int[2];
+ Colour_tolerance[x] = Settings_int[3];
+ Area[x] = Settings_float[0];
+ Area_tolerance[x] = Settings_float[1];
+ }
+ lcd.locate(0,2);
+ lcd.printf("Settings Sorted");
+
+ bool r_check;
+ bool g_check;
+ bool b_check;
+ bool a_check;
+ bool Hazard = 0;
+ Sort = false;
+ Stop = false;
+ while(true)
+ {
+ if(Sort)
+ {
+ wait(1);
+ if(!Stop)
+ {
+ lcd.cls();
+ Colour_sensor.getAllColors( rgb_readings );
+ r_check = int_numcmp(H_Settings.size(), rgb_readings[0], r, Colour_tolerance);
+ g_check = int_numcmp(H_Settings.size(), rgb_readings[1], g, Colour_tolerance);
+ b_check = int_numcmp(H_Settings.size(), rgb_readings[2], b, Colour_tolerance);
+
+ Area_readings = AreaScan(Resolution);
+ a_check = float_numcmp(H_Settings.size(), Area_readings, Area, Area_tolerance);
+ lcd.locate(0,0);
+ lcd.printf("Red : %d", rgb_readings[0]);
+ lcd.locate(0,1);
+ lcd.printf("Green : %d", rgb_readings[1]);
+ lcd.locate(0,2);
+ lcd.printf("Blue : %d", rgb_readings[2]);
+ lcd.locate(0,3);
+ lcd.printf("Area : %f", Area_readings);
+ if(r_check && g_check && b_check && a_check) Hazard = 1;
+ else Hazard = 0;
+ pc.printf("%d,%d,%d, ,%f, , , ,%d",rgb_readings[0], rgb_readings[1], rgb_readings[2], Area_readings, Hazard);//, Colour_tolerance[x]);
+ //string fej("%d,%d,%d, ,%d, , ,%d",rgb_readings[0], rgb_readings[1], rgb_readings[2], Area_readings, Hazard);
+
+ ArmIn = 1;
+ if(Hazard) Object_Hazardous();
+ else Object_NonHazardous();
+ Sort = false;
+ }
+ }
+ else
+ {
+ RGB_Red = 1.0;
+ RGB_Green = 1.0;
+ }
+
+ if(Stop)
+ {
+ RGB_Red = 1.0;
+ RGB_Green = 1.0;
+ Stop = false;
+ break;
+ }
+ }
+
+}
+
+
+
+void Object_Hazardous()
+{
+ SL1 = 1;
+ RGB_Red = 0.95;
+ //RGB_Blue = 1.0;
+ RGB_Green = 1.0;
+ SweeperArmACC();
+ wait(1);
+ ArmIn = 0;
+ SL1 = 0;
+}
+
+void Object_NonHazardous()
+{
+ SL2 = 1;
+ RGB_Red = 1.0;
+ //RGB_Blue = 1.0;
+ RGB_Green = 0.95;
+ SweeperArmCC();
+ wait(1);
+ ArmIn = 0;
+ SL2 = 0;
+}
\ No newline at end of file