From 246490b5a5f6dc60239b3d9c8df2b470268dfb6b Mon Sep 17 00:00:00 2001 From: Willem Oldemans Date: Wed, 27 Sep 2023 20:45:18 +0200 Subject: [PATCH] add callibrtion (partial) --- blinds/src/main.cpp | 167 +++++++++++++++++++++++++++++++++++--------- 1 file changed, 133 insertions(+), 34 deletions(-) diff --git a/blinds/src/main.cpp b/blinds/src/main.cpp index 5c41a46..be0a7d1 100644 --- a/blinds/src/main.cpp +++ b/blinds/src/main.cpp @@ -21,12 +21,16 @@ #define MOTOR_IN2 7 #define SPEED 92 + +#define POWERMETER 4 // True = Forward; False = Reverse bool Direction_right = true; // Keep track of the number of right wheel pulses volatile long right_wheel_pulse_count = 0; +volatile long pulscountswap = 0; +volatile long absPulseCount = 0; // One-second interval for measurements int interval = 1000; @@ -46,16 +50,49 @@ float ang_velocity_right_deg = 0; const float rpm_to_radians = 0.10471975512; const float rad_to_deg = 57.29578; bool dir = false; +bool motorruns = false; void right_wheel_pulse(); + +// ***** function calls ****** +float getVPP() +{ + float result; + int readValue; // value read from the sensor + int maxValue = 0; // store max value here + int minValue = 4096; // store min value here ESP32 ADC resolution + + uint32_t start_time = millis(); + while((millis()-start_time) < 1000) //sample for 1 Sec + { + readValue = analogRead(POWERMETER); + // see if you have a new maxValue + if (readValue > maxValue) + { + /*record the maximum sensor value*/ + maxValue = readValue; + } + if (readValue < minValue) + { + /*record the minimum sensor value*/ + minValue = readValue; + } + } + + // Subtract min from max + result = ((maxValue - minValue) * 3.3)/4096.0; //ESP32 ADC resolution 4096 + log_i("ADC: %0.0f", result); + return result; + } + void run_stop() { digitalWrite(MOTOR_IN1, LOW); digitalWrite(MOTOR_IN2, LOW); analogWrite(MOTOR_IN1, 0); analogWrite(MOTOR_IN2, 0); - + motorruns = false; log_i("stop"); } @@ -65,25 +102,25 @@ void run_to() } -void run_left() +void run_left(int speed = SPEED) { - run_stop(); - delay(swapdelay); + // run_stop(); + // delay(swapdelay); digitalWrite(MOTOR_IN1, LOW); - analogWrite(MOTOR_IN2, SPEED); - log_i("left"); - + analogWrite(MOTOR_IN2, speed); + log_i("left"); + motorruns = true; dir = true; } -void run_right() +void run_right(int speed = SPEED) { - run_stop(); - delay(swapdelay); + // run_stop(); + // delay(swapdelay); digitalWrite(MOTOR_IN2, LOW); - analogWrite(MOTOR_IN1, SPEED); + analogWrite(MOTOR_IN1, speed); log_i("right"); - + motorruns = true; dir = false; } @@ -104,23 +141,78 @@ void swap_dir() log_i("swap"); } +long previouspulses = 0; + +void calibratemotor() +{ + delay(300); + + bool stop = false; + run_right(); + previouspulses = absPulseCount; + while(!stop) + { + delay(100); + if(absPulseCount - previouspulses > 40) + { + log_i("absPulseCount (%i)- previouspulses(%i) = %i", absPulseCount, previouspulses, absPulseCount - previouspulses); + previouspulses = absPulseCount; + delay(50); + } + else + { + stop = true; + run_stop(); + log_i("left done, %i", absPulseCount); + absPulseCount = 0; + } + } + stop = false; + delay(1000); + run_left(); + previouspulses = absPulseCount; + while(!stop) + { + delay(100); + if(absPulseCount - previouspulses < -30) + { + log_i("absPulseCount (%i)- previouspulses(%i) = %i", absPulseCount, previouspulses, absPulseCount - previouspulses); + previouspulses = absPulseCount; + delay(50); + } + else + { + stop = true; + run_stop(); + log_i("right done, %i", absPulseCount); + } + } +} + void setup() { // Open the serial port at 9600 bps Serial.begin(115200); - + delay(5000); + log_i("start sketch"); // Set pin states of the encoder pinMode(ENC_IN_RIGHT_A , INPUT_PULLUP); pinMode(ENC_IN_RIGHT_B , INPUT); pinMode(MOTOR_IN1, OUTPUT); pinMode(MOTOR_IN2, OUTPUT); + pinMode(POWERMETER, ANALOG); + // Every time the pin goes high, this is a pulse attachInterrupt(digitalPinToInterrupt(ENC_IN_RIGHT_A), right_wheel_pulse, RISING); - run_left(); + log_i("init done"); + calibratemotor(); + //run_left(); } + + long swaptime = 0; @@ -130,7 +222,8 @@ void loop() { currentMillis = millis(); // If one second has passed, print the number of pulses - if (currentMillis - previousMillis > interval) { + if ((currentMillis - previousMillis > interval) && (motorruns = true)) + { previousMillis = currentMillis; @@ -139,29 +232,31 @@ void loop() { ang_velocity_right = rpm_right * rpm_to_radians; ang_velocity_right_deg = ang_velocity_right * rad_to_deg; - Serial.print(" Pulses/s: "); - Serial.println(right_wheel_pulse_count); - Serial.print(" Speed: "); - Serial.print(rpm_right); - Serial.println(" RPM"); - Serial.print(" Angular Velocity: "); - Serial.print(rpm_right); - Serial.print(" rad per second"); - Serial.print("\t"); - Serial.print(ang_velocity_right_deg); - Serial.println(" deg per second"); - Serial.println(); - + log_i("Pulses/s: %i", right_wheel_pulse_count); + log_i("Speed: %0.0f", rpm_right); + log_i("RPM"); + log_i("Angular Velocity: %0.00f", rpm_right); + log_i("rad per second \t %f deg per second", ang_velocity_right_deg); + log_i("powermeter %0.0f", getVPP()); + log_i("absulote pulsecount = %i", absPulseCount); + right_wheel_pulse_count = 0; - } - - if(millis() - swaptime > 5000) - { - swaptime = millis(); - swap_dir(); } + // if(pulscountswap > ENC_COUNT_REV/4) + // { + // swap_dir(); + // pulscountswap = 0; + // } + + // if(millis() - swaptime > 5000) + // { + // swaptime = millis(); + // swap_dir(); + + // } + // if(rpm_right < 1 && dir) // { // run_stop(); @@ -183,9 +278,13 @@ void right_wheel_pulse() { if (Direction_right) { right_wheel_pulse_count++; + pulscountswap ++; + absPulseCount ++; } else { right_wheel_pulse_count--; + pulscountswap ++; + absPulseCount --; } }