add callibrtion (partial)

blinds/src/main.cpp

@@ -22,11 +22,15 @@
 
 #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();
+  // run_stop();
-  delay(swapdelay);
+  // delay(swapdelay);
   digitalWrite(MOTOR_IN1, LOW);
-  analogWrite(MOTOR_IN2, SPEED);
+  analogWrite(MOTOR_IN2, speed);
-    log_i("left");
+  log_i("left");
-
+  motorruns = true;
   dir = true;
 }
 
-void run_right()
+void run_right(int speed = SPEED)
 {
-  run_stop();
+  // run_stop();
-  delay(swapdelay);
+  // delay(swapdelay);
   digitalWrite(MOTOR_IN2, LOW);
-  analogWrite(MOTOR_IN1, SPEED);
+  analogWrite(MOTOR_IN1, speed);
   log_i("right");
-
+  motorruns = true;
   dir = false;
 }
 
@@ -104,24 +141,79 @@ 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;
 
 void loop() {
@@ -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: ");
+    log_i("Pulses/s: %i", right_wheel_pulse_count);
-    Serial.println(right_wheel_pulse_count);
+    log_i("Speed: %0.0f", rpm_right);
-    Serial.print(" Speed: ");
+    log_i("RPM");
-    Serial.print(rpm_right);
+    log_i("Angular Velocity: %0.00f", rpm_right);
-    Serial.println(" RPM");
+    log_i("rad per second \t %f deg per second", ang_velocity_right_deg);
-    Serial.print(" Angular Velocity: ");
+    log_i("powermeter %0.0f", getVPP());
-    Serial.print(rpm_right);
+    log_i("absulote pulsecount = %i", absPulseCount);
-    Serial.print(" rad per second");
-    Serial.print("\t");
-    Serial.print(ang_velocity_right_deg);
-    Serial.println(" deg per second");
-    Serial.println();
 
     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 --;
   }
 }