Just trying to figure out what I am missing for the external screen to work, it only works when I run the i2c example code.
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SH110X.h>
#include <SPI.h>
#define i2c_Address 0x3c
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET -1 // QT-PY / XIAO
Adafruit_SH1106G display = Adafruit_SH1106G(128, 64, &Wire, -1);
Servo myservo;
const int SERVO_PIN = 9;
const int SERVO_HOME = 35;
const int SERVO_90 = 143;
const int SHAKE_STEP = 6;
const int TOTAL_SHAKES = 30; // Number of times to attempt to jiggle the pin free
//Set up button and valve pins
const int ROLL_SOLENOID_PIN = 10;
const int SPRING_SOLENOID_PIN = 11;
const int L_BUTTON_PIN = 7;
const int R_BUTTON_PIN = 8;
const int LED_PIN = A2;
const int PHOTORESIST_PIN = A3;
const int PRESSURE_SENSOR1_PIN = A1;
const int SENSOR2_PIN = A0;
const int BACKUP_SOLENOID_PIN = 12;
int l_buttonState = LOW; // Initial state of buttons
int r_buttonState = LOW; // Initial state of buttons
int pressure_sensorState = LOW;
unsigned long lastDebounceTime = 0; // the last time the output pin was toggled
unsigned long debounceDelay = 50; // the debounce time; increase if the output flickers
const int RESISTOR_THRESHOLD = 250; // analog read threshold determined via testing
int step = 1;
void setup() {
// Start serial monitor
Serial.begin(9600);
// Attach servo obj to pin
myservo.attach(SERVO_PIN);
myservo.write(SERVO_HOME);
// Set up pin modes
pinMode(ROLL_SOLENOID_PIN, OUTPUT);
pinMode(SPRING_SOLENOID_PIN, OUTPUT);
pinMode(L_BUTTON_PIN, INPUT_PULLUP);
pinMode(R_BUTTON_PIN, INPUT_PULLUP);
pinMode(LED_PIN, OUTPUT);
pinMode(PHOTORESIST_PIN, INPUT);
pinMode(BACKUP_SOLENOID_PIN, OUTPUT);
pinMode(PRESSURE_SENSOR1_PIN, INPUT);
pinMode(SENSOR2_PIN, INPUT);
int l_buttonState = LOW; // Initial stPIN, INPUT_PULLUP);
pinMode(R_BUTTON_PIN, INPUT_PULLUP);
pinMode(LED_PIN, OUTPUT);
pinMode(PHOTORESIST_PIN, INPUT);
pinMode(BACKUP_SOLENOID_PIN, OUTPUT);
// Turn on the LED for the pin sensor
digitalWrite(LED_PIN, HIGH);
}
void errorBlink(int error_code){
// Blink various error patterns for different failures
// Pull the cylinders back
digitalWrite(SPRING_SOLENOID_PIN, LOW);
digitalWrite(ROLL_SOLENOID_PIN, LOW);
Serial.println("Entering error blink mode...");
Serial.print("Error code: ");
Serial.println(error_code);
// Blinking 3 times in quick succession with 2 sec pause is no pin
if (error_code == 1){
Serial.println("Error message: Pin not dropping into place");
while(true){
digitalWrite(LED_PIN, LOW);
delay(2000);
for (int i=0; i < 3; i++){
digitalWrite(LED_PIN, LOW);
delay(200);
digitalWrite(LED_PIN, HIGH);
delay(200);
}
}
}
// Blinking 5 times in quick succession with 2 second pause is carriage not forward error
if (error_code == 2){
Serial.println("Error message: Carriage is not fully forward after 2nd check. Look for debris in lock pin");
while(true){
// Turn off LED
digitalWrite(LED_PIN, LOW);
// Hold off for 2 seconds
delay(2000);
// Blink 5 times
for (int i=0; i < 5; i++){
digitalWrite(LED_PIN, LOW);
delay(200);
digitalWrite(LED_PIN, HIGH);
delay(200);
}
}
}
}
void servoSerial(){
Serial.print("Max:");
Serial.print(180);
Serial.print(",");
Serial.print("Min:");
Serial.print(0);
Serial.print(",");
Serial.print("Servo:");
Serial.println(myservo.read());
// This is microseconds
Serial.println(myservo.readMicroseconds());
}
bool checkPin(){
// Return True if the pin is in place (ie - the value is below threshold)
// Return False if the pin is not in place yet
// Reading is averaged over several readings defined by AVG_VALS
// Set up some variables for smoothing photoresistor readings
const int AVG_VALS = 5;
int vals[AVG_VALS] = {0, 0, 0, 0, 0};
float avg = 0;
int total = 0;
int reading = 0;
for (int i = 0; i < AVG_VALS; ++i){
reading = analogRead(PHOTORESIST_PIN);
Serial.print("Photoresistor reading:");
Serial.println(reading);
vals[i] = reading;
delay(50);
}
for (int k = 0; k < AVG_VALS; k++){
total = total + vals[k];
}
avg = total/AVG_VALS;
Serial.print("Average of photoresistor:");
Serial.println(avg);
if(avg < RESISTOR_THRESHOLD){
return true;
}else{
return false;
}
}
bool shakePin(){
// Move the turret in a small step to try to shake the roll pin into the turret
// Continue to shake until the TOTAL_SHAKES is reached or until the buttons are released
// Return true if the pin falls in place
// Return false if the buttons are released
// Error out and blink the LED if the max number of shakes is reached
//Serial.println("Shaking the pin into place...");
int shakes = 0;
bool pinOut = true;
bool button_check = checkButtons();
Serial.println("Shaking the pin into place");
while (pinOut && shakes < TOTAL_SHAKES && button_check){
myservo.write(SERVO_HOME+SHAKE_STEP);
delay(80);
myservo.write(SERVO_HOME);
pinOut = !checkPin();
shakes += 1;
button_check = checkButtons();
}
if (!button_check){
return false;
}else{
if (pinOut){
errorBlink(1);
}else{
Serial.println("Pin has dropped into place");
return true;
}
}
Serial.println("Done shaking");
}
void moveServo(int dest){
// dest is the destination value 0-180
int current = myservo.read();
while (current != dest){
if (dest > current){
myservo.write(current + 2);
Serial.println("servo below destination");
servoSerial();
}else{
myservo.write(current - 2 );
Serial.println("servo above destination");
servoSerial();
}
delay(10);
current = myservo.read();
}
}
void turnTurret(bool home){
// Turn the servo which drives the roll pin turret between vertical and horizontal
// If home=true move back to vertical
if (home){
Serial.println("Moving turret back to vertical");
moveServo(SERVO_HOME);
}
else{
Serial.println("Moving the pin from vertical to horizontal...");
moveServo(SERVO_90);
}
Serial.println("Moved turret");
}
int buttonRead(int pin, int lastState, bool flip=true){
// Checks if the button reading was just intermittent or if reading has been greater than the delay time.
int reading = LOW;
if(flip){
reading = !digitalRead(pin);
}else{
reading = digitalRead(pin);
}
int value = reading;
if (reading != lastState){
lastDebounceTime = millis();
}
if ((millis() - lastDebounceTime) <= debounceDelay){
int value = lastState;
}
return value;
}
bool checkButtons(){
l_buttonState = buttonRead(L_BUTTON_PIN, l_buttonState);
r_buttonState = buttonRead(R_BUTTON_PIN, r_buttonState);
if (l_buttonState == HIGH && r_buttonState == HIGH){
//if (l_buttonState == HIGH) { // Uncomment for single button operation
Serial.println("Both buttons are pressed!");
return true;
}
if (l_buttonState == HIGH){
Serial.println("Only the Left Button is pressed");
display.setTextSize(1);
display.setTextColor(SH110X_WHITE);
display.setCursor(0, 0);
display.println("Only the left button is pressed");
display.display();
delay(1000);
}
if (r_buttonState == HIGH){
Serial.println("Only the Right Button is pressed");
display.println("Only the Right Button is pressed");
}
return false;
}
bool checkSensor(){
Serial.println("Checking carriage sensor**************************");
pressure_sensorState = buttonRead(PRESSURE_SENSOR1_PIN, pressure_sensorState, false);
Serial.print("pressure_sensorState=");
Serial.println(pressure_sensorState);
Serial.println("Done checking carriage sensor ***********************");
if (pressure_sensorState == HIGH){
Serial.println("carriage is forward");
return true;
}else{Serial.println("carriage is not forward");
display.println("carriage is not forward");
return false;
}
}
bool checkSensor2(){
Serial.println("Checking carriage sensor**************************");
pressure_sensorState = buttonRead(SENSOR2_PIN, pressure_sensorState, false);
Serial.print("pressure_sensorState=");
Serial.println(pressure_sensorState);
Serial.println("Done checking carriage sensor ***********************");
if (pressure_sensorState == HIGH){
Serial.println("stock is down");
return true;
}else{Serial.println("stock not down");
display.println("stock not down");
return false;
}
}
void displayError(char msg[32]){
display.clearDisplay();
display.setCursor(0, 0);
display.print(F("ERROR:"));
display.print(msg);
display.display();
}
void loop() {
bool pin_result = false;
while (checkButtons()){
Serial.print("step = ");
Serial.println(step);
switch (step) {
case 1:
// Push the spring and locking pin in
digitalWrite(SPRING_SOLENOID_PIN, HIGH);
Serial.println("Extending the spring piston..");
step = 2;
break;
case 2:
//check carriage
delay(250);
if (checkSensor() == true){
step = 3;
}else{
delay(500);
if(checkSensor() == false){
// Throw error
display.clearDisplay();
display.setCursor(0, 0);
display.println("carriage still not forward");
display.display();
errorBlink(2);
}else{
step = 3;
}
}
Serial.println("executedcase2");
break;
case 3:
//check stock
delay(250);
if (checkSensor2() == true){
step = 3;
}else{
delay(500);
if(checkSensor2() == false){
// Throw error
display.clearDisplay();
display.setCursor(0, 0);
display.println(F("stock still not down"));
display.display();
errorBlink(2);
}else{
step = 4;
}
}
Serial.println("executedcase3");
break;
case 4:
// Turn the servo to shake the roll pin into the turret cylinder
pin_result = shakePin();
if (pin_result){
step = 5;
}
break;
case 5:
// Turn the servo to get roll pin in line with drive pin
delay(1000);
turnTurret(false);
step = 6;
break;
case 6:
// Fire the backup cylinder
digitalWrite(BACKUP_SOLENOID_PIN, HIGH);
step = 7;
break;
case 7:
// drive the roll pin into the stock
delay(1500);
digitalWrite(ROLL_SOLENOID_PIN, HIGH);
Serial.println("Extending the roll pin piston...");
step = 7;
break;
default:
// wait until buttons are depressed
delay(50);
break;
}
}
// This stuff happens when buttons are released
// Retract the roll pin drive pin
digitalWrite(ROLL_SOLENOID_PIN, LOW);
// Regtract the backup solenoid
digitalWrite(BACKUP_SOLENOID_PIN, LOW);
//Serial.println("Retracting the roll pin piston...");
// Retract the latch pin drive pin
digitalWrite(SPRING_SOLENOID_PIN, LOW);
//Serial.println("Retracting the spring piston...");
delay(2000);
// Check if turret was turned
if (myservo.read() != SERVO_HOME){
Serial.print("Servo is at ");
Serial.println(myservo.read());
// Return the turret to vertical if turned
turnTurret(true);
}
step = 1;
delay(50);
}