![](/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
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; }