Untitled

#include <Encoder.h>

// Motor & Encoder Pins (for now motor pins 5 & 6 will be tied to both channels on motor driver)
const int motorPin1 = 5;
const int motorPin2 = 6;
const int encoderPinA = 2;
const int encoderPinB = 3;

// Encoder library callout
Encoder myEncoder(encoderPinA, encoderPinB);

// Setup for target and control. 0 is the "Home position", with full bouyancy. This is set on powerup
long targetPosition = 0;
long error = 0;
long currentPosition = 0;
const long deadzone = 10;
long floodpos = -12430; // 1% of fully flooded tube. this is so that the input accepts a range of 0-100, to indicate % of flooded
long motorSpeed;

// Proportional control constant
const float Kp = 0.1;


void setup() {
  // Initialize motor control pins
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(encoderPinA, INPUT);  
  pinMode(encoderPinB, INPUT);

  // Initialize serial communication
  Serial.begin(9600);

  // Zero the encoder position
  myEncoder.write(0);
}

void loop() {

  // location & motor control calc
  currentPosition = myEncoder.read();
  error = targetPosition - currentPosition;
  motorSpeed = Kp * error;
  motorSpeed = constrain(abs(motorSpeed), 20, 255);

// wait for set point

if (abs(error)< deadzone) {
    analogWrite(motorPin1, 0);
    analogWrite(motorPin2, 0);
    Serial.print("Position Error: ");
    Serial.println(error);
  Serial.print("At target position. Waiting for new target input");
  while (true) {
    if (Serial.available() > 0) {
      String inputString = Serial.readStringUntil('\n'); // Read until newline
      long newPosition = inputString.toInt();
      if ((newPosition >= 0) && (newPosition <= 100)) { 
        targetPosition = floodpos * newPosition;
        Serial.print("New Target Position: ");
        Serial.println(targetPosition);
        break;
      } else {
        Serial.println("Invalid input. Enter a range from 0-100");
      }
    }
  }

  // Control the motor based on the calculated speed
}
else if (error > deadzone) {
    analogWrite(motorPin1, abs(motorSpeed));
    analogWrite(motorPin2, 0);
    //delay(1000);
  } else if (error < -deadzone) {
    analogWrite(motorPin1, 0);
    analogWrite(motorPin2, abs(motorSpeed));
    //delay(1000);
  
   }

  // Print current position to serial monitor
  Serial.print("Target Position:");
  Serial.print(targetPosition);
  Serial.print("Current Position:");
  Serial.print(currentPosition);
  Serial.print(", error:");
  Serial.print(error);
  Serial.print(", Motorspeed:");
  Serial.println(motorSpeed);
  //delay(100);
}