diff --git a/src/main/java/org/baxter/disco/ocr/Cli.java b/src/main/java/org/baxter/disco/ocr/Cli.java index a1b869d..bbce347 100644 --- a/src/main/java/org/baxter/disco/ocr/Cli.java +++ b/src/main/java/org/baxter/disco/ocr/Cli.java @@ -81,75 +81,80 @@ public class Cli ErrorLogging.logError("Version: " + version); ErrorLogging.logError("========================"); try{ - inputScanner = new Scanner(System.in); + //inputScanner = new Scanner(System.in); - ConfigFacade.init(); + //ConfigFacade.init(); - int userInput = 0; + //int userInput = 0; ErrorLogging.logError("Calibrating motor movement. This may take several minutes..."); - MovementFacade.calibrate(); + ErrorLogging.logError("The piston will fire momentarily when the motor calibration is complete."); + MovementFacade.pressButton(); - do - { - printMainMenu(); - userInput = (int)inputFiltering(inputScanner.nextLine()); + ErrorLogging.logError("Testing config."); + MovementFacade.goDown(); + MovementFacade.goUp(); - switch (userInput) - { - case 1: - println("Setting up cameras..."); - println("This may take a moment..."); - configureCameras(); - camerasConfigured = true; - break; - case 2: - setDUTSerials(); - break; - case 3: - setIterationCount(); - break; - case 4: - setActiveCameras(); - break; - case 5: - if(!camerasConfigured) - { - prompt("You have not configured the cameras yet! Are you sure you would like to continue? (y/N): "); - String input = inputScanner.nextLine().toLowerCase().trim(); - if( input.isBlank() || input.charAt(0) != 'y' ) break; - else - ErrorLogging.logError("DEBUG: Potential for error: Un-initialised cameras."); - } + //do + //{ + // printMainMenu(); + // userInput = (int)inputFiltering(inputScanner.nextLine()); - serialsSet = true; - for(String cameraName : OpenCVFacade.getCameraNames()) - { - if(ConfigFacade.getValue(cameraName,ConfigProperties.ACTIVE) != 0 && - ConfigFacade.getSerial(cameraName) == null ) - serialsSet = false; - } - if(!serialsSet) - { - prompt("You have not set the serial numbers for your DUTs yet! Are you sure you would like to continue? (y/N): "); - String input = inputScanner.nextLine().toLowerCase().trim(); - if( input.isBlank() || input.charAt(0) != 'y' ) break; - else - ErrorLogging.logError("DEBUG: Potential for error: Un-initialised DUT Serial numbers."); - } + // switch (userInput) + // { + // case 1: + // println("Setting up cameras..."); + // println("This may take a moment..."); + // configureCameras(); + // camerasConfigured = true; + // break; + // case 2: + // setDUTSerials(); + // break; + // case 3: + // setIterationCount(); + // break; + // case 4: + // setActiveCameras(); + // break; + // case 5: + // if(!camerasConfigured) + // { + // prompt("You have not configured the cameras yet! Are you sure you would like to continue? (y/N): "); + // String input = inputScanner.nextLine().toLowerCase().trim(); + // if( input.isBlank() || input.charAt(0) != 'y' ) break; + // else + // ErrorLogging.logError("DEBUG: Potential for error: Un-initialised cameras."); + // } - runTests(); - break; - case 6: - printHelp(); - break; - case 8: - break; - default: - //Input handling already done by inputFiltering() - } + // serialsSet = true; + // for(String cameraName : OpenCVFacade.getCameraNames()) + // { + // if(ConfigFacade.getValue(cameraName,ConfigProperties.ACTIVE) != 0 && + // ConfigFacade.getSerial(cameraName) == null ) + // serialsSet = false; + // } + // if(!serialsSet) + // { + // prompt("You have not set the serial numbers for your DUTs yet! Are you sure you would like to continue? (y/N): "); + // String input = inputScanner.nextLine().toLowerCase().trim(); + // if( input.isBlank() || input.charAt(0) != 'y' ) break; + // else + // ErrorLogging.logError("DEBUG: Potential for error: Un-initialised DUT Serial numbers."); + // } - } while (userInput != mainMenuOptionCount); + // runTests(); + // break; + // case 6: + // printHelp(); + // break; + // case 8: + // break; + // default: + // //Input handling already done by inputFiltering() + // } + + //} while (userInput != mainMenuOptionCount); } //If anything ever goes wrong, catch the error and exit diff --git a/src/main/java/org/baxter/disco/ocr/MovementFacade.java b/src/main/java/org/baxter/disco/ocr/MovementFacade.java index b889c47..6d9ac7f 100644 --- a/src/main/java/org/baxter/disco/ocr/MovementFacade.java +++ b/src/main/java/org/baxter/disco/ocr/MovementFacade.java @@ -35,19 +35,6 @@ public class MovementFacade */ private static Thread runSwitchThread; - /** - * Conversion factor from cm/s to PWM frequency. - * - * PWM to linear speed conversion: - * Motor controller should be set to 6400 pulses / revolution - * (See motor controller documentation) - * Fixture corkscrew has a lead of 3 revolutions / cm - * (Lead = thread pitch * # of thread starts) - * - * Frequency (Hz) = Speed (cm/s) * (pulses/rev) * (lead) - */ - private static final double PWM_FREQ_CONVERT = 19200; - /** * Max allowed speed by current fixture design. * Motor appears to start acting erratically over 192kHz. @@ -65,26 +52,21 @@ public class MovementFacade private static final int MIN_FREQUENCY = 10000; /** - * Distance in cm the fixture needs to travel. - * - * Distance between limit switches: ~80cm. - * Thickness of fixture arm: ~50cm. - * *** ABOVE MATH IS WRONG *** - * I don't know what I did wrong, but this value doesn't work - * properly if straight calculated. - * I will look into this more at some point. + * Fraction of the total travel time at speed to start slowing down. */ - private static final int TRAVEL_DIST = 12; + private static final double SLOW_POLL_FACTOR = 3.0 / 4.0; /** - * What percentage of the travel to slow down the motor. + * Amount to slow down the speed by. */ - private static final double STEP_1 = 2.0/3.0; + private static final int SPEED_DOWN_FACTOR = 2; /** - * What percentage of the travel to slow down the motor farther. + * Amount of distance to travel. + * Measured in... seemingly arbitrary units? Not sure on the math here. + * Set in {@link #findDistance()} */ - private static final double STEP_2 = 5.0/6.0; + private static double TRAVEL_DIST; /** * Frequency fed to the PWM pin, which the motor controller converts into movement speed. @@ -231,7 +213,6 @@ public class MovementFacade } }, "Run switch monitor."); runSwitchThread.start(); - //ErrorLogging.logError("DEBUG: Lock thread started!"); //Initialise Pi4J pi4j = Pi4J.newAutoContext(); @@ -249,6 +230,9 @@ public class MovementFacade //Initialise PWM object. pwm = pwmBuilder("pwm","PWM Pin",PWM_PIN_ADDR); pwm.on(DUTY_CYCLE, FREQUENCY); + + //Find Distance and max speeds + calibrate(); } /** @@ -351,7 +335,7 @@ public class MovementFacade public static int resetArm() { ErrorLogging.logError("DEBUG: --------------------------------------"); - int counter = 0; + int counter; ErrorLogging.logError("DEBUG: Setting minimum frequency of PWM..."); pwm.on(DUTY_CYCLE, MIN_FREQUENCY); if(upperLimit.isHigh()) @@ -372,24 +356,29 @@ public class MovementFacade motorEnable.on(); ErrorLogging.logError("DEBUG: Is the upper limit switch reached? " + upperLimit.isHigh()); - while(!upperLimit.isHigh()) + for(counter = 0; counter < Integer.MAX_VALUE; counter++) { - try{ Thread.sleep(100); } - catch (Exception e) { ErrorLogging.logError(e); } - counter++; + try{ Thread.sleep(POLL_WAIT); } catch(Exception e){ ErrorLogging.logError(e); } + if(upperLimit.isOn()) + { + try{ Thread.sleep(1); } catch(Exception e){ErrorLogging.logError(e); } + if(upperLimit.isOn()) break; + } } motorEnable.off(); - ErrorLogging.logError("DEBUG: Motor returned after " + counter + " polls."); + ErrorLogging.logError("Motor returned after " + counter + " polls."); ErrorLogging.logError("DEBUG: --------------------------------------"); return counter; } /** - * Used to set the motor's max speed. + * Used to programmatically determine the motor's max speed. */ - public static void calibrate() + private static void calibrate() { - ErrorLogging.logError("Initial Calibration reset."); + ErrorLogging.logError("Determining distance to limit switches..."); + findDistance(); + ErrorLogging.logError("Resetting arm to set speed."); resetArm(); ErrorLogging.logError("Coarse calibrating..."); FREQUENCY = calib(MIN_FREQUENCY, MAX_FREQUENCY, 10000); @@ -401,10 +390,64 @@ public class MovementFacade } /** - * Find the max speed of the fixure between two points. + * Used to programmatically find the distance between the upper and lower limit switches. + */ + private static void findDistance() + { + resetArm(); + int downTravelCounter = 0; + int upTravelCounter = 0; + pwm.on(DUTY_CYCLE, MIN_FREQUENCY); + motorDirection.low(); + motorEnable.on(); + for(downTravelCounter = 0; downTravelCounter < Integer.MAX_VALUE; downTravelCounter++) + { + try{ Thread.sleep(POLL_WAIT); } catch(Exception e){ ErrorLogging.logError(e); } + if(lowerLimit.isOn()) + { + try{ Thread.sleep(1); } catch(Exception e){ErrorLogging.logError(e); } + if(lowerLimit.isOn()) break; + } + } + motorEnable.off(); + if(lowerLimit.isOff()) ErrorLogging.logError("False positive on findDistance down!"); + + int downTravelDist = downTravelCounter * MIN_FREQUENCY; + ErrorLogging.logError("DEBUG: Down travel distance found to be: " + downTravelDist); + ErrorLogging.logError("Down travel count: " + downTravelCounter); + + motorDirection.high(); + motorEnable.on(); + for(upTravelCounter = 0; upTravelCounter < Integer.MAX_VALUE; upTravelCounter++) + { + try{ Thread.sleep(POLL_WAIT); } catch(Exception e){ ErrorLogging.logError(e); } + if(upperLimit.isOn()) + { + try{ Thread.sleep(1); } catch(Exception e){ErrorLogging.logError(e); } + if(upperLimit.isOn()) break; + } + } + motorEnable.off(); + if(upperLimit.isOff()) ErrorLogging.logError("False positive on findDistance up!"); + + int upTravelDist = upTravelCounter * MIN_FREQUENCY; + ErrorLogging.logError("DEBUG: Up travel distance found to be: " + upTravelDist); + ErrorLogging.logError("Up travel count: " + downTravelCounter); + + if(Math.abs(upTravelCounter - downTravelCounter) > 3) + { + ErrorLogging.logError("DEBUG: Values differ too far to be error. Setting to lower value."); + } + int travelCounter = Math.min(upTravelCounter, downTravelCounter); + TRAVEL_DIST = travelCounter * MIN_FREQUENCY; + } + + + /** + * Find the max frequency to feed to the motor. * - * @param start Lowest speed to check - * @param max Highest speed to check + * @param start Lowest frequency to check + * @param max Highest frequency to check * @param iterate How much to iterate by * * @return The largest safe value between start and max. @@ -424,10 +467,10 @@ public class MovementFacade ErrorLogging.logError("DEBUG: Motor Frequency: " + FREQUENCY); ErrorLogging.logError("DEBUG: Motor calibrate on."); motorEnable.on(); - for(int j = 0; j < 20; j++) + int TWO_SECONDS = 2 * TIME_CONVERSION; + for(int j = 0; j < TWO_SECONDS; j++) { - try{ Thread.sleep(100); } - catch (Exception e){ ErrorLogging.logError(e); } + try{ Thread.sleep(POLL_WAIT); } catch(Exception e){ ErrorLogging.logError(e); } if(lowerLimit.isHigh()) { ErrorLogging.logError("DEBUG: Breaking loop early!"); @@ -445,7 +488,7 @@ public class MovementFacade { ErrorLogging.logError("DEBUG: Motor moved at speed " + i + ". Checking for errors."); - if(resetArm() < 10 && i > 3.0) + if(resetArm() < 10) { ErrorLogging.logError("DEBUG: Motor failed to move! Returning " + (i - iterate)); return i - iterate; @@ -508,59 +551,59 @@ public class MovementFacade if(limitSense.isHigh()) return FinalState.SAFE; pwm.on(DUTY_CYCLE, FREQUENCY); - int TRAVEL_TIME = (int)(TRAVEL_DIST / (FREQUENCY / PWM_FREQ_CONVERT)); - int POLL_COUNT = TRAVEL_TIME * TIME_CONVERSION; - int VEL_STEP_1 = (int)(STEP_1 * POLL_COUNT); - int VEL_STEP_2 = (int)(STEP_2 * POLL_COUNT); - - ErrorLogging.logError("DEBUG: ================================="); - ErrorLogging.logError("DEBUG: Total Poll count: " + POLL_COUNT); - ErrorLogging.logError("DEBUG: Transition 1: " + VEL_STEP_1); - ErrorLogging.logError("DEBUG: Transition 2: " + VEL_STEP_2); - ErrorLogging.logError("DEBUG: Travel time: " + TRAVEL_TIME); - ErrorLogging.logError("DEBUG: Travel speed: " + (FREQUENCY/PWM_FREQ_CONVERT)); - ErrorLogging.logError("DEBUG: STEP_1: " + STEP_1); - ErrorLogging.logError("DEBUG: STEP_2: " + STEP_2); - ErrorLogging.logError("DEBUG: ================================="); + int travelTime = (int)(TRAVEL_DIST / FREQUENCY); + int totalPollCount = travelTime * TIME_CONVERSION; + int highSpeedPolls = (int)(totalPollCount * SLOW_POLL_FACTOR); + int notHighSpeedPolls = totalPollCount - highSpeedPolls; + int medSpeedPolls = (int)(notHighSpeedPolls * SLOW_POLL_FACTOR); + int lowSpeedPolls = notHighSpeedPolls - medSpeedPolls; motorEnable.on(); - for(int i = 0; i < (POLL_COUNT);i++) + for(int i = 0; i < highSpeedPolls; i++) { try{ Thread.sleep(POLL_WAIT); } catch(Exception e){ ErrorLogging.logError(e); } if(limitSense.isOn()) { - ErrorLogging.logError("DEBUG: Motor at lower limit switch early! Pre-emptively exiting loop on poll " + i); + ErrorLogging.logError("DEBUG: Motor moved too fast! Stopping motor early."); + motorEnable.off(); + output = FinalState.FAILED; break; } - else if(i >= VEL_STEP_1 && i < VEL_STEP_2) + } + + if(motorEnable.isOn()) + { + output = FinalState.UNSAFE; + pwm.on(DUTY_CYCLE, (FREQUENCY / SPEED_DOWN_FACTOR)); + for(int i = 0; i < medSpeedPolls; i++) { - if(output != FinalState.UNSAFE) + try{ Thread.sleep(POLL_WAIT); } catch(Exception e){ ErrorLogging.logError(e); } + if(limitSense.isOn()) { - output = FinalState.UNSAFE; - ErrorLogging.logError("DEBUG: Slowing down."); - pwm.on(DUTY_CYCLE, (int)(FREQUENCY / 1.25)); - } - } - else if(i >= VEL_STEP_2) - { - if(output != FinalState.SAFE) - { - ErrorLogging.logError("DEBUG: Slowing down more."); - pwm.on(DUTY_CYCLE, FREQUENCY / 2); - output = FinalState.SAFE; + ErrorLogging.logError("DEBUG: Motor only partially slowed down! Stopping motor early."); + motorEnable.off(); + break; } } } + + if(motorEnable.isOn()) + { + output = FinalState.SAFE; + pwm.on(DUTY_CYCLE, (FREQUENCY / SPEED_DOWN_FACTOR)); + for(int i = 0; i < lowSpeedPolls; i++) + { + try{ Thread.sleep(POLL_WAIT); } catch(Exception e){ ErrorLogging.logError(e); } + if(limitSense.isOn()) + { + ErrorLogging.logError("DEBUG: Motor slowed down completely, but hit limit switch early."); + motorEnable.off(); + break; + } + } + } + motorEnable.off(); - if(output == FinalState.FAILED) - ErrorLogging.logError("Motor moved too fast!"); - else if(output == FinalState.UNSAFE) - ErrorLogging.logError("DEBUG: Motor only slowed down partially."); - else if(output == FinalState.SAFE) - { - if(limitSense.isOn()) ErrorLogging.logError("DEBUG: Motor slowed down successfully."); - else ErrorLogging.logError("DEBUG: Motor did not reach limit switch successfully."); - } pwm.on(DUTY_CYCLE, FREQUENCY); return output; }