|
|
|
|
|
|
|
|
int displaycounter = 0; // Store current counter value |
|
|
int displaycounter = 0; // Store current counter value |
|
|
volatile boolean TurnDetected; |
|
|
volatile boolean TurnDetected; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void printStatus(String actualStatus) { |
|
|
void printStatus(String actualStatus) { |
|
|
if (!oldStatus.equals(actualStatus)) { |
|
|
if (!oldStatus.equals(actualStatus)) { |
|
|
Serial.println(actualStatus); |
|
|
Serial.println(actualStatus); |
|
|
|
|
|
|
|
|
//*** MAIN LOOP ******** |
|
|
//*** MAIN LOOP ******** |
|
|
//********************** |
|
|
//********************** |
|
|
void loop() { |
|
|
void loop() { |
|
|
RedButton.loop(); // MUST call the loop() function first |
|
|
|
|
|
GreenButton.loop(); |
|
|
|
|
|
BlueButton.loop(); |
|
|
|
|
|
|
|
|
|
|
|
WlsDetect.loop(); |
|
|
WlsDetect.loop(); |
|
|
Wls.loop(); |
|
|
Wls.loop(); |
|
|
RotaryControler.loop(); |
|
|
|
|
|
|
|
|
|
|
|
Display.setWlsConnected(WlsDetect.isConnected()); |
|
|
Display.setWlsConnected(WlsDetect.isConnected()); |
|
|
Display.showFrame(actualStatus); |
|
|
Display.showFrame(actualStatus); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
case TOOL_CHANGE: |
|
|
case TOOL_CHANGE: |
|
|
printStatus("TOOL_CHANGE"); |
|
|
printStatus("TOOL_CHANGE"); |
|
|
|
|
|
BlueButton.loop(); |
|
|
|
|
|
RotaryControler.loop(); |
|
|
originStatus = TOOL_CHANGE; |
|
|
originStatus = TOOL_CHANGE; |
|
|
if (intermediateState == 0) { |
|
|
if (intermediateState == 0) { |
|
|
Router_Elevator.moveToUpperLimitSwitch(); |
|
|
Router_Elevator.moveToUpperLimitSwitch(); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
case CONFIGURATION: |
|
|
case CONFIGURATION: |
|
|
printStatus("CONFIGURATION"); |
|
|
printStatus("CONFIGURATION"); |
|
|
|
|
|
BlueButton.loop(); |
|
|
|
|
|
RotaryControler.loop(); |
|
|
Router_Setup.initialize(); |
|
|
Router_Setup.initialize(); |
|
|
Router_Setup.onRotaryControlerTurn(RotaryControler.getEncoderMove()); |
|
|
Router_Setup.onRotaryControlerTurn(RotaryControler.getEncoderMove()); |
|
|
if (RotaryControler.isSwitchPressed()) { |
|
|
if (RotaryControler.isSwitchPressed()) { |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
case NULLING: |
|
|
case NULLING: |
|
|
printStatus("NULLING"); |
|
|
printStatus("NULLING"); |
|
|
|
|
|
RedButton.loop(); |
|
|
if (RedButton.isPressed()) { |
|
|
if (RedButton.isPressed()) { |
|
|
Router_Elevator.setZeroPosition(); |
|
|
Router_Elevator.setZeroPosition(); |
|
|
SetActualStatus(IDLE); |
|
|
SetActualStatus(IDLE); |
|
|
|
|
|
|
|
|
break; |
|
|
break; |
|
|
|
|
|
|
|
|
case NULLING_TLS: |
|
|
case NULLING_TLS: |
|
|
|
|
|
RedButton.loop(); |
|
|
originStatus = NULLING_TLS; |
|
|
originStatus = NULLING_TLS; |
|
|
if (intermediateState == 0) { |
|
|
if (intermediateState == 0) { |
|
|
printStatus("NULLING_TLS,0"); |
|
|
printStatus("NULLING_TLS,0"); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
case IDLE: |
|
|
case IDLE: |
|
|
printStatus("IDLE"); |
|
|
printStatus("IDLE"); |
|
|
|
|
|
RedButton.loop(); |
|
|
|
|
|
GreenButton.loop(); |
|
|
|
|
|
BlueButton.loop(); |
|
|
|
|
|
RotaryControler.loop(); |
|
|
originStatus = IDLE; |
|
|
originStatus = IDLE; |
|
|
if (RotaryControler.isSwitchPressed()) { |
|
|
if (RotaryControler.isSwitchPressed()) { |
|
|
Router_Elevator.toggleMode(); |
|
|
Router_Elevator.toggleMode(); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
case DIVING: |
|
|
case DIVING: |
|
|
printStatus("DIVING"); |
|
|
printStatus("DIVING"); |
|
|
|
|
|
GreenButton.loop(); |
|
|
originStatus = DIVING; |
|
|
originStatus = DIVING; |
|
|
Router_Elevator.setMaxDiveDistance(); |
|
|
Router_Elevator.setMaxDiveDistance(); |
|
|
if (GreenButton.isPressed()) { |
|
|
if (GreenButton.isPressed()) { |
|
|
Serial.println("GB && BB pressing"); |
|
|
|
|
|
|
|
|
Serial.println("GB pressed"); |
|
|
if (!Router_Elevator.maxDiveDistanceReached()) { |
|
|
if (!Router_Elevator.maxDiveDistanceReached()) { |
|
|
Router_Elevator.incrementDiveDistance(); |
|
|
Router_Elevator.incrementDiveDistance(); |
|
|
Router_Elevator.moveToTarget(); |
|
|
Router_Elevator.moveToTarget(); |