/* ------------------------------------------------------------------------------------------- * BR89Turntable_V11 - 2022-5-27 * ------------------------------------------------------------------------------------------- * * Max. connect time is (currently) 3 min. * * * TTH = Turn table hub; BRH = BR89 hub; Remote = PUp remote. * */ #include "Lpf2Hub.h" // create hub instances Lpf2Hub Remote; Lpf2Hub BRH; Lpf2Hub TTH; /* * Running sequence: * * TT YE BL GN RD BL YE GN RD * |--------| |--------|- -|--------|----- // ------|--------|- -|--------| * 18 14 15 | | | | 10 11 * | 17 16| | | | | 13 12 * | | | | | | | | * |<- - - <<|<--- 31 --<<|<------------- // - 30 ------------<<| | * |>> - - ->|>>-------------- 32 // -------------->|>>-- 33 --->| * |<- - - - >>|<--- 35 -->>|<---- // --------- 34 ------------->>| * |<<- - - - >|<<----------- 36 -------- // ----->|<<--- 37 ---| | * | | | | | | | | * |--------| |--------|- -|--------|----- // ------|--------|- -|--------| * TT YE BL GN RD BL YE GN RD */ // Global constants ---------------------------------------------------------------------------- // Errors const uint8_t TT_WRONG_END_POSITION = 1; // TT did not reach end position const uint8_t TT_WRONG_START_POSITION = 2; // TT did not reach start position const uint8_t BR_WRONG_TT_POSITION = 1; // BR reached either end of TT // BR: Position, direction, and orientation on track: #10 - 29 const uint8_t BR_START_POSITION_To_TT_FWD = 10; // Arbitrary const for position + direction const uint8_t BR_START_POSITION_To_TT_BWD = 11; // const uint8_t BR_START_POSITION_FroTT_FWD = 12; // => same as To_TT_BWD const uint8_t BR_START_POSITION_FroTT_BWD = 13; // => same as To_TT_FWD = close loop??? const uint8_t BR_END_POSITION_To_TT_FWD = 14; // const uint8_t BR_END_POSITION_To_TT_BWD = 15; // const uint8_t BR_END_POSITION_FroTT_FWD = 16; // => position not assoc. with tile const uint8_t BR_END_POSITION_FroTT_BWD = 17; // => position not assoc. with tile const uint8_t BR_TT_POSITION_FWD = 18; // BR stopped on TT in FWD direction const uint8_t BR_TT_POSITION_BWD = 19; // BR stopped on TT in BWD direction // BR: Segment names: #30+ const uint8_t ACC_To_TT_FWD = 30; // Arbitrary const for segements const uint8_t DEC_To_TT_FWD = 31; // const uint8_t ACC_FroTT_FWD = 32; // const uint8_t DEC_FroTT_FWD = 33; // const uint8_t ACC_To_TT_BWD = 34; // const uint8_t DEC_To_TT_BWD = 35; // const uint8_t ACC_FroTT_BWD = 36; // const uint8_t DEC_FroTT_BWD = 37; // // TT positions const uint8_t TT_START_POSITION = 0; // Arbitrary const for TT position const uint8_t TT_END_POSITION = 1; // // Speed for TT and BR [% of full speed] const uint8_t BR_FULLSPEED = 30; // BR full fwd/bwd speed const uint8_t BR_LOWSPEED = 5; // BR minimun fwd/bwd speed const uint8_t BR_TT_SPEED = 10; // BR speed for moving on/off TT const uint8_t TT_TURN_SPEED = 20; // TT speed when turning table // Timing [ms] const uint16_t BR_MOVE_ON_TT_TIME = 6000; // Time for BR to move onto/off from TT const uint16_t TT_TURN_TIME = 13000; // Time to turn TT 180° + slight delay const uint16_t MINUTE = 60000; // Other const int8_t FORWARD = 1; // motor/gearing rotation mapping const int8_t BACKWARD = -1; // - " - const uint8_t UNDETERMINED = 255; // Arbitrary const for undetermined values // Global variables -------------------------------------------------------------------------- // Timing uint32_t BLEDiscoveryAutoOffTime = millis() + 3*MINUTE; // Time before BLE discovery ends // Hub related uint8_t buttonsLeft = (byte)PoweredUpRemoteHubPort::LEFT; uint8_t buttonsRight = (byte)PoweredUpRemoteHubPort::RIGHT; uint8_t portA = (byte)PoweredUpHubPort::A; uint8_t portB = (byte)PoweredUpHubPort::B; // Hub status uint8_t BR_status = UNDETERMINED; // Currently only at program startup uint8_t Old_BR_status = UNDETERMINED; uint8_t TT_status = UNDETERMINED; // - " - int8_t currentTTHSpeed = 0, updatedTTHSpeed = 0; int8_t currentBRHSpeed = 0, updatedBRHSpeed = 0; // Colors int BRHColor = LPF2_BLACK, OldBRHColor = LPF2_BLACK; int TTHColor = LPF2_BLACK, OldTTHColor = LPF2_BLACK; // Flags bool Remote_isInitialized = false; bool TTH_isInitialized = false; bool BRH_isInitialized = false; bool System_isInitialized = false; bool BRHButton = false; bool debug_colors = true; bool debug = true; // ---------------------------------------------------------------------------------------------- void setup(){ Serial.begin(115200); while (!Serial) {}; // Wait for serial port to connect. delay(1000); // ESP module needs max. 1 second to boot up. Serial.println(); Serial.println("--------------------------------"); Serial.println(" ESP32 board init done "); Serial.println(" Scanning for BLE devices "); Serial.println("--------------------------------"); Serial.println(); } // end setup ------------------------------------------------------------------------------------- // main loop ------------------------------------------------------------------------------------- void loop(){ // Automatic setup procedure, no control buttons other than available on BLE devices used static bool flag_RunOnce = true; // Default = run one full cycle. Changed when pressing // hub on/off key longer than 0.5 s in START_POSITION static bool TT_InitRun = true; // TT initial position search static bool TT_MotorInit = false; // Init TT motor only once for search unsigned long start_time; // For explicit delay timing //-- Process loop ------------------------------------------------------------------------------ ScanBLEDevices(); // Runs for BLEDiscoveryAutoOffTime. Restarts when hubs // are shut down by remote with same run time. CheckSystemStatus(); // Sets flag System_isInitialized after sign-up of // all hubs if(System_isInitialized){ // Init: Locate BR and TT start positions. Use remote to locate start position. Remote // setting is superseeded, if a green tile is found. BR needs to be in forward (<--) position // as color sensor is located in the front section, near the cylinders. TT needs to be rotated // to "green" terminal position before system is enabled. // // The following condition holds true upon startup (UNDETERMINED) and green color found at // current BR position as well as upon final approach (BR BWD) // -> loop entry point for a) BR init and b) repeated BR run/s. if((BRHColor == LPF2_GREEN) && ((BR_status == UNDETERMINED) || (BR_status == DEC_FroTT_BWD))){ BRH.setAccelerationProfile(portA, 10000); // This is required only once, when no /change/ BRH.setDecelerationProfile(portA, 10000); // is required. SetBRMotorSpeed(0); // Convenience call; also updates speed settings. BR_status = BR_START_POSITION_To_TT_FWD; if(flag_RunOnce) BRH.setLedColor(LPF2_GREEN); if(debug) Serial.println("BR: Pos.1: BR_START_POSITION_To_TT_FWD"); } // Check/init TT position - this code runs only once for TT. TT motor if(TT_InitRun == true){ if(TT_MotorInit == false){ // Begin search (TT_MotorInit -> true). TTH.setAccelerationProfile(portA, 10000); // No further change -> valid throughout program. TTH.setDecelerationProfile(portA, 10000); // This is changing for different segments. TTH.setTachoMotorSpeed(portA,5,40); // PID, speed = 5, max. power = 40. Serial.println("TT: Searching for TT start position"); TT_MotorInit = true; } if(TTHColor == LPF2_GREEN){ TTH.stopTachoMotor(portA); TTH.setBasicMotorSpeed(portA, 5); // Make sure that position is "secured" for 1 s. Delay(1000); // No PID control, just power setting 5. TTH.stopBasicMotor(portA); TT_status = TT_START_POSITION; updatedTTHSpeed = 0; // Required for synching remote settings currentTTHSpeed = 0; // - " - TT_InitRun = false; TTH.setLedColor(LPF2_GREEN); // Green: TT in correct position, BR runs once. Serial.println("TT: Pos.1: TT_START_POSITION"); } } // This code runs after BR and TT clearance. // Seg. 1: ACC_To_TT_FWD: a) Wait for hub button: Select mode then start sequence // Short (<0.5 s) = run one loop // Long (>0.5 s) = run forever // Mode change only in start position! // b) Accelerate to full fwd // if((BR_status == BR_START_POSITION_To_TT_FWD) && (TT_status == TT_START_POSITION)){ while((BRHButton == false) && (flag_RunOnce == true)){ Delay(10); // Wait for BR on/off button pressed (= true) // or continue with next run depending on flag_RunOnce } // Does not work w/o additional Delay(10)! Delay(1000); // Wait 1 s and recheck BR on/off button if(BRHButton == true){ // Button still pressed => toggle mode; if already // released, no mode change. flag_RunOnce = !flag_RunOnce; // Button still pressed -> toggle mode. if(flag_RunOnce){ BRH.setLedColor(LPF2_GREEN); // green = run once TTH.setLedColor(LPF2_GREEN); Remote.setLedColor(LPF2_GREEN); }else{ BRH.setLedColor(LPF2_BLUE); // blue = run forever TTH.setLedColor(LPF2_BLUE); Remote.setLedColor(LPF2_BLUE); } } delay(1000); // Wait 1 s, then start run sequence. // Nothing to do, -> delay(). SetBRMotorSpeed(FORWARD*BR_FULLSPEED); // Max. power 100; portA; synced w/ remote speed // setting. BR_status = ACC_To_TT_FWD; Serial.println("BR: Seg.1: ACC_To_TT_FWD"); delay(6000); // Don't use void Delay() here; simply pause (sensors). // Get off the green tile; condition is still true for // locate start position: color = green && BR_status != start position. // (This stops the motor before it even starts to move) // The two seconds also allow to securely release the hub button // after mode change as indicated by LED color change. } // Seg. 2: DEC_To_TT_FWD if((BR_status == ACC_To_TT_FWD) && (BRHColor == LPF2_GREEN)){ BRH.setDecelerationProfile(portA, 5000); SetBRMotorSpeed(FORWARD*BR_LOWSPEED); BR_status = DEC_To_TT_FWD; if(debug) Serial.println("BR: Seg.2: DEC_To_TT_FWD"); } // Position: BR_END_POSITION_To_TT_FWD if((BR_status == DEC_To_TT_FWD) && (BRHColor == LPF2_YELLOW)){ SetBRMotorSpeed(0); BR_status = BR_END_POSITION_To_TT_FWD; if(debug) Serial.println("BR: Pos.2: BR_END_POSITION_To_TT_FWD"); } // Move onto TT; FWD if(BR_status == BR_END_POSITION_To_TT_FWD){ BRH.setDecelerationProfile(portA, 10000); BRH.setTachoMotorSpeedForDegrees(portA, FORWARD*BR_TT_SPEED, 900, 100, BrakingStyle::BRAKE); if(debug) Serial.println("BR: Moving onto TT"); Delay(BR_MOVE_ON_TT_TIME); // Record color changes. if(BRHColor != LPF2_GREEN){ // BR on wrong FWD position on TT. BRError(BR_WRONG_TT_POSITION); // Shut down all hubs. }else{ BR_status = BR_TT_POSITION_FWD; if(debug) Serial.println("BR: Pos.3: BR_TT_POSITION_FWD reached by degrees"); } } // Turn TT by 180° --------------------------------------------------------------------------------- if((BR_status == BR_TT_POSITION_FWD) && (TT_status == TT_START_POSITION)){ TTH.setTachoMotorSpeedForDegrees(portA, -TT_TURN_SPEED, 3620, 100, BrakingStyle::BRAKE); if(debug) Serial.println("TT: Moving to TT_END_POSITION"); Delay(TT_TURN_TIME); // This is long enough for the sequence to complete. Don't use delay; need // to monitor sensors! if(TTHColor == LPF2_RED){ // Check color >after< turn by degrees! TT_status = TT_END_POSITION; if(debug) Serial.println("TT: TT_END_POSITION reached by degrees"); }else{ TTError(TT_WRONG_END_POSITION); // Shut down all hubs } } // Moving off from TT; FWD if((BR_status == BR_TT_POSITION_FWD) && (TT_status == TT_END_POSITION)){ BRH.setTachoMotorSpeedForDegrees(portA, FORWARD*BR_TT_SPEED, 900, 100, BrakingStyle::BRAKE); if(debug) Serial.println("BR: Moving off from TT"); delay(6000); //This needs to be long enough for degrees to complete -> use delay BR_status = BR_END_POSITION_FroTT_FWD; if(debug) Serial.println("BR: BR_END_POSITION_FroTT_FWD reached by degrees"); } // Seg. 3: ACC_FroTT_FWD if(BR_status == BR_END_POSITION_FroTT_FWD){ BRH.setAccelerationProfile(portA, 10000); SetBRMotorSpeed(FORWARD*BR_FULLSPEED); BR_status = ACC_FroTT_FWD; if(debug) Serial.println("BR: Seg. 3: ACC_FroTT_FWD"); } // Seg. 4: DEC_FroTT_FWD if((BR_status == ACC_FroTT_FWD) && (BRHColor == LPF2_YELLOW)){ BRH.setDecelerationProfile(portA, 5000); SetBRMotorSpeed(FORWARD*BR_LOWSPEED); BR_status = DEC_FroTT_FWD; if(debug) Serial.println("BR: Seg. 4: DEC_FroTT_FWD"); } // Stop at start position (after 180° turn): BR_START_POSITION_To_TT_BWD if((BR_status == DEC_FroTT_FWD) && (BRHColor == LPF2_RED)){ SetBRMotorSpeed(0); BR_status = BR_START_POSITION_To_TT_BWD; if(debug) Serial.println("BR: Pos.4: BR_START_POSITION_To_TT_BWD"); } // Seg. 5: ACC_To_TT_BWD if(BR_status == BR_START_POSITION_To_TT_BWD){ delay(1000); // Wait 1 s before starting again BRH.setAccelerationProfile(portA, 10000); SetBRMotorSpeed(BACKWARD*BR_FULLSPEED); BR_status = ACC_To_TT_BWD; if(debug) Serial.println("BR: Seg.5: ACC_To_TT_BWD"); delay(2000); // Get off from red tile -> as for green upon start. BRHColor = LPF2_BLACK; // Clear last red color reading ???????? } // Seg. 6: DEC_To_TT_BWD if((BR_status == ACC_To_TT_BWD) && (BRHColor == LPF2_RED)){ BRH.setDecelerationProfile(portA, 5000); SetBRMotorSpeed(BACKWARD*BR_LOWSPEED); BR_status = DEC_To_TT_BWD; if(debug) Serial.println("BR: Seg.6: DEC_To_TT_BWD"); } // Pos. 5: BR_END_POSITION_To_TT_BWD if((BR_status == DEC_To_TT_BWD) && (BRHColor == LPF2_BLUE)){ SetBRMotorSpeed(0); BR_status = BR_END_POSITION_To_TT_BWD; if(debug) Serial.println("BR: Pos. 5: BR_END_POSITION_To_TT_BWD"); } // Move onto TT; BWD if(BR_status == BR_END_POSITION_To_TT_BWD){ BRH.setDecelerationProfile(portA, 10000); BRH.setTachoMotorSpeedForDegrees(portA, BACKWARD*BR_TT_SPEED, 900, 100, BrakingStyle::BRAKE); Delay(BR_MOVE_ON_TT_TIME + 1000); if(BRHColor != LPF2_GREEN){ // BR on wrong FWD position on TT BRError(BR_WRONG_TT_POSITION); }else{ BR_status = BR_TT_POSITION_BWD; if(debug) Serial.println("BR: Pos. 6: BR_TT_POSITION_BWD"); } } // Turn table -180° ----------------------------------------------------------------- if((BR_status == BR_TT_POSITION_BWD) && (TT_status == TT_END_POSITION)){ TTH.setTachoMotorSpeedForDegrees(portA, TT_TURN_SPEED, 3620, 100, BrakingStyle::BRAKE); if(debug) Serial.println("TT: Moving to TT_START_POSITION"); Delay(TT_TURN_TIME); if(TTHColor == LPF2_GREEN){ // Check color >after< turn by degrees! TT_status = TT_START_POSITION; if(debug) Serial.println("TT: TT_END_POSITION reached by degrees"); }else{ TTError(TT_WRONG_START_POSITION); // Shut down all hubs } } // Move off from TT; BWD if((BR_status == BR_TT_POSITION_BWD) && (TT_status == TT_START_POSITION)){ BRH.setTachoMotorSpeedForDegrees(portA, BACKWARD*BR_TT_SPEED, 900, 100, BrakingStyle::BRAKE); if(debug) Serial.println("Moving off TT to position BR_END_POSITION_FroTT_BWD"); delay(6000); //This needs to be long enough for degrees to complete BR_status = BR_END_POSITION_FroTT_BWD; if(debug) Serial.println("BR: BR_END_POSITION_FroTT_BWD reached by degrees"); } // Seg. 7: ACC_FroTT_BWD if(BR_status == BR_END_POSITION_FroTT_BWD){ SetBRMotorSpeed(BACKWARD*BR_FULLSPEED); BR_status = ACC_FroTT_BWD; if(debug) Serial.println("BR: Seg. 7: ACC_FroTT_BWD"); delay(6000); // Pass blue and green tile } // Seg. 8: DEC_FroTT_BWD if((BR_status == ACC_FroTT_BWD) && (BRHColor == LPF2_BLUE)){ BRH.setDecelerationProfile(portA, 5000); SetBRMotorSpeed(BACKWARD*BR_LOWSPEED); BR_status = DEC_FroTT_BWD; if(debug) Serial.println("BR: Seg. 8: DEC_FroTT_BWD"); } //Stop at start terminal is found, when green is detected (same as locate start pos.) }//if system is initialized } // End main loop ------------------------------------------------------------------------------- //-- Callback functions for activated notifications ---------------------------------------------- // //-- Actions on remote control buttons notifictions (-> not on/off button) void RemoteControlButtonsCallback(void *hub, byte portNumber, DeviceType deviceType, uint8_t *pData){ Lpf2Hub *myRemoteHub = (Lpf2Hub *)hub; //if (debug) Serial.print("RCB: Sensor message callback for port: "); //if (debug) Serial.println(portNumber, DEC); if (deviceType == DeviceType::REMOTE_CONTROL_BUTTON){ ButtonState buttonState = myRemoteHub->parseRemoteButton(pData); //if (debug) Serial.print("RCB: Buttonstate: "); //if (debug) Serial.println((byte)buttonState, HEX); if (portNumber == 0){ if (buttonState == ButtonState::UP){ updatedTTHSpeed = min(100, currentTTHSpeed + 5); } else if (buttonState == ButtonState::DOWN){ updatedTTHSpeed = max(-100, currentTTHSpeed - 5); } else if (buttonState == ButtonState::STOP){ updatedTTHSpeed = 0; } if (currentTTHSpeed != updatedTTHSpeed){ if(updatedTTHSpeed == 0){ TTH.stopTachoMotor(portA); }else { //TTH.setAccelerationProfile(portA, 10000); //TTH.setDecelerationProfile(portA, 10000); TTH.setTachoMotorSpeed(portA, updatedTTHSpeed, 40); } currentTTHSpeed = updatedTTHSpeed; if(debug){ Serial.print("RCB: Current TTH motor speed: "); Serial.println(currentTTHSpeed, DEC); } } } if (portNumber == 1){ if (buttonState == ButtonState::UP){ updatedBRHSpeed = min(100, currentBRHSpeed - 5); } else if (buttonState == ButtonState::DOWN){ updatedBRHSpeed = max(-100, currentBRHSpeed + 5); } else if (buttonState == ButtonState::STOP){ updatedBRHSpeed = 0; } if (currentBRHSpeed != updatedBRHSpeed){ if(updatedBRHSpeed == 0){ BRH.stopTachoMotor(portA); } else { //BRH.setAccelerationProfile(portA, 5000); //BRH.setDecelerationProfile(portA, 5000); BRH.setTachoMotorSpeed(portA, updatedBRHSpeed, 100); } currentBRHSpeed = updatedBRHSpeed; if(debug){ Serial.print("RCB: Current BRH motor speed: "); Serial.println(currentBRHSpeed, DEC); } } } } } //-- Actions on remote on/off button notifications ----------------------------------------- void RemoteOnOffCallback(void *hub, HubPropertyReference hubProperty, uint8_t *pData){ Lpf2Hub *myHub = (Lpf2Hub *)hub; if (hubProperty == HubPropertyReference::BUTTON){ ButtonState buttonState = myHub->parseHubButton(pData); // if (debug) Serial.print("RCB: Button: "); // if (debug) Serial.println((byte)buttonState, HEX); if (buttonState == ButtonState::PRESSED){ ShutDownAllHubs(); // Restart BLE discovery BLEDiscoveryAutoOffTime = millis() + 3*MINUTE; Serial.println(); Serial.println("--------------------------------"); Serial.println(" Scanning for BLE devices "); Serial.println("--------------------------------"); Serial.println(); } } } //-- Actions on turn table hub on/off button notifications ---------------------------- void TTHButtonCallback(void *hub, HubPropertyReference hubProperty, uint8_t *pData){ Lpf2Hub *myHub = (Lpf2Hub *)hub; //static bool flag_MotorOn = false; if (hubProperty == HubPropertyReference::BUTTON){ ButtonState buttonState = myHub->parseHubButton(pData); if (debug) Serial.print("TCB: Button: "); if (debug) Serial.println((byte)buttonState, HEX); if (buttonState == ButtonState::PRESSED){ Serial.println("TCB: Turn table hub button pressed"); //if(flag_MotorOn == false){ // myHub->setBasicMotorSpeed(portA, 15); // flag_MotorOn = true; }else{ // Serial.println("TCB: Turn table hub button released"); // myHub->setBasicMotorSpeed(portA, 0); // flag_MotorOn = false; } } } //-- Actions on BR89 hub on/off button notifictions ------------------------------------- void BRHButtonCallback(void *hub, HubPropertyReference hubProperty, uint8_t *pData){ Lpf2Hub *myHub = (Lpf2Hub *)hub; static bool flag_MotorOn = false; if (hubProperty == HubPropertyReference::BUTTON){ ButtonState buttonState = myHub->parseHubButton(pData); if (buttonState == ButtonState::PRESSED){ BRHButton = true; Serial.println("BCB: BR89 hub button pressed"); }else{ BRHButton = false; } } } //-- Actions on turn table hub color sensor values (attached to port B) -------------------------- void TTHColorSensorCallback(void *hub, byte portNumber, DeviceType deviceType, uint8_t *pData){ Lpf2Hub *myHub = (Lpf2Hub *)hub; if (deviceType == DeviceType::COLOR_DISTANCE_SENSOR){ int color = myHub->parseColor(pData); // set hub LED color to detected color of sensor and set motor speed dependent on color if (color == (byte)Color::LPF2_RED){ TTHColor = LPF2_RED; if(OldTTHColor != LPF2_RED && debug_colors){ OldTTHColor = TTHColor; Serial.print("TCB: Color change: "); Serial.println("Red"); } } else if (color == (byte)Color::LPF2_YELLOW){ TTHColor = LPF2_YELLOW; if(OldTTHColor != LPF2_YELLOW && debug_colors){ OldTTHColor = TTHColor; Serial.print("TCB: Color change: "); Serial.println("Yellow"); } } else if (color == (byte)Color::LPF2_BLUE){ TTHColor = LPF2_BLUE; if(OldTTHColor != LPF2_BLUE && debug_colors){ OldTTHColor = TTHColor; Serial.print("TCB: Color change: "); Serial.println("Blue"); } } else if (color == (byte)Color::LPF2_GREEN){ TTHColor = LPF2_GREEN; if(OldTTHColor != LPF2_GREEN && debug_colors){ OldTTHColor = TTHColor; Serial.print("TCB: Color change: "); Serial.println("Green"); } }//else{ // TTHColor = LPF2_BLACK; //} } } //-- Actions on BR89 hub color sensor values (attached to port B) -------------------------- void BRHColorSensorCallback(void *hub, byte portNumber, DeviceType deviceType, uint8_t *pData) { Lpf2Hub *myHub = (Lpf2Hub *)hub; if (deviceType == DeviceType::COLOR_DISTANCE_SENSOR){ int color = myHub->parseColor(pData); if (color == (byte)Color::LPF2_RED){ BRHColor = LPF2_RED; if(OldBRHColor != LPF2_RED && debug_colors){ OldBRHColor = BRHColor; Serial.print("BCB: Color change: "); Serial.println("Red"); } } else if (color == (byte)Color::LPF2_YELLOW){ BRHColor = LPF2_YELLOW; if(OldBRHColor != LPF2_YELLOW && debug_colors){ OldBRHColor = BRHColor; Serial.print("BCB: Color change: "); Serial.println("Yellow"); } } else if (color == (byte)Color::LPF2_BLUE){ BRHColor = LPF2_BLUE; if(OldBRHColor != LPF2_BLUE && debug_colors){ OldBRHColor = BRHColor; Serial.print("BCB: Color change: "); Serial.println("Blue"); } } else if (color == (byte)Color::LPF2_GREEN){ BRHColor = LPF2_GREEN; if(OldBRHColor != LPF2_GREEN && debug_colors){ OldBRHColor = BRHColor; Serial.print("BCB: Color change: "); Serial.println("Green"); } }//else{ // BRHColor = LPF2_BLACK; //} } } //-- BLE device sign up and activation of notifications ----------------------------------------- void ScanBLEDevices(){ static bool flag_TimeOutPrinted; // Needs to be static. if (BLEDiscoveryAutoOffTime > millis()){ // Discover BLE devices. flag_TimeOutPrinted = false; FindBLEHubs(); // Find, connect, and discover BLE devices with } else { // known addresses. if (flag_TimeOutPrinted == false){ Serial.println(); Serial.println("--------------------------------"); if (!System_isInitialized){ Serial.println(" Time out - reset ESP32 board, "); Serial.println(" or press right stop button on "); Serial.println(" (connected) remote. "); } else { Serial.println(" BLE scanning stopped. "); } Serial.println("--------------------------------"); Serial.println(); flag_TimeOutPrinted = true; } } } void CheckSystemStatus(){ // Set flag System_isInitialized to true and print message if(Remote_isInitialized && TTH_isInitialized && BRH_isInitialized && !System_isInitialized){ System_isInitialized = true; Remote.setLedColor(LPF2_GREEN); BLEDiscoveryAutoOffTime = millis(); // stops calling FindBLEHubs Serial.println(); Serial.println("--------------------------------"); Serial.println(" MLP: System is initialized"); Serial.println("--------------------------------"); Serial.println(); } } void FindBLEHubs(){ uint8_t TTH_PortForDevice; uint8_t BRH_PortForDevice; //-- Remote ---------------------------------------------------------------------------- if (!Remote.isConnected() && !Remote.isConnecting() && !Remote.isScanning()){ System_isInitialized = false; Remote_isInitialized = false; // isInitialized = true = notifications on if (debug) Serial.println("BLE: Remote.init"); Remote.init("a4:34:f1:cc:1e:2a"); } if (Remote.isConnecting()) { Remote.connectHub(); if (Remote.isConnected()) { if (debug){ Serial.print("BLE: BR89 remote connected. Remote address: "); Serial.println(Remote.getHubAddress().toString().c_str()); } //char hubName[] = "Remote"; Remote.setHubName(hubName); if (debug){ Serial.print("BLE: Hub name: "); Serial.println(Remote.getHubName().c_str()); } // Activate notifications for remote control buttons. // Delay needed because otherwise the message is to fast after the connection procedure and // the message will get lost. delay(200); // -> Remote control buttons (left/right dials) // Both activations are needed to get status updates on both remote dials. Remote.activatePortDevice(buttonsLeft, RemoteControlButtonsCallback); if(debug) Serial.println("BLE: BR89 remote left buttons activated"); Remote.activatePortDevice(buttonsRight, RemoteControlButtonsCallback); if(debug) Serial.println("BLE: BR89 right buttons activated"); // -> Remote on/off button. Remote.activateHubPropertyUpdate(HubPropertyReference::BUTTON, RemoteOnOffCallback); if (debug) Serial.println("BLE: BR89 remote On/Off Button is activated"); // Remote initialization done. Remote.setLedColor(LPF2_LIGHTBLUE); Remote_isInitialized = true; if(debug) Serial.println("BLE: BR89 remote is initialized"); } else { Serial.println("BLE: BLE ERROR: Failed to connect to Remote"); //Has never occurred yet. } } //-- TTH ---------------------------------------------------------------------------- if (!TTH.isConnected() && !TTH.isConnecting() && !TTH.isScanning()){ // Initialize variables upon startup and after shutdown. System_isInitialized = false; TTH_isInitialized = false; // isInitialized = true = notifications on TTH_PortForDevice = 0xFF; // required after shutdown if (debug) Serial.println("BLE: TTH.init"); TTH.init("90:84:2b:06:7b:73"); } if (TTH.isConnecting()){ TTH.connectHub(); if (TTH.isConnected()) { if (debug){ Serial.print("BLE: Turn table hub connected. Hub address: "); Serial.println(TTH.getHubAddress().toString().c_str()); } //char hubName[] = "TTH"; TTH.setHubName(hubName); if (debug){ Serial.print("BLE: Hub name: "); Serial.println(TTH.getHubName().c_str()); } // Activate notifications for TTH: Color sensor and hub button (optional). // -> Turn table hub button. delay(200); TTH.activateHubPropertyUpdate(HubPropertyReference::BUTTON, TTHButtonCallback); if (debug) Serial.println("BLE: TTH On/Off Button is activated"); // -> Color distance sensor. uint8_t loopcount = 0; while(TTH_PortForDevice == 0xFF && loopcount < 10){ // max. 10 s delay. delay(1000); // After shutdown and restart, portForDevice is readily available(?) // but color sensor is not ready and activatePortDevice does not work. // 1 s is long enough to enter the repeating delay loop. TTH_PortForDevice = TTH.getPortForDeviceType((byte)DeviceType::COLOR_DISTANCE_SENSOR); if (debug && TTH_PortForDevice == 0xFF) Serial.println("BLE: TTH - NO COLOR SENSOR"); loopcount ++; } if (debug && loopcount == 9){ Serial.print("BLE: Color sensor activation failed"); } else { Serial.print("BLE: TTH color sensor is on port "); Serial.print(TTH_PortForDevice, HEX); if (debug) Serial.println(" and activated"); } TTH.activatePortDevice(portB, TTHColorSensorCallback); TTH.setLedColor(LPF2_LIGHTBLUE); // This is no guarantee that sensor is activated! TTH_isInitialized = true; // This is no guarantee that hub is initialized! if(debug) Serial.println("BLE: TTH is initialized"); } else { Serial.println("BLE ERROR: Failed to connect to TTH"); } } //-- BRH ----------------------------------------------------------------------------------- if (!BRH.isConnected() && !BRH.isConnecting() && !BRH.isScanning()){ // Initialize variables upon startup and after shutdown. System_isInitialized = false; BRH_isInitialized = false; BRH_PortForDevice = 0xFF; if (debug) Serial.println("BLE: BRH.init"); BRH.init("90:84:2b:09:e3:0a"); } if (BRH.isConnecting()) { BRH.connectHub(); if (BRH.isConnected()) { if (debug){ Serial.print("BLE: Connected. Hub address: "); Serial.println(BRH.getHubAddress().toString().c_str()); } //char hubName[] = "BRH"; BRH.setHubName(hubName); if (debug) Serial.print("BLE: Hub name: "); Serial.println(BRH.getHubName().c_str()); // Activate notifications for BRH: Color sensor and hub button (optional). // -> BR89 hub button. delay(200); BRH.activateHubPropertyUpdate(HubPropertyReference::BUTTON, BRHButtonCallback); if (debug) Serial.println("BLE: BRH On/Off Button is activated"); // -> Color distance sensor attached to BR89 hub. while(BRH_PortForDevice == 0xFF){ delay(1000); BRH_PortForDevice = BRH.getPortForDeviceType((byte)DeviceType::COLOR_DISTANCE_SENSOR); if (debug && BRH_PortForDevice == 0xFF) Serial.println("BLE: BRH - NO COLOR SENSOR"); } if (debug){ Serial.print("BLE: BRH color sensor is on port "); Serial.print(BRH_PortForDevice, HEX); } BRH.activatePortDevice(portB, BRHColorSensorCallback); if (debug) Serial.println(" and activated"); BRH.setLedColor(LPF2_LIGHTBLUE); // This is no guarantee that sensor is activated! BRH_isInitialized = true; // This is no guarantee that hub is initialized! if(debug) Serial.println("BLE: BRH is initialized"); } else { Serial.println("BLE ERROR: Failed to connect to BRH"); } } } // - Other -------------------------------------------------------------------------------------- void TTError(uint8_t error){ if(error == TT_WRONG_END_POSITION){ Serial.println("TT ERROR: Wrong end position!"); } if(error == TT_WRONG_START_POSITION){ Serial.println("TT ERROR: Wrong start position!"); } ShutDownAllHubs(); } void BRError(uint8_t error){ if(error == BR_WRONG_TT_POSITION){ Serial.println("BR ERROR: Wrong TT position!"); } ShutDownAllHubs(); } void ShutDownAllHubs(){ // SDA Serial.println("Shutting down all hubs."); if(TTH_isInitialized){ TTH.shutDownHub(); TTH_isInitialized = false; if (debug) Serial.println("SDA: TTH turned off"); }else{ Serial.println("SDA: TTH is not connected"); } if(BRH_isInitialized){ BRH.shutDownHub(); BRH_isInitialized = false; if (debug) Serial.println("SDA: BRH turned off"); }else{ Serial.println("SDA: BRH is not connected"); } if(Remote_isInitialized){ Remote.shutDownHub(); Remote_isInitialized = false; if (debug) Serial.println("SDA: Remote turned off"); }else{ Serial.println("RCB: Remote is not connected"); } System_isInitialized = false; } void SetBRMotorSpeed(int8_t BRMotorSpeed){ if(BRMotorSpeed == 0){ BRH.stopTachoMotor(portA); }else{ BRH.setTachoMotorSpeed(portA, BRMotorSpeed, 100); } updatedBRHSpeed = BRMotorSpeed; currentBRHSpeed = BRMotorSpeed; } void Delay(uint16_t msec){ unsigned long start_time = millis(); while (millis() < (start_time + msec)); }