![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
MBED file for Heriot-Watt Malaysia System Project 2016
Dependencies: PinDetect SDFileSystem ShiftReg TCS3472_I2C TextLCD mRotaryEncoder mbed
Revision 0:e052961f153d, committed 2017-01-16
- Comitter:
- Khayhen
- Date:
- Mon Jan 16 02:02:13 2017 +0000
- Commit message:
- Change LCD file
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PinDetect.lib Mon Jan 16 02:02:13 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Reiko/code/PinDetect/#d78a22c040e4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SDFileSystem.lib Mon Jan 16 02:02:13 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/neilt6/code/SDFileSystem/#e4d2567200db
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ShiftReg.lib Mon Jan 16 02:02:13 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/yoonghm/code/ShiftReg/#a0e3fd47970f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TCS3472_I2C.lib Mon Jan 16 02:02:13 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/karlmaxwell67/code/TCS3472_I2C/#6d5bb4ad7d6e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD.lib Mon Jan 16 02:02:13 2017 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/Khayhen/code/TextLCD/#6daa3ac2d48d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mRotaryEncoder.lib Mon Jan 16 02:02:13 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/charly/code/mRotaryEncoder/#2502b829d452
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Jan 16 02:02:13 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/9baf128c2fab \ No newline at end of file