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); }
Leave a Comment