add processing filtering test with sliders

2019-12-17 17:09:56 +01:00 · 2019-12-17 17:09:56 +01:00 · 7c81d1ded0
parent daeabed5ac
commit 7c81d1ded0
1 changed files with 123 additions and 0 deletions
--- a/other/filtertest/filtertest.pde
+++ b/other/filtertest/filtertest.pde
@ -0,0 +1,123 @@
+
+
+import controlP5.*;
+
+ControlP5 cp5;
+int myColor = color(0,0,0);
+
+
+
+
+long lastReadADC=0;
+long lastSend=0;
+long lastHBLoop=0;
+
+int ADC_CALIB_THROTTLE_MIN=2000; //controller
+int ADC_CALIB_THROTTLE_MAX=3120; //controller
+int ADC_READTIME=10; //controller. in ms
+int adc_throttle_raw=0; //controller. raw value read from adc (here slider)
+float adc_throttle=0; //controller. internally filtered value
+float ADC_THROTTLE_FILTER=0.05; //controller
+
+int SENDPERIOD=50; //controller. in ms
+int sendSpeed=0; //transmittet speed value over usart
+
+int LOOPTIME=5; //hoverboard.
+int RATE=30; //hoverboard. rate value, see config.h. default 30
+float FILTER=0.1; //hoverboard. filter value, see config.h default 0.1
+float speedRate=0; //hoverboard.implemented as fixdt. what comes out of rate limiting.
+int outSpeed=0; //hoverboard. speedvalue for pwm. filtered speedRate
+
+Slider SinputValue;
+Slider SfilteredInputValue;
+Slider SspeedRate;
+Slider SoutSpeed;
+
+int inputValue = ADC_CALIB_THROTTLE_MIN;
+int maxvalue=1000;
+
+void setup() {
+  frameRate(int(1000.0/5));
+  size(700,400);
+  noStroke();
+  cp5 = new ControlP5(this);
+  
+  // add a horizontal sliders, the value of this slider will be linked
+  // to variable 'sliderValue' 
+  SinputValue=cp5.addSlider("inputValue")
+     .setPosition(10,50)
+     .setSize(500,30)
+     .setRange(ADC_CALIB_THROTTLE_MIN,ADC_CALIB_THROTTLE_MAX)
+     ;
+  SfilteredInputValue=cp5.addSlider("filteredInputValue")
+     .setPosition(10,50+30+10)
+     .setSize(500,20)
+     .setRange(ADC_CALIB_THROTTLE_MIN,ADC_CALIB_THROTTLE_MAX)
+     ;
+     
+  SspeedRate=cp5.addSlider("speedRate")
+     .setPosition(10,50+30+10+20+10)
+     .setSize(500,20)
+     .setRange(0,1000)
+     ;
+     
+  SoutSpeed=cp5.addSlider("outSpeed")
+     .setPosition(10,50+30+10+20+10+20+10)
+     .setSize(500,30)
+     .setRange(0,1000)
+     ;
+  
+
+}
+
+void draw() {
+  long loopmillis=millis();
+  if (loopmillis-lastReadADC>ADC_READTIME){ //frequency of controller adc read/filtering
+    lastReadADC=millis();
+    adc_throttle_raw = inputValue; //adc_throttle_raw = analogRead(PIN_THROTTLE);
+    adc_throttle = adc_throttle*(1-ADC_THROTTLE_FILTER) + adc_throttle_raw*ADC_THROTTLE_FILTER; //adc_throttle = adc_throttle*(1-ADC_THROTTLE_FILTER) + adc_throttle_raw*ADC_THROTTLE_FILTER;
+    SfilteredInputValue.setValue(adc_throttle);
+  }
+  if (loopmillis-lastSend>SENDPERIOD){ //Frequency of transmitting new values to the hoverboard
+    lastSend=millis();
+    sendSpeed=int(constrain( map(adc_throttle, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, 1000 ) ,0, 1000));
+  }
+  
+  if (loopmillis-lastHBLoop>LOOPTIME){ //frequency of hoverboard mainboard (looptime)
+    lastHBLoop=millis();
+    int cmd=sendSpeed; //commanded value
+    
+    //rateLimiter16 function
+    float q0;  //int16_t q0;
+    float q1;  //int16_t q1;
+    
+    q0 = (cmd-speedRate);  //q0 = (u << 4)  - *y;
+    
+    if (q0 > RATE) {  //if (q0 > rate) {
+      q0=RATE; //  q0 = rate;
+    }else{ //} else {
+      q1=-RATE;  //  q1 = -rate;
+      if (q0<q1) {//  if (q0 < q1) {
+        q0=q1;//    q0 = q1;
+      }//  }
+    }  //}
+    
+    speedRate = q0 + speedRate; //*y = q0 + *y;
+    SspeedRate.setValue(speedRate);
+    
+    outSpeed= int(outSpeed*(1-FILTER) + speedRate*FILTER); //filtLowPass16 function
+    SoutSpeed.setValue(outSpeed);
+  }
+  
+  
+  //fill(inputValue*1.0/maxvalue*255);
+  //rect(0,0,width,100);  
+}
+
+/*
+void slider(float theColor) {
+  myColor = color(theColor);
+  println("a slider event. setting background to "+theColor);
+}
+
+*/