Khayhen Sarveswarah
MBED file for Heriot-Watt Malaysia System Project 2016
Dependencies: PinDetect SDFileSystem ShiftReg TCS3472_I2C TextLCD mRotaryEncoder mbed
main.cpp
- Committer:
- Khayhen
- Date:
- 2017-01-16
- Revision:
- 0:e052961f153d
File content as of revision 0:e052961f153d:
#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;
}